From 98fa918b8484dc2ae0c61883d8586e4739cdd0cc Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 12 Mar 2015 18:12:08 -0700 Subject: [PATCH] AP_NavEKF: Add new compass learning option Enables compass learning to be on continuously for non-position hold operation --- libraries/AP_NavEKF/AP_NavEKF.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp index 3e62fa566f..5e7b3630b0 100644 --- a/libraries/AP_NavEKF/AP_NavEKF.cpp +++ b/libraries/AP_NavEKF/AP_NavEKF.cpp @@ -290,7 +290,7 @@ const AP_Param::GroupInfo NavEKF::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 + // @Values: 0:Speed and Height,1:Acceleration,2:Never,3:Always // @Increment: 1 // @User: Advanced AP_GROUPINFO("MAG_CAL", 22, NavEKF, _magCal, MAG_CAL_DEFAULT), @@ -3778,9 +3778,9 @@ void NavEKF::SetFlightAndFusionModes() // If we are on ground, or in constant position mode, or don't have the right vehicle and sensing to estimate wind, inhibit wind states inhibitWindStates = ((!useAirspeed() && !assume_zero_sideslip()) || onGround || constPosMode); // request mag calibration for both in-air and manoeuvre threshold options - bool magCalRequested = ((_magCal == 0) && !onGround) || ((_magCal == 1) && manoeuvring); + bool magCalRequested = ((_magCal == 0) && !onGround) || ((_magCal == 1) && manoeuvring) || (_magCal == 3); // deny mag calibration request if we aren't using the compass, are in the pre-arm constant position mode or it has been inhibited by the user - bool magCalDenied = (!use_compass() || constPosMode || (_magCal == 2)); + bool magCalDenied = !use_compass() || constPosMode || (_magCal == 2); // inhibit the magnetic field calibration if not requested or denied inhibitMagStates = (!magCalRequested || magCalDenied); }