From 51197a5d82d3bbd07e6f594d2b4c61b3b8369877 Mon Sep 17 00:00:00 2001 From: bresch Date: Mon, 3 May 2021 15:17:12 +0200 Subject: [PATCH] GNSS yaw selection logic refactor --- EKF/control.cpp | 76 ++++++++++++++++++++++++++++++++++++++----------- EKF/ekf.h | 5 +++- 2 files changed, 63 insertions(+), 18 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index a34d9ea97d..8a4f23a050 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -509,12 +509,13 @@ void Ekf::controlGpsFusion() // Check for new GPS data that has fallen behind the fusion time horizon if (_gps_data_ready) { - controlGpsYawFusion(); + const bool gps_checks_passing = isTimedOut(_last_gps_fail_us, (uint64_t)5e6); + const bool gps_checks_failing = isTimedOut(_last_gps_pass_us, (uint64_t)5e6); + + controlGpsYawFusion(gps_checks_passing, gps_checks_failing); // Determine if we should use GPS aiding for velocity and horizontal position // To start using GPS we need angular alignment completed, the local NED origin set and GPS data that has not failed checks recently - const bool gps_checks_passing = isTimedOut(_last_gps_fail_us, (uint64_t)5e6); - const bool gps_checks_failing = isTimedOut(_last_gps_pass_us, (uint64_t)5e6); if ((_params.fusion_mode & MASK_USE_GPS) && !_control_status.flags.gps) { if (_control_status.flags.tilt_align && _NED_origin_initialised && gps_checks_passing) { // If the heading is not aligned, reset the yaw and magnetic field states @@ -724,18 +725,20 @@ void Ekf::controlGpsFusion() } else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)10e6)) { stopGpsFusion(); + stopGpsYawFusion(); _warning_events.flags.gps_data_stopped = true; ECL_WARN("GPS data stopped"); } else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)1e6) && isOtherSourceOfHorizontalAidingThan(_control_status.flags.gps)) { // Handle the case where we are fusing another position source along GPS, // stop waiting for GPS after 1 s of lost signal stopGpsFusion(); + stopGpsYawFusion(); _warning_events.flags.gps_data_stopped_using_alternate = true; ECL_WARN("GPS data stopped, using only EV, OF or air data" ); } } -void Ekf::controlGpsYawFusion() +void Ekf::controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing) { if (!(_params.fusion_mode & MASK_USE_GPSYAW) || _is_gps_yaw_faulty) { @@ -744,30 +747,69 @@ void Ekf::controlGpsYawFusion() return; } - if (ISFINITE(_gps_sample_delayed.yaw)) { + const bool is_new_data_available = ISFINITE(_gps_sample_delayed.yaw); + + if (is_new_data_available) { + + const bool continuing_conditions_passing = !gps_checks_failing; + + const bool is_gps_yaw_data_intermittent = !isRecent(_time_last_gps_yaw_data, 2 * GPS_MAX_INTERVAL); + + const bool starting_conditions_passing = continuing_conditions_passing + && _control_status.flags.tilt_align + && gps_checks_passing + && !is_gps_yaw_data_intermittent + && !_gps_hgt_intermittent; + + _time_last_gps_yaw_data = _time_last_imu; if (_control_status.flags.gps_yaw) { - fuseGpsYaw(); - } else { - // Try to activate GPS yaw fusion - if (_control_status.flags.tilt_align - && !_gps_hgt_intermittent) { + if (continuing_conditions_passing) { + fuseGpsYaw(); + + const bool is_fusion_failing = isTimedOut(_time_last_gps_yaw_fuse, _params.reset_timeout_max); + + if (is_fusion_failing) { + if (_nb_gps_yaw_reset_available > 0) { + // Data seems good, attempt a reset + resetYawToGps(); + _nb_gps_yaw_reset_available--; + + } else if (starting_conditions_passing) { + // Data seems good, but previous reset did not fix the issue + // something else must be wrong, declare the sensor faulty and stop the fusion + _is_gps_yaw_faulty = true; + stopGpsYawFusion(); + + } else { + // A reset did not fix the issue but all the starting checks are not passing + // This could be a temporary issue, stop the fusion without declaring the sensor faulty + stopGpsYawFusion(); + } + // TODO: should we give a new reset credit when the fusion does not fail for some time? + } + + } else { + // Stop GPS yaw fusion but do not declare it faulty + stopGpsYawFusion(); + } + + } else { + if (starting_conditions_passing) { + // Try to activate GPS yaw fusion if (resetYawToGps()) { _control_status.flags.yaw_align = true; + _nb_gps_yaw_reset_available = 1; startGpsYawFusion(); } } } - } - // Check if the data is constantly fused by the estimator, - // if not, declare the sensor faulty and stop the fusion - // By doing this, another source of yaw aiding is allowed to start - if (_control_status.flags.gps_yaw - && isTimedOut(_time_last_gps_yaw_fuse, (uint64_t)5e6)) { - _is_gps_yaw_faulty = true; + } else if (_control_status.flags.gps_yaw + && isTimedOut(_time_last_gps_yaw_data, _params.reset_timeout_max)) { + // No yaw data in the message anymore. Stop until it comes back. stopGpsYawFusion(); } } diff --git a/EKF/ekf.h b/EKF/ekf.h index 77f40cce5b..65b633f48c 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -367,6 +367,9 @@ private: uint64_t _time_last_fake_pos{0}; ///< last time we faked position measurements to constrain tilt errors during operation without external aiding (uSec) uint64_t _time_last_gps_yaw_fuse{0}; ///< time the last fusion of GPS yaw measurements were performed (uSec) + uint64_t _time_last_gps_yaw_data{0}; ///< time the last GPS yaw measurement was available (uSec) + uint8_t _nb_gps_yaw_reset_available{0}; ///< remaining number of resets allowed before switching to another aiding source + Vector2f _last_known_posNE; ///< last known local NE position vector (m) float _imu_collection_time_adj{0.0f}; ///< the amount of time the IMU collection needs to be advanced to meet the target set by FILTER_UPDATE_PERIOD_MS (sec) @@ -786,7 +789,7 @@ private: // control fusion of GPS observations void controlGpsFusion(); - void controlGpsYawFusion(); + void controlGpsYawFusion(bool gps_checks_passing, bool gps_checks_failing); // control fusion of magnetometer observations void controlMagFusion();