From 081e17729c6f249054e857d11523ff5a47fdce4f Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 6 Jun 2016 21:59:46 +1000 Subject: [PATCH] EKF: delay commencement of 3D mag fusion until clear of ground Wait until enough height has been gained to be clear of ground based magnetic anomalies. Failure to do so can result in incorrect earth field initialisation. --- EKF/control.cpp | 38 ++++++++++++++++++++++++-------------- EKF/ekf.cpp | 3 ++- EKF/ekf.h | 6 ++++-- 3 files changed, 30 insertions(+), 17 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 9266315cec..3188311189 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -552,6 +552,11 @@ void Ekf::controlHeightAiding() _control_status.flags.ev_hgt = true; } + + // If we are on ground, store the local position and time to use as a reference for takeoff checks + if (!_control_status.flags.in_air) { + _last_on_ground_posD = _state.pos(2); + } } void Ekf::controlMagAiding() @@ -564,22 +569,27 @@ void Ekf::controlMagAiding() // Determine if we should use simple magnetic heading fusion which works better when there are large external disturbances // or the more accurate 3-axis fusion if (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTO) { + // start 3D fusion if in-flight and height has increased sufficiently + // to be away from ground magnetic anomalies + // don't switch back to heading fusion until we are back on the ground + bool height_achieved = (_last_on_ground_posD - _state.pos(2)) > 1.5f; + bool use_3D_fusion = _control_status.flags.in_air && (_control_status.flags.mag_3D || height_achieved); + + if (use_3D_fusion && _control_status.flags.tilt_align) { + // if transitioning into 3-axis fusion mode, we need to initialise the yaw angle and field states + if (!_control_status.flags.mag_3D) { + _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); + } - if (_control_status.flags.in_air && _control_status.flags.tilt_align) { - // if transitioning into 3-axis fusion mode, we need to initialise the yaw angle and field states - if (!_control_status.flags.mag_3D) { - _control_status.flags.yaw_align = resetMagHeading(_mag_sample_delayed.mag); - } - - // use 3D mag fusion when airborne - _control_status.flags.mag_hdg = false; - _control_status.flags.mag_3D = true; + // use 3D mag fusion when airborne + _control_status.flags.mag_hdg = false; + _control_status.flags.mag_3D = true; - } else { - // use heading fusion when on the ground - _control_status.flags.mag_hdg = true; - _control_status.flags.mag_3D = false; - } + } else { + // use heading fusion when on the ground + _control_status.flags.mag_hdg = true; + _control_status.flags.mag_3D = false; + } } else if (_params.mag_fusion_type == MAG_FUSE_TYPE_HEADING) { // always use heading fusion diff --git a/EKF/ekf.cpp b/EKF/ekf.cpp index 35488673d1..b4f09b02fb 100644 --- a/EKF/ekf.cpp +++ b/EKF/ekf.cpp @@ -98,6 +98,8 @@ Ekf::Ekf(): _ev_counter(0), _time_last_mag(0), _hgt_sensor_offset(0.0f), + _baro_hgt_offset(0.0f), + _last_on_ground_posD(0.0f), _terrain_vpos(0.0f), _terrain_var(1.e4f), _hagl_innov(0.0f), @@ -106,7 +108,6 @@ Ekf::Ekf(): _baro_hgt_faulty(false), _gps_hgt_faulty(false), _rng_hgt_faulty(false), - _baro_hgt_offset(0.0f), _time_bad_vert_accel(0) { _state = {}; diff --git a/EKF/ekf.h b/EKF/ekf.h index 88237bd3f3..17aa1bf28a 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -248,6 +248,10 @@ private: Vector3f _mag_filt_state; // filtered magnetometer measurement Vector3f _delVel_sum; // summed delta velocity float _hgt_sensor_offset; // set as necessary if desired to maintain the same height after a height reset (m) + float _baro_hgt_offset; // baro height reading at the local NED origin (m) + + // Variables used to control activation of post takeoff functionality + float _last_on_ground_posD; // last vertical position when the in_air status was false (m) gps_check_fail_status_u _gps_check_fail_status; @@ -265,8 +269,6 @@ private: bool _rng_hgt_faulty; // true if valid rnage finder height data is unavailable for use int _primary_hgt_source; // priary source of height data set at initialisation - float _baro_hgt_offset; // baro height reading at the local NED origin (m) - // imu fault status uint64_t _time_bad_vert_accel; // last time a bad vertical accel was detected (usec)