From 7b4c957ad4c9943d4d88dd0635c34211f603d07d Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Mon, 3 Apr 2017 10:42:26 +1000 Subject: [PATCH] ekf2: Add new mag fusion mode Adds a mode where mag fusion is only used update the field estimates --- EKF/common.h | 1 + EKF/control.cpp | 2 +- EKF/mag_fusion.cpp | 9 +++++++++ 3 files changed, 11 insertions(+), 1 deletion(-) diff --git a/EKF/common.h b/EKF/common.h index 643ccecdb5..70cb67cf49 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -179,6 +179,7 @@ struct dragSample { #define MAG_FUSE_TYPE_AUTO 0 ///< The selection of either heading or 3D magnetometer fusion will be automatic #define MAG_FUSE_TYPE_HEADING 1 ///< Simple yaw angle fusion will always be used. This is less accurate, but less affected by earth field distortions. It should not be used for pitch angles outside the range from -60 to +60 deg #define MAG_FUSE_TYPE_3D 2 ///< Magnetometer 3-axis fusion will always be used. This is more accurate, but more affected by localised earth field distortions +#define MAG_FUSE_TYPE_AUTOFW 3 ///< The same as option 0, but if fusing airspeed, magnetometer fusion is only allowed to modify the magnetic field states. // Maximum sensor intervals in usec #define GPS_MAX_INTERVAL 5e5 ///< Maximum allowable time interval between GPS measurements (uSec) diff --git a/EKF/control.cpp b/EKF/control.cpp index e8c1673d1f..f2b555d4b8 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -1105,7 +1105,7 @@ void Ekf::controlMagFusion() // do no magnetometer fusion at all _control_status.flags.mag_hdg = false; _control_status.flags.mag_3D = false; - } else if (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTO) { + } else if (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTO || _params.mag_fusion_type == MAG_FUSE_TYPE_AUTOFW) { // Check if height has increased sufficiently to be away from ground magnetic anomalies bool height_achieved = (_last_on_ground_posD - _state.pos(2)) > 1.5f; diff --git a/EKF/mag_fusion.cpp b/EKF/mag_fusion.cpp index 2294d1a5b0..7581901000 100644 --- a/EKF/mag_fusion.cpp +++ b/EKF/mag_fusion.cpp @@ -163,6 +163,15 @@ void Ekf::fuseMag() return; } + /* + When flying as a fixed wing aircraft, a misaligned magnetometer can cause an error in pitch/roll and accel bias estimates. + When MAG_FUSE_TYPE_AUTOFW is selected and the vehicle is flying as a fixed wing as detected by the use of wind estimation + and current airspeed estimates, then magnetometer fusion is only allowed to access the magnetic field states. + */ + _control_status.flags.update_mag_states_only = (_params.mag_fusion_type == MAG_FUSE_TYPE_AUTOFW) + && _control_status.flags.wind + && (_time_last_imu - _time_last_airspeed < 1E7); + // update the states and covariance using sequential fusion of the magnetometer components for (uint8_t index = 0; index <= 2; index++) {