From 4c22db3b4704e78700d2c5a2cefc6c20c3936fae Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 28 Oct 2022 09:53:24 -0400 Subject: [PATCH] ekf2: handle GPS vel/pos timeout in addition to overall reset check - in a system with good EV + GPS it's possible that horizontal vel/pos fusion continues successfully, but GPS fusion is failing due to mediocre yaw alignment - GPS always check for yaw failure and act on it if there's corresponding GPS vel or GPS pos fusion timeout - add additional protections to prevent multiple yaw resets/alignments per update (the controllers won't get the proper heading delta if this happens) --- src/modules/ekf2/EKF/control.cpp | 8 +--- src/modules/ekf2/EKF/ekf.h | 3 -- src/modules/ekf2/EKF/ekf_helper.cpp | 53 +++++++++---------------- src/modules/ekf2/EKF/gps_control.cpp | 6 ++- src/modules/ekf2/EKF/gps_yaw_fusion.cpp | 37 +++++++++++------ 5 files changed, 49 insertions(+), 58 deletions(-) diff --git a/src/modules/ekf2/EKF/control.cpp b/src/modules/ekf2/EKF/control.cpp index 4a4c3a8461..56103a4b7a 100644 --- a/src/modules/ekf2/EKF/control.cpp +++ b/src/modules/ekf2/EKF/control.cpp @@ -86,10 +86,7 @@ void Ekf::controlFusionModes() } if (_gps_buffer) { - _gps_intermittent = !isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GPS_MAX_INTERVAL); - // check for arrival of new sensor data at the fusion time horizon - _time_prev_gps_us = _gps_sample_delayed.time_us; _gps_data_ready = _gps_buffer->pop_first_older_than(_imu_sample_delayed.time_us, &_gps_sample_delayed); if (_gps_data_ready) { @@ -103,8 +100,6 @@ void Ekf::controlFusionModes() const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; _gps_sample_delayed.pos -= pos_offset_earth.xy(); _gps_sample_delayed.hgt += pos_offset_earth(2); - - _gps_sample_delayed.sacc = fmaxf(_gps_sample_delayed.sacc, _params.gps_vel_noise); } } @@ -210,13 +205,14 @@ void Ekf::controlGpsYawFusion(const gpsSample &gps_sample, bool gps_checks_passi const bool continuing_conditions_passing = !gps_checks_failing; + const bool gps_intermittent = !isNewestSampleRecent(_time_last_gps_buffer_push, 2 * GPS_MAX_INTERVAL); const bool is_gps_yaw_data_intermittent = !isNewestSampleRecent(_time_last_gps_yaw_buffer_push, 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_intermittent; + && !gps_intermittent; if (_control_status.flags.gps_yaw) { diff --git a/src/modules/ekf2/EKF/ekf.h b/src/modules/ekf2/EKF/ekf.h index 669c456975..758d30d213 100644 --- a/src/modules/ekf2/EKF/ekf.h +++ b/src/modules/ekf2/EKF/ekf.h @@ -632,7 +632,6 @@ private: // height sensor status bool _baro_hgt_faulty{false}; ///< true if baro data have been declared faulty TODO: move to fault flags - bool _gps_intermittent{true}; ///< true if data into the buffer is intermittent // imu fault status uint64_t _time_bad_vert_accel{0}; ///< last time a bad vertical accel was detected (uSec) @@ -1015,8 +1014,6 @@ private: void stopAirspeedFusion(); void stopGpsFusion(); - void stopGpsPosFusion(); - void stopGpsVelFusion(); void startGpsYawFusion(const gpsSample &gps_sample); void stopGpsYawFusion(); diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index 92f6bde383..19b1c31c00 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -1220,8 +1220,10 @@ void Ekf::stopAirspeedFusion() void Ekf::stopGpsFusion() { if (_control_status.flags.gps) { - stopGpsPosFusion(); - stopGpsVelFusion(); + ECL_INFO("stopping GPS position and velocity fusion"); + + resetEstimatorAidStatus(_aid_src_gnss_pos); + resetEstimatorAidStatus(_aid_src_gnss_vel); _control_status.flags.gps = false; } @@ -1235,23 +1237,6 @@ void Ekf::stopGpsFusion() _inhibit_ev_yaw_use = false; } -void Ekf::stopGpsPosFusion() -{ - if (_control_status.flags.gps) { - ECL_INFO("stopping GPS position fusion"); - _control_status.flags.gps = false; - - resetEstimatorAidStatus(_aid_src_gnss_pos); - } -} - -void Ekf::stopGpsVelFusion() -{ - ECL_INFO("stopping GPS velocity fusion"); - - resetEstimatorAidStatus(_aid_src_gnss_vel); -} - void Ekf::startGpsYawFusion(const gpsSample &gps_sample) { if (!_control_status.flags.gps_yaw && resetYawToGps(gps_sample.yaw)) { @@ -1336,30 +1321,25 @@ void Ekf::resetQuatStateYaw(float yaw, float yaw_variance) // Returns true if the reset was successful bool Ekf::resetYawToEKFGSF() { - if (!isYawEmergencyEstimateAvailable()) { + if (!isYawEmergencyEstimateAvailable() || !isTimedOut(_ekfgsf_yaw_reset_time, 5'000'000)) { return false; } + // prevent a reset being performed more than once on the same frame + if ((_flt_mag_align_start_time == _imu_sample_delayed.time_us) || (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align)) { + return false; + } + + // otherwise reset yaw resetQuatStateYaw(_yawEstimator.getYaw(), _yawEstimator.getYawVar()); + _information_events.flags.yaw_aligned_to_imu_gps = true; + ECL_INFO("Yaw aligned using IMU and GPS"); + // record a magnetic field alignment event to prevent possibility of the EKF trying to reset the yaw to the mag later in flight _flt_mag_align_start_time = _imu_sample_delayed.time_us; _control_status.flags.yaw_align = true; - if (_control_status.flags.mag_hdg || _control_status.flags.mag_3D) { - // stop using the magnetometer in the main EKF otherwise it's fusion could drag the yaw around - // and cause another navigation failure - _control_status.flags.mag_fault = true; - _warning_events.flags.emergency_yaw_reset_mag_stopped = true; - - } else if (_control_status.flags.gps_yaw) { - _control_status.flags.gps_yaw_fault = true; - _warning_events.flags.emergency_yaw_reset_gps_yaw_stopped = true; - - } else if (_control_status.flags.ev_yaw) { - _inhibit_ev_yaw_use = true; - } - _ekfgsf_yaw_reset_time = _imu_sample_delayed.time_us; _ekfgsf_yaw_reset_count++; @@ -1398,6 +1378,11 @@ void Ekf::runYawEKFGSF() const Vector3f imu_gyro_bias = getGyroBias(); _yawEstimator.update(_imu_sample_delayed, _control_status.flags.in_air, TAS, imu_gyro_bias); + + // reset when landed + if (!_control_status.flags.in_air) { + _ekfgsf_yaw_reset_count = 0; + } } void Ekf::resetGpsDriftCheckFilters() diff --git a/src/modules/ekf2/EKF/gps_control.cpp b/src/modules/ekf2/EKF/gps_control.cpp index 6f5062282e..fab4306452 100644 --- a/src/modules/ekf2/EKF/gps_control.cpp +++ b/src/modules/ekf2/EKF/gps_control.cpp @@ -92,8 +92,10 @@ void Ekf::controlGpsFusion() _aid_src_gnss_pos.fusion_enabled = (_params.gnss_ctrl & GnssCtrl::HPOS); // update GSF yaw estimator velocity (basic sanity check on GNSS velocity data) - if (gps_checks_passing && !gps_checks_failing) { - _yawEstimator.setVelocity(velocity.xy(), gps_sample.sacc); + if (_gps_speed_valid && velocity.isAllFinite() + && (gps_sample.sacc > FLT_EPSILON) && (gps_sample.sacc <= _params.req_sacc)) { + + _yawEstimator.setVelocity(velocity.xy(), vel_noise); } // if GPS is otherwise ready to go, but yaw_align is blocked by EV give mag a chance to start diff --git a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp index c5017d5fed..318dc41744 100644 --- a/src/modules/ekf2/EKF/gps_yaw_fusion.cpp +++ b/src/modules/ekf2/EKF/gps_yaw_fusion.cpp @@ -203,23 +203,34 @@ void Ekf::fuseGpsYaw(const gpsSample& gps_sample) bool Ekf::resetYawToGps(const float gnss_yaw) { - // define the predicted antenna array vector and rotate into earth frame - const Vector3f ant_vec_bf = {cosf(_gps_yaw_offset), sinf(_gps_yaw_offset), 0.0f}; - const Vector3f ant_vec_ef = _R_to_earth * ant_vec_bf; - - // check if antenna array vector is within 30 degrees of vertical and therefore unable to provide a reliable heading - if (fabsf(ant_vec_ef(2)) > cosf(math::radians(30.0f))) { + // prevent a reset being performed more than once on the same frame + if ((_flt_mag_align_start_time == _imu_sample_delayed.time_us) || (_control_status_prev.flags.yaw_align != _control_status.flags.yaw_align)) { return false; } - // GPS yaw measurement is alreday compensated for antenna offset in the driver - const float measured_yaw = gnss_yaw; + if (PX4_ISFINITE(gnss_yaw) && !_control_status.flags.gps_yaw_fault) { + // define the predicted antenna array vector and rotate into earth frame + const Vector3f ant_vec_bf = {cosf(_gps_yaw_offset), sinf(_gps_yaw_offset), 0.0f}; + const Vector3f ant_vec_ef = _R_to_earth * ant_vec_bf; - const float yaw_variance = sq(fmaxf(_params.gps_heading_noise, 1.e-2f)); - resetQuatStateYaw(measured_yaw, yaw_variance); + // check if antenna array vector is within 30 degrees of vertical and therefore unable to provide a reliable heading + if (fabsf(ant_vec_ef(2)) > cosf(math::radians(30.0f))) { + return false; + } - _aid_src_gnss_yaw.time_last_fuse = _imu_sample_delayed.time_us; - _gnss_yaw_signed_test_ratio_lpf.reset(0.f); + // GPS yaw measurement is alreday compensated for antenna offset in the driver + const float measured_yaw = gnss_yaw; - return true; + const float yaw_variance = sq(fmaxf(_params.gps_heading_noise, 1.e-2f)); + resetQuatStateYaw(measured_yaw, yaw_variance); + + _control_status.flags.yaw_align = true; + + _aid_src_gnss_yaw.time_last_fuse = _imu_sample_delayed.time_us; + _gnss_yaw_signed_test_ratio_lpf.reset(0.f); + + return true; + } + + return false; }