From ea9e8246edbf0b1c5298f505708019c6ca1630b7 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 16 May 2018 12:14:08 +1000 Subject: [PATCH] EKF: Improve detection of indoor flight condition --- EKF/control.cpp | 6 +++--- EKF/ekf.h | 1 + EKF/mag_fusion.cpp | 6 +++++- 3 files changed, 9 insertions(+), 4 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 2320cc0614..4ef4dfb491 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -1400,13 +1400,13 @@ void Ekf::controlMagFusion() _control_status.flags.mag_dec = false; } - // If optical flow is the only aiding source and GPS checks are failing, then assume that we are operating + // If optical flow is enabled there is no other aiding active and GPS checks are failing, then assume that we are operating // indoors and the magnetometer is unreliable. Becasue the optical flow sensor is body fixed, absolute yaw // wrt North is not required for navigation and it is safer not to use the magnetometer. - if ((_control_status.flags.opt_flow || _mag_use_inhibit) + if (((_params.fusion_mode & MASK_USE_OF) || _mag_use_inhibit) && !_control_status.flags.gps && !_control_status.flags.ev_pos - && (_time_last_imu - _last_gps_pass_us > (uint64_t)5e6)) { + && ((_time_last_imu - _last_gps_pass_us > (uint64_t)5e6) || !(_params.fusion_mode & MASK_USE_GPS))) { _mag_use_inhibit = true; } else { _mag_use_inhibit = false; diff --git a/EKF/ekf.h b/EKF/ekf.h index f2a1174a09..6a2361e361 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -326,6 +326,7 @@ private: bool _mag_use_inhibit{false}; ///< true when magnetomer use is being inhibited bool _mag_inhibit_yaw_reset_req{false}; ///< true when magnetomer inhibit has been active for long enough to require a yaw reset when conditons improve. float _last_static_yaw{0.0f}; ///< last yaw angle recorded when on ground motion checks were passing (rad) + bool _vehicle_at_rest_prev{false}; ///< true when the vehicle was at rest the previous time the status was checked float P[_k_num_states][_k_num_states] {}; ///< state covariance matrix diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index b8894dee7c..1b3d9e50c8 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -624,11 +624,15 @@ void Ekf::fuseHeading() // Vehicle is not at rest so fuse a zero innovation and record the // predicted heading to use as an observation when movement ceases. _heading_innov = 0.0f; - _last_static_yaw = predicted_hdg; + _vehicle_at_rest_prev = false; } else { // Vehicle is at rest so use the last moving prediciton as an observation // to prevent the heading from drifting and to enable yaw gyro bias learning // before takeoff. + if (!_vehicle_at_rest_prev) { + _last_static_yaw = predicted_hdg; + _vehicle_at_rest_prev = true; + } _heading_innov = predicted_hdg - _last_static_yaw; R_YAW = 0.01f; innov_gate = 5.0f;