diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp index 6d1172dc046..7130822bc0e 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp @@ -234,8 +234,8 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = { AP_GROUPINFO("MAG_M_NSE", 13, NavEKF3, _magNoise, MAG_M_NSE_DEFAULT), // @Param: MAG_CAL - // @DisplayName: Magnetometer calibration mode - // @Description: EKF_MAG_CAL = 0 enables calibration when airborne and is the default setting for Plane users. EKF_MAG_CAL = 1 enables calibration when manoeuvreing. EKF_MAG_CAL = 2 prevents magnetometer calibration regardless of flight condition, is recommended if the external magnetic field is varying and is the default for rovers. EKF_MAG_CAL = 3 enables calibration when the first in-air field and yaw reset has completed and is the default for copters. EKF_MAG_CAL = 4 enables calibration all the time. This determines when the filter will use the 3-axis magnetometer fusion model that estimates both earth and body fixed magnetic field states. This model is only suitable for use when the external magnetic field environment is stable. + // @DisplayName: Magnetometer default fusion mode + // @Description: This determines when the filter will use the 3-axis magnetometer fusion model that estimates both earth and body fixed magnetic field states and when it will use a simpler magnetic heading fusion model that does not use magnetic field states. The 3-axis magnetometer fusion is only suitable for use when the external magnetic field environment is stable. EK3_MAG_CAL = 0 uses heading fusion on ground, 3-axis fusion in-flight, and is the default setting for Plane users. EK3_MAG_CAL = 1 uses 3-axis fusion only when manoeuvring. EK3_MAG_CAL = 2 uses heading fusion at all times, is recommended if the external magnetic field is varying and is the default for rovers. EK3_MAG_CAL = 3 uses heading fusion on the ground and 3-axis fusion after the first in-air field and yaw reset has completed, and is the default for copters. EK3_MAG_CAL = 4 uses 3-axis fusion at all times. NOTE : Use of simple heading magnetometer fusion makes vehicle compass calibration and alignment errors harder for the EKF to detect which reduces the sensitivity of the Copter EKF failsafe algorithm. NOTE: The fusion mode can be forced to 2 for specific EKF cores using the EK3_MAG_MASK parameter. // @Values: 0:When flying,1:When manoeuvring,2:Never,3:After first climb yaw reset,4:Always // @User: Advanced AP_GROUPINFO("MAG_CAL", 14, NavEKF3, _magCal, MAG_CAL_DEFAULT), @@ -524,6 +524,13 @@ const AP_Param::GroupInfo NavEKF3::var_info[] = { // @Units: m/s/s AP_GROUPINFO("ACC_BIAS_LIM", 48, NavEKF3, _accBiasLim, 1.0f), + // @Param: MAG_MASK + // @DisplayName: Bitmask of active EKF cores that will always use heading fusion + // @Description: 1 byte bitmap of EKF cores that will disable magnetic field states and use simple magnetic heading fusion at all times. This parameter enables specified cores to be used as a backup for flight into an environment with high levels of external magnetic interference which may degrade the EKF attitude estimate when using 3-axis magnetometer fusion. NOTE : Use of a different magnetometer fusion algorithm on different cores makes unwanted EKF core switches due to magnetometer errors more likely. + // @Bitmask: 0:FirstEKF,1:SecondEKF,2:ThirdEKF,3:FourthEKF,4:FifthEKF,5:SixthEKF + // @User: Advanced + AP_GROUPINFO("MAG_MASK", 49, NavEKF3, _magMask, 0), + AP_GROUPEND }; diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.h b/libraries/AP_NavEKF3/AP_NavEKF3.h index 6e18aec1a78..be259366c4c 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3.h +++ b/libraries/AP_NavEKF3/AP_NavEKF3.h @@ -358,6 +358,7 @@ private: AP_Int8 _rngBcnDelay_ms; // effective average delay of range beacon measurements rel to IMU (msec) AP_Float _useRngSwSpd; // Maximum horizontal ground speed to use range finder as the primary height source (m/s) AP_Float _accBiasLim; // Accelerometer bias limit (m/s/s) + AP_Int8 _magMask; // Bitmask forcng specific EKF core instances to use simple heading magnetometer fusion. // Tuning parameters const float gpsNEVelVarAccScale; // Scale factor applied to NE velocity measurement variance due to manoeuvre acceleration diff --git a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp index 8fd587c1369..3578743a7c6 100644 --- a/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp +++ b/libraries/AP_NavEKF3/AP_NavEKF3_Control.cpp @@ -45,7 +45,12 @@ void NavEKF3_core::controlFilterModes() */ uint8_t NavEKF3_core::effective_magCal(void) const { - return frontend->_magCal; + // force use of simple magnetic heading fusion for specified cores + if (frontend->_magMask & core_index) { + return 2; + } else { + return frontend->_magCal; + } } // Determine if learning of wind and magnetic field will be enabled and set corresponding indexing limits to