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

InertialNav: remove baro glitch protection

This commit is contained in:
Randy Mackay 2015-03-12 20:08:21 +09:00
parent 8e8487c699
commit c54b5b9af9
3 changed files with 12 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
libraries/AP_InertialNav/AP_InertialNav.cpp
View File

@ -390,33 +390,17 @@ void AP_InertialNav::correct_with_baro(float baro_alt, float dt)
first_reads++;
}
// 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{
// 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);
_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);
}
// 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;
}
// update our internal record of glitching flag so that we can notice a change
_flags.baro_glitching = _baro_glitch.glitching();
// calculate error in position from baro with our estimate
_position_error.z = baro_alt - (hist_position_base_z + _position_correction.z);
}
// set_altitude - set base altitude estimate in cm

6
libraries/AP_InertialNav/AP_InertialNav.h
View File

@ -8,7 +8,6 @@
#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
#include "../AP_NavEKF/AP_Nav_Common.h" // definitions shared by inertial and ekf nav filters
#define AP_INTERTIALNAV_TC_XY 2.5f // default time constant for complementary filter's X & Y axis
@ -39,7 +38,7 @@ class AP_InertialNav
public:
// Constructor
AP_InertialNav(AP_AHRS &ahrs, AP_Baro &baro, GPS_Glitch& gps_glitch, Baro_Glitch &baro_glitch) :
AP_InertialNav(AP_AHRS &ahrs, AP_Baro &baro, GPS_Glitch& gps_glitch) :
_ahrs(ahrs),
_baro(baro),
_xy_enabled(false),
@ -55,7 +54,6 @@ 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);
@ -261,7 +259,6 @@ 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;
@ -300,7 +297,6 @@ 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
int32_t _last_home_lat;

4
libraries/AP_InertialNav/AP_InertialNav_NavEKF.h
View File

@ -16,8 +16,8 @@ class AP_InertialNav_NavEKF : public AP_InertialNav
{
public:
// Constructor
AP_InertialNav_NavEKF(AP_AHRS_NavEKF &ahrs, AP_Baro &baro, GPS_Glitch& gps_glitch, Baro_Glitch& baro_glitch) :
AP_InertialNav(ahrs, baro, gps_glitch, baro_glitch),
AP_InertialNav_NavEKF(AP_AHRS_NavEKF &ahrs, AP_Baro &baro, GPS_Glitch& gps_glitch) :
AP_InertialNav(ahrs, baro, gps_glitch),
_haveabspos(false),
_ahrs_ekf(ahrs)
{