mirror of https://github.com/ArduPilot/ardupilot
AP_Baro: use DerivativeFilter in barometer climb rate
This commit is contained in:
parent
f97289792b
commit
a5d607d25a
|
@ -83,29 +83,15 @@ float AP_Baro::get_altitude(void)
|
||||||
// note that this relies on read() being called regularly to get new data
|
// note that this relies on read() being called regularly to get new data
|
||||||
float AP_Baro::get_climb_rate(void)
|
float AP_Baro::get_climb_rate(void)
|
||||||
{
|
{
|
||||||
float alt, deltat;
|
|
||||||
|
|
||||||
if (_last_climb_rate_t == _last_update) {
|
if (_last_climb_rate_t == _last_update) {
|
||||||
// no new information
|
// no new information
|
||||||
return _climb_rate;
|
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;
|
_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;
|
return _climb_rate;
|
||||||
}
|
}
|
||||||
|
|
|
@ -5,7 +5,7 @@
|
||||||
|
|
||||||
#include <AP_Param.h>
|
#include <AP_Param.h>
|
||||||
#include <Filter.h>
|
#include <Filter.h>
|
||||||
#include <AverageFilter.h>
|
#include <DerivativeFilter.h>
|
||||||
#include "../AP_PeriodicProcess/AP_PeriodicProcess.h"
|
#include "../AP_PeriodicProcess/AP_PeriodicProcess.h"
|
||||||
|
|
||||||
class AP_Baro
|
class AP_Baro
|
||||||
|
@ -47,11 +47,10 @@ private:
|
||||||
AP_Int16 _ground_temperature;
|
AP_Int16 _ground_temperature;
|
||||||
AP_Int32 _ground_pressure;
|
AP_Int32 _ground_pressure;
|
||||||
float _altitude;
|
float _altitude;
|
||||||
uint32_t _last_altitude_t;
|
|
||||||
float _last_altitude;
|
|
||||||
float _climb_rate;
|
float _climb_rate;
|
||||||
uint32_t _last_climb_rate_t;
|
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"
|
#include "AP_Baro_MS5611.h"
|
||||||
|
|
Loading…
Reference in New Issue