From c54b5b9af938dcc763ca089cbecd92a6a4a1ef6e Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 12 Mar 2015 20:08:21 +0900 Subject: [PATCH] InertialNav: remove baro glitch protection --- libraries/AP_InertialNav/AP_InertialNav.cpp | 34 +++++-------------- libraries/AP_InertialNav/AP_InertialNav.h | 6 +--- .../AP_InertialNav/AP_InertialNav_NavEKF.h | 4 +-- 3 files changed, 12 insertions(+), 32 deletions(-) diff --git a/libraries/AP_InertialNav/AP_InertialNav.cpp b/libraries/AP_InertialNav/AP_InertialNav.cpp index 8959d1f112..702f2a6139 100644 --- a/libraries/AP_InertialNav/AP_InertialNav.cpp +++ b/libraries/AP_InertialNav/AP_InertialNav.cpp @@ -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 diff --git a/libraries/AP_InertialNav/AP_InertialNav.h b/libraries/AP_InertialNav/AP_InertialNav.h index dfba6228bf..586cd65076 100644 --- a/libraries/AP_InertialNav/AP_InertialNav.h +++ b/libraries/AP_InertialNav/AP_InertialNav.h @@ -8,7 +8,6 @@ #include // ArduPilot Mega Barometer Library #include // FIFO buffer library #include // GPS Glitch detection library -#include // 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; diff --git a/libraries/AP_InertialNav/AP_InertialNav_NavEKF.h b/libraries/AP_InertialNav/AP_InertialNav_NavEKF.h index b3e14a9571..347210bdd4 100644 --- a/libraries/AP_InertialNav/AP_InertialNav_NavEKF.h +++ b/libraries/AP_InertialNav/AP_InertialNav_NavEKF.h @@ -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) {