Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-03-03 12:14:10 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Baro: tolerate GND_ALT_OFFSET step inputs

Browse Source 
large inputs make the EKF angry, this will allow the param update to slew over a few seconds
This commit is contained in:
Tom Pittenger 2016-01-30 01:43:43 -08:00
parent 046741d23b
commit f435c5ee50
2 changed files with 10 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
libraries/AP_Baro/AP_Baro.cpp
View File

@ -351,6 +351,14 @@ void AP_Baro::init(void)
*/
void AP_Baro::update(void)
{
if (fabsf(_alt_offset - _alt_offset_active) > 0.1f) {
// if there's more than 10cm difference then slowly slew to it via LPF.
// The EKF does not like step inputs so this keeps it happy
_alt_offset_active = (0.9f*_alt_offset_active) + (0.1f*_alt_offset);
} else {
_alt_offset_active = _alt_offset;
}
if (!_hil_mode) {
for (uint8_t i=0; i<_num_drivers; i++) {
drivers[i]->update();
@ -374,7 +382,7 @@ void AP_Baro::update(void)
// sanity check altitude
sensors[i].alt_ok = !(isnan(altitude) || isinf(altitude));
if (sensors[i].alt_ok) {
sensors[i].altitude = altitude + _alt_offset;
sensors[i].altitude = altitude + _alt_offset_active;
}
}
}

1
libraries/AP_Baro/AP_Baro.h
View File

@ -148,6 +148,7 @@ private:
} sensors[BARO_MAX_INSTANCES];
AP_Float _alt_offset;
float _alt_offset_active;
AP_Int8 _primary_baro; // primary chosen by user
float _last_altitude_EAS2TAS;
float _EAS2TAS;