diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index 012354b16f..0d2cacd613 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -52,7 +52,7 @@ #define POS_GATE_DEFAULT 5 #define HGT_GATE_DEFAULT 5 #define MAG_GATE_DEFAULT 3 -#define MAG_CAL_DEFAULT 3 +#define MAG_CAL_DEFAULT 2 #define GLITCH_RADIUS_DEFAULT 25 #define FLOW_MEAS_DELAY 10 #define FLOW_NOISE_DEFAULT 0.25f @@ -75,7 +75,7 @@ #define POS_GATE_DEFAULT 5 #define HGT_GATE_DEFAULT 10 #define MAG_GATE_DEFAULT 3 -#define MAG_CAL_DEFAULT 3 +#define MAG_CAL_DEFAULT 0 #define GLITCH_RADIUS_DEFAULT 25 #define FLOW_MEAS_DELAY 10 #define FLOW_NOISE_DEFAULT 0.25f @@ -209,8 +209,8 @@ const AP_Param::GroupInfo NavEKF2::var_info[] PROGMEM = { // @Param: MAG_CAL // @DisplayName: Magnetometer calibration mode - // @Description: EKF_MAG_CAL = 0 enables calibration based on flying speed and altitude and is the default setting for Plane users. EKF_MAG_CAL = 1 enables calibration based on manoeuvre level and is the default setting for Copter and Rover users. EKF_MAG_CAL = 2 prevents magnetometer calibration regardless of flight condition and is recommended if in-flight magnetometer calibration is unreliable. - // @Values: 0:Speed and Height,1:Acceleration,2:Never,3:Always + // @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. + // @Values: 0:When flying,1:When manoeuvring,2:Never,3:After first climb yaw reset,4:Always // @User: Advanced AP_GROUPINFO("MAG_CAL", 14, NavEKF2, _magCal, MAG_CAL_DEFAULT), diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp index 2880aaa02c..1c95337af4 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp @@ -64,10 +64,14 @@ void NavEKF2_core::setWindMagStateLearningMode() } // Determine if learning of magnetic field states has been requested by the user - bool magCalRequested = ((frontend._magCal == 0) && !onGround) || ((frontend._magCal == 1) && manoeuvring) || (frontend._magCal == 3); + bool magCalRequested = + ((frontend._magCal == 0) && inFlight) || // when flying + ((frontend._magCal == 1) && manoeuvring) || // when manoeuvring + ((frontend._magCal == 3) && firstMagYawInit) || // when initial in-air yaw and field reset has completed + (frontend._magCal == 4); // all the time - // Deny mag calibration request if we aren't using the compass, have not perfomred the first in-air calibration are on the ground or it has been inhibited by the user - bool magCalDenied = !use_compass() || !firstMagYawInit || (frontend._magCal == 2) || onGround; + // Deny mag calibration request if we aren't using the compass or it has been inhibited by the user + bool magCalDenied = !use_compass() || (frontend._magCal == 2); // Inhibit the magnetic field calibration if not requested or denied inhibitMagStates = (!magCalRequested || magCalDenied);