From 185c6e5b322daae7e6181f0ae93da17bca5ee2af Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 5 Jul 2012 13:00:31 +1000 Subject: [PATCH] AP_Baro: change to a 7 point DerivativeFilter for climb rate --- libraries/AP_Baro/AP_Baro.cpp | 2 +- libraries/AP_Baro/AP_Baro.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp index 1ad4bee451..a38f8e778b 100644 --- a/libraries/AP_Baro/AP_Baro.cpp +++ b/libraries/AP_Baro/AP_Baro.cpp @@ -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; } diff --git a/libraries/AP_Baro/AP_Baro.h b/libraries/AP_Baro/AP_Baro.h index edc6c41faf..27a9120845 100644 --- a/libraries/AP_Baro/AP_Baro.h +++ b/libraries/AP_Baro/AP_Baro.h @@ -50,7 +50,7 @@ private: float _climb_rate; uint32_t _last_climb_rate_t; uint32_t _last_altitude_t; - DerivativeFilter _climb_rate_filter; + DerivativeFilterFloat_Size7 _climb_rate_filter; }; #include "AP_Baro_MS5611.h"