From 466a104534e5b687e26cf8a792d222498fae4069 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Sat, 30 Jan 2016 16:05:18 +1100 Subject: [PATCH] EKF: additional GPS check logic Improve naming of NED origin initialisation status Add check for GPS solution type Use GPS checks when regaining GPs in-flight --- EKF/control.cpp | 2 +- EKF/ekf.cpp | 2 +- EKF/ekf.h | 21 ++++++++++----------- EKF/estimator_base.cpp | 6 +++--- EKF/estimator_base.h | 9 ++++----- EKF/gps_checks.cpp | 39 ++++++++++++++++++++++----------------- 6 files changed, 41 insertions(+), 38 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index cc69c650c3..1a1a23dca9 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -52,7 +52,7 @@ void Ekf::controlFusionModes() // GPS fusion mode selection logic // To start use GPS we need angular alignment completed, the local NED origin set and fresh GPS data if (!_control_status.flags.gps) { - if (_control_status.flags.angle_align && (_time_last_imu - _time_last_gps) < 5e5 && _gps_initialised) { + if (_control_status.flags.angle_align && (_time_last_imu - _time_last_gps) < 5e5 && _NED_origin_initialised && (_time_last_imu - _last_gps_fail_us > 5e6)) { _control_status.flags.gps = true; resetPosition(); resetVelocity(); diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 09f8aaddf3..dc4e71a64f 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -224,7 +224,7 @@ bool Ekf::initialiseFilter(void) void Ekf::predictState() { if (!_earth_rate_initialised) { - if (_gps_initialised) { + if (_NED_origin_initialised) { calcEarthRateNED(_earth_rate_NED, _pos_ref.lat_rad); _earth_rate_initialised = true; } diff --git a/EKF/ekf.h b/EKF/ekf.h index 06ce176ca1..b18b16dc10 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -159,15 +159,16 @@ private: // publish the status of various GPS quality checks union gps_check_fail_status_u { struct { - uint16_t nsats : 1; // 0 - true if number of satellites used is insufficient - uint16_t gdop : 1; // 1 - true if geometric dilution of precision is insufficient - uint16_t hacc : 1; // 2 - true if reported horizontal accuracy is insufficient - uint16_t vacc : 1; // 3 - true if reported vertical accuracy is insufficient - uint16_t sacc : 1; // 4 - true if reported speed accuracy is insufficient - uint16_t hdrift : 1; // 5 - true if horizontal drift is excessive (can only be used when stationary on ground) - uint16_t vdrift : 1; // 6 - true if vertical drift is excessive (can only be used when stationary on ground) - uint16_t hspeed : 1; // 7 - true if horizontal speed is excessive (can only be used when stationary on ground) - uint16_t vspeed : 1; // 8 - true if vertical speed error is excessive + uint16_t fix : 1; // 0 - true if the fix type is insufficient (no 3D solution) + uint16_t nsats : 1; // 1 - true if number of satellites used is insufficient + uint16_t gdop : 1; // 2 - true if geometric dilution of precision is insufficient + uint16_t hacc : 1; // 3 - true if reported horizontal accuracy is insufficient + uint16_t vacc : 1; // 4 - true if reported vertical accuracy is insufficient + uint16_t sacc : 1; // 5 - true if reported speed accuracy is insufficient + uint16_t hdrift : 1; // 6 - true if horizontal drift is excessive (can only be used when stationary on ground) + uint16_t vdrift : 1; // 7 - true if vertical drift is excessive (can only be used when stationary on ground) + uint16_t hspeed : 1; // 8 - true if horizontal speed is excessive (can only be used when stationary on ground) + uint16_t vspeed : 1; // 9 - true if vertical speed error is excessive } flags; uint16_t value; }_gps_check_fail_status; @@ -216,8 +217,6 @@ private: void calcEarthRateNED(Vector3f &omega, double lat_rad) const; - void initialiseGPS(struct gps_message *gps); - // return true id the GPS quality is good enough to set an origin and start aiding bool gps_is_good(struct gps_message *gps); diff --git a/EKF/estimator_base.cpp b/EKF/estimator_base.cpp index dfff630dd9..5ad9e98989 100644 --- a/EKF/estimator_base.cpp +++ b/EKF/estimator_base.cpp @@ -247,7 +247,7 @@ void EstimatorBase::initialiseVariables(uint64_t time_usec) _imu_updated = false; _start_predict_enabled = false; _initialised = false; - _gps_initialised = false; + _NED_origin_initialised = false; _gps_speed_valid = false; _mag_healthy = false; @@ -266,8 +266,8 @@ void EstimatorBase::initialiseVariables(uint64_t time_usec) bool EstimatorBase::position_is_valid() { // return true if the position estimate is valid - // TODO implement proper check based on published GPS accuracy, innovaton consistency checks and timeout status - return _gps_initialised && (_time_last_imu - _time_last_gps) < 5e6; + // TOTO implement proper check based on published GPS accuracy, innovaton consistency checks and timeout status + return _NED_origin_initialised && (_time_last_imu - _time_last_gps) < 5e6; } void EstimatorBase::printStoredIMU() diff --git a/EKF/estimator_base.h b/EKF/estimator_base.h index b948856f29..30f4424ccd 100644 --- a/EKF/estimator_base.h +++ b/EKF/estimator_base.h @@ -314,14 +314,14 @@ protected: outputSample _output_new; imuSample _imu_sample_new; - - uint64_t _imu_ticks; + uint64_t _imu_ticks; bool _imu_updated = false; bool _start_predict_enabled = false; bool _initialised = false; + bool _vehicle_armed = false; // vehicle arm status used to turn off functionality used on the ground - bool _gps_initialised = false; + bool _NED_origin_initialised = false; bool _gps_speed_valid = false; struct map_projection_reference_s _pos_ref = {}; // Contains WGS-84 position latitude and longitude (radians) @@ -356,8 +356,7 @@ protected: bool bad_sideslip: 1; } _fault_status; + // initialise variables to default startup values and set time stamps to specified value void initialiseVariables(uint64_t timestamp); - bool _vehicle_armed = false; // vehicle arm status used to turn off funtionality used on the ground - }; diff --git a/EKF/gps_checks.cpp b/EKF/gps_checks.cpp index 6c78abc957..d2d6f9f842 100644 --- a/EKF/gps_checks.cpp +++ b/EKF/gps_checks.cpp @@ -54,29 +54,30 @@ bool Ekf::collect_gps(uint64_t time_usec, struct gps_message *gps) { - if(_gps_initialised) { + // run gps checks if we have not yet set the NED origin or have not started using GPS + if(!_NED_origin_initialised || !_control_status.flags.gps) { + // if we have good GPS data and the NED origin is not set, set to the last GPS fix + if (gps_is_good(gps) && !_NED_origin_initialised) { + printf("gps is good - setting EKF origin\n"); + // Initialise projection + double lat = gps->lat / 1.0e7; + double lon = gps->lon / 1.0e7; + map_projection_init(&_pos_ref, lat, lon); + _gps_alt_ref = gps->alt / 1e3f; + _NED_origin_initialised = true; + _last_gps_origin_time_us = _time_last_imu; + } + } + + // start collecting GPS if there is a 3D fix and the NED origin has been set + if (_NED_origin_initialised && gps->fix_type >= 3) { return true; } else { - initialiseGPS(gps); + return false; } return false; } -void Ekf::initialiseGPS(struct gps_message *gps) -{ - //Check if the GPS fix is good enough for us to use - if (gps_is_good(gps)) { - printf("gps is good\n"); - // Initialise projection - double lat = gps->lat / 1.0e7; - double lon = gps->lon / 1.0e7; - map_projection_init(&_pos_ref, lat, lon); - _gps_alt_ref = gps->alt / 1e3f; - _gps_initialised = true; - _last_gps_origin_time_us = _time_last_imu; - } -} - /* * Return true if the GPS solution quality is adequate to set an origin for the EKF * and start GPS aiding. @@ -86,6 +87,9 @@ void Ekf::initialiseGPS(struct gps_message *gps) */ bool Ekf::gps_is_good(struct gps_message *gps) { + // Check the fix type + _gps_check_fail_status.flags.fix = (gps->fix_type < 3); + // Check the number of satellites _gps_check_fail_status.flags.nsats = (gps->nsats < _params.req_nsats); @@ -185,6 +189,7 @@ bool Ekf::gps_is_good(struct gps_message *gps) // if any user selected checks have failed, record the fail time if ( + _gps_check_fail_status.flags.fix || (_gps_check_fail_status.flags.nsats && (_params.gps_check_mask & MASK_GPS_NSATS)) || (_gps_check_fail_status.flags.gdop && (_params.gps_check_mask & MASK_GPS_GDOP)) || (_gps_check_fail_status.flags.hacc && (_params.gps_check_mask & MASK_GPS_HACC)) ||