From f9fc7aafe77c3bffc8c7acec5b484571ee4965cd Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Thu, 5 Jul 2012 08:08:16 +1000
Subject: [PATCH] AP_Baro: use DerivativeFilter in barometer climb rate

---
 libraries/AP_Baro/AP_Baro.cpp | 22 ++++------------------
 libraries/AP_Baro/AP_Baro.h   |  7 +++----
 2 files changed, 7 insertions(+), 22 deletions(-)

diff --git a/libraries/AP_Baro/AP_Baro.cpp b/libraries/AP_Baro/AP_Baro.cpp
index 626e6f9481..1ad4bee451 100644
--- a/libraries/AP_Baro/AP_Baro.cpp
+++ b/libraries/AP_Baro/AP_Baro.cpp
@@ -83,29 +83,15 @@ 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)
 {
-    float alt, deltat;
-
     if (_last_climb_rate_t == _last_update) {
         // no new information
         return _climb_rate;
     }
-    if (_last_climb_rate_t == 0) {
-        // first call
-        _last_altitude = get_altitude();
-        _last_climb_rate_t = _last_update;
-        _climb_rate = 0.0;
-        return _climb_rate;
-    }
-
-    deltat = (_last_update - _last_climb_rate_t) * 1.0e-3;
-
-    alt = get_altitude();
-
-    // we use a 5 point average filter on the climb rate. This seems
-    // to produce somewhat reasonable results on real hardware
-    _climb_rate = _climb_rate_filter.apply((alt - _last_altitude) / deltat);
-    _last_altitude = alt;
     _last_climb_rate_t = _last_update;
 
+    // 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());
+
 	return _climb_rate;
 }
diff --git a/libraries/AP_Baro/AP_Baro.h b/libraries/AP_Baro/AP_Baro.h
index ec9f13194d..edc6c41faf 100644
--- a/libraries/AP_Baro/AP_Baro.h
+++ b/libraries/AP_Baro/AP_Baro.h
@@ -5,7 +5,7 @@
 
 #include <AP_Param.h>
 #include <Filter.h>
-#include <AverageFilter.h>
+#include <DerivativeFilter.h>
 #include "../AP_PeriodicProcess/AP_PeriodicProcess.h"
 
 class AP_Baro
@@ -47,11 +47,10 @@ private:
     AP_Int16    _ground_temperature;
     AP_Int32    _ground_pressure;
     float       _altitude;
-    uint32_t    _last_altitude_t;
-    float		_last_altitude;
     float       _climb_rate;
     uint32_t    _last_climb_rate_t;
-    AverageFilterFloat_Size5 _climb_rate_filter;
+    uint32_t    _last_altitude_t;
+    DerivativeFilter<float,float,9> _climb_rate_filter;
 };
 
 #include "AP_Baro_MS5611.h"