From e7de2d3ea3a08780006a106a02927f5ec986f4b8 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 16 Oct 2015 23:18:23 +1100 Subject: [PATCH] AP_NavEKF: Update magnetic field learning options Provide an option to always do learning Make field learning decision logic clearer Change defaults so that plane learns when airborne Change defaults so that Rover does not learn (large external magnetic interference) --- libraries/AP_NavEKF2/AP_NavEKF2.cpp | 8 ++++---- libraries/AP_NavEKF2/AP_NavEKF2_Control.cpp | 10 +++++++--- 2 files changed, 11 insertions(+), 7 deletions(-) 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);