Browse Source

AP_Baro: change to a 7 point DerivativeFilter for climb rate

mission-4.1.18
Andrew Tridgell 13 years ago
parent
commit
185c6e5b32
  1. 2
      libraries/AP_Baro/AP_Baro.cpp
  2. 2
      libraries/AP_Baro/AP_Baro.h

2
libraries/AP_Baro/AP_Baro.cpp

@ -91,7 +91,7 @@ float AP_Baro::get_climb_rate(void) @@ -91,7 +91,7 @@ float AP_Baro::get_climb_rate(void)
// we use a 9 point derivative filter on the climb rate. This seems
// to produce somewhat reasonable results on real hardware
_climb_rate = _climb_rate_filter.apply(get_altitude());
_climb_rate = _climb_rate_filter.apply(get_altitude(), _last_update) * 1.0e3;
return _climb_rate;
}

2
libraries/AP_Baro/AP_Baro.h

@ -50,7 +50,7 @@ private: @@ -50,7 +50,7 @@ private:
float _climb_rate;
uint32_t _last_climb_rate_t;
uint32_t _last_altitude_t;
DerivativeFilter<float,float,9> _climb_rate_filter;
DerivativeFilterFloat_Size7 _climb_rate_filter;
};
#include "AP_Baro_MS5611.h"

Loading…
Cancel
Save