diff --git a/EKF/common.h b/EKF/common.h index 4b76296457..fad9a04a14 100644 --- a/EKF/common.h +++ b/EKF/common.h @@ -189,6 +189,7 @@ struct auxVelSample { #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. +#define MAG_FUSE_TYPE_INDOOR 4 ///< The same as option 0, but magnetomer or yaw fusion will not be used unless earth frame external aiding (GPS or External Vision) is being used. This prevents inconsistent magnetic fields associated with indoor operation degrading state estimates. // Maximum sensor intervals in usec #define GPS_MAX_INTERVAL (uint64_t)5e5 ///< Maximum allowable time interval between GPS measurements (uSec) diff --git a/EKF/control.cpp b/EKF/control.cpp index 7339733251..7cdcb7c30b 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -1408,8 +1408,14 @@ void Ekf::controlMagFusion() _control_status.flags.mag_dec = false; } - // If GPS is not being used and there is no other earth frame aiding, assume that we are operating indoors and the magnetometer is unreliable. - if ((!_control_status.flags.gps || !(_params.fusion_mode & MASK_USE_GPS)) && !_control_status.flags.ev_pos) { + // If the user has selected auto protection against indoor magnetic field errors, only use the magnetomer + // if a yaw angle relative to true North is required for navigation. If no GPS or other earth frame aiding + // is available, assume that we are operating indoors and the magnetometer should not be used. + bool user_selected = (_params.mag_fusion_type == MAG_FUSE_TYPE_INDOOR); + bool not_using_gps = !(_params.fusion_mode & MASK_USE_GPS) || !_control_status.flags.gps; + bool not_using_evpos = !(_params.fusion_mode & MASK_USE_EVPOS) || !_control_status.flags.ev_pos; + bool not_selected_evyaw = !(_params.fusion_mode & MASK_USE_EVYAW); + if (user_selected && not_using_gps && not_using_evpos && not_selected_evyaw) { _mag_use_inhibit = true; } else { _mag_use_inhibit = false;