From 48722a2a920624d2d499a07f77d12afd0ff3ba75 Mon Sep 17 00:00:00 2001 From: bresch Date: Fri, 7 May 2021 16:14:49 +0200 Subject: [PATCH] control: inhibit EV yaw from restarting when GPS aid wants to start --- EKF/control.cpp | 19 +++++++++++++------ EKF/ekf.h | 1 + EKF/ekf_helper.cpp | 4 ++++ 3 files changed, 18 insertions(+), 6 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 0556f4391b..24946db33f 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -190,6 +190,10 @@ void Ekf::controlExternalVisionFusion() // Check for new external vision data if (_ev_data_ready) { + if (_inhibit_ev_yaw_use) { + stopEvYawFusion(); + } + // if the ev data is not in a NED reference frame, then the transformation between EV and EKF navigation frames // needs to be calculated and the observations rotated into the EKF frame of reference if ((_params.fusion_mode & MASK_ROTATE_EV) && ((_params.fusion_mode & MASK_USE_EVPOS) || (_params.fusion_mode & MASK_USE_EVVEL)) && !_control_status.flags.ev_yaw) { @@ -215,7 +219,7 @@ void Ekf::controlExternalVisionFusion() } // external vision yaw aiding selection logic - if (!_control_status.flags.gps && (_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw && _control_status.flags.tilt_align) { + if (!_inhibit_ev_yaw_use && (_params.fusion_mode & MASK_USE_EVYAW) && !_control_status.flags.ev_yaw && _control_status.flags.tilt_align) { // don't start using EV data unless data is arriving frequently if (isRecent(_time_last_ext_vision, 2 * EV_MAX_INTERVAL)) { startEvYawFusion(); @@ -521,13 +525,16 @@ void Ekf::controlGpsFusion() // If the heading is not aligned, reset the yaw and magnetic field states // Do not use external vision for yaw if using GPS because yaw needs to be // defined relative to an NED reference frame - const bool want_to_reset_mag_heading = !_control_status.flags.yaw_align || - _control_status.flags.ev_yaw || - _mag_inhibit_yaw_reset_req; + if (!_control_status.flags.yaw_align + || _control_status.flags.ev_yaw + || _mag_inhibit_yaw_reset_req + || _mag_yaw_reset_req) { - if (want_to_reset_mag_heading) { _mag_yaw_reset_req = true; - _control_status.flags.ev_yaw = false; + + // Stop the vision for yaw fusion and do not allow it to start again + stopEvYawFusion(); + _inhibit_ev_yaw_use = true; } else { // If the heading is valid start using gps aiding diff --git a/EKF/ekf.h b/EKF/ekf.h index 65b633f48c..f753aa530f 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -341,6 +341,7 @@ private: Vector2f _hpos_pred_prev; ///< previous value of NE position state used by odometry fusion (m) bool _hpos_prev_available{false}; ///< true when previous values of the estimate and measurement are available for use Dcmf _R_ev_to_ekf; ///< transformation matrix that rotates observations from the EV to the EKF navigation frame, initialized with Identity + bool _inhibit_ev_yaw_use{false}; ///< true when the vision yaw data should not be used (e.g.: NE fusion requires true North) // booleans true when fresh sensor data is available at the fusion time horizon bool _gps_data_ready{false}; ///< true when new GPS data has fallen behind the fusion time horizon and is available to be fused diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index c44e063686..21e5a3a623 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -1458,6 +1458,10 @@ void Ekf::stopGpsFusion() stopGpsPosFusion(); stopGpsVelFusion(); stopGpsYawFusion(); + + // We do not need to know the true North anymore + // EV yaw can start again + _inhibit_ev_yaw_use = false;; } void Ekf::stopGpsPosFusion()