diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp index c87c3ad8df..c226cb785d 100644 --- a/libraries/AP_Baro/AP_Baro.cpp +++ b/libraries/AP_Baro/AP_Baro.cpp @@ -103,15 +103,8 @@ float AP_Baro::get_altitude(void) // note that this relies on read() being called regularly to get new data float AP_Baro::get_climb_rate(void) { - if (_last_climb_rate_t == _last_altitude_t) { - // no new information - return _climb_rate; - } - _last_climb_rate_t = _last_altitude_t; - // we use a 7 point derivative filter on the climb rate. This seems // to produce somewhat reasonable results on real hardware - _climb_rate = _climb_rate_filter.slope() * 1.0e3; - - return _climb_rate; + return _climb_rate_filter.slope() * 1.0e3; } + diff --git a/libraries/AP_Baro/AP_Baro.h b/libraries/AP_Baro/AP_Baro.h index afb373acc4..6170f9d249 100644 --- a/libraries/AP_Baro/AP_Baro.h +++ b/libraries/AP_Baro/AP_Baro.h @@ -52,8 +52,6 @@ private: AP_Float _ground_temperature; AP_Float _ground_pressure; float _altitude; - float _climb_rate; - uint32_t _last_climb_rate_t; uint32_t _last_altitude_t; DerivativeFilterFloat_Size7 _climb_rate_filter; };