Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-04 15:08:39 -04:00
Code Issues Packages Projects Releases Wiki Activity

InertialNav: skip baro updates when glitching

Browse Source 
Also resets inertial nav alt estimate once glitch clears
This commit is contained in:
Randy Mackay 2014-07-28 17:36:46 +09:00
parent ee6d4d570a
commit 322f527c4f
2 changed files with 30 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
libraries/AP_InertialNav/AP_InertialNav.cpp
View File

@ -345,17 +345,34 @@ void AP_InertialNav::correct_with_baro(float baro_alt, float dt)
first_reads++;
}
// 3rd order samples (i.e. position from baro) are delayed by 150ms (15 iterations at 100hz)
// so we should calculate error using historical estimates
float hist_position_base_z;
if( _hist_position_estimate_z.is_full() ) {
hist_position_base_z = _hist_position_estimate_z.front();
// sanity check the baro position. Relies on the main code calling Baro_Glitch::check_alt() immediatley after a baro update
if (_baro_glitch.glitching()) {
// failed sanity check so degrate position_error to 10% over 2 seconds (assumes 10hz update rate)
_position_error.z *= 0.89715f;
}else{
hist_position_base_z = _position_base.z;
// if our internal baro glitching flag (from previous iteration) is true we have just recovered from a glitch
// reset the inertial nav alt to baro alt
if (_flags.baro_glitching) {
set_altitude(baro_alt);
set_velocity_z(_baro.get_climb_rate() * 100.0f);
_position_error.z = 0.0f;
}else{
// 3rd order samples (i.e. position from baro) are delayed by 150ms (15 iterations at 100hz)
// so we should calculate error using historical estimates
float hist_position_base_z;
if (_hist_position_estimate_z.is_full()) {
hist_position_base_z = _hist_position_estimate_z.front();
} else {
hist_position_base_z = _position_base.z;
}
// calculate error in position from baro with our estimate
_position_error.z = baro_alt - (hist_position_base_z + _position_correction.z);
}
}
// calculate error in position from baro with our estimate
_position_error.z = baro_alt - (hist_position_base_z + _position_correction.z);
// update our internal record of glitching flag so that we can notice a change
_flags.baro_glitching = _baro_glitch.glitching();
}
// set_altitude - set base altitude estimate in cm

6
libraries/AP_InertialNav/AP_InertialNav.h
View File

@ -8,6 +8,7 @@
#include <AP_Baro.h> // ArduPilot Mega Barometer Library
#include <AP_Buffer.h> // FIFO buffer library
#include <AP_GPS_Glitch.h> // GPS Glitch detection library
#include <AP_Baro_Glitch.h> // Baro Glitch detection library
#define AP_INTERTIALNAV_TC_XY 2.5f // default time constant for complementary filter's X & Y axis
#define AP_INTERTIALNAV_TC_Z 5.0f // default time constant for complementary filter's Z axis
@ -34,7 +35,7 @@ class AP_InertialNav
public:
// Constructor
AP_InertialNav(AP_AHRS &ahrs, AP_Baro &baro, GPS_Glitch& gps_glitch ) :
AP_InertialNav(AP_AHRS &ahrs, AP_Baro &baro, GPS_Glitch& gps_glitch, Baro_Glitch &baro_glitch) :
_ahrs(ahrs),
_baro(baro),
_xy_enabled(false),
@ -50,6 +51,7 @@ public:
_k3_z(0.0f),
_baro_last_update(0),
_glitch_detector(gps_glitch),
_baro_glitch(baro_glitch),
_error_count(0)
{
AP_Param::setup_object_defaults(this, var_info);
@ -275,6 +277,7 @@ protected:
// structure for holding flags
struct InertialNav_flags {
uint8_t gps_glitching : 1; // 1 if glitch detector was previously indicating a gps glitch
uint8_t baro_glitching : 1; // 1 if baro glitch detector was previously indicating a baro glitch
uint8_t ignore_error : 3; // the number of iterations for which we should ignore errors
} _flags;
@ -313,6 +316,7 @@ protected:
// error handling
GPS_Glitch& _glitch_detector; // GPS Glitch detector
Baro_Glitch& _baro_glitch; // Baro glitch detector
uint8_t _error_count; // number of missed GPS updates
};