Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Baro: use DerivativeFilter in barometer climb rate

This commit is contained in:
Andrew Tridgell 2012-07-05 08:08:16 +10:00
parent f19e5a6273
commit f9fc7aafe7
2 changed files with 7 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AP_Baro/AP_Baro.cpp
View File

@ -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;
}

7
libraries/AP_Baro/AP_Baro.h
View File

@ -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"