AP_NavEKF : Compensate mag bias states for external copass offset cha…

…nges
3drobotics · Mar 20, 2015 · 45a65db · 45a65db
1 parent 54b230c
commit 45a65db
Show file tree

Hide file tree

Showing 2 changed files with 18 additions and 4 deletions.
diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -3650,12 +3650,12 @@ void NavEKF::getMagXYZ(Vector3f &magXYZ) const
 // return true if offsets are valid
 bool NavEKF::getMagOffsets(Vector3f &magOffsets) const
 {
-    // compass offsets are valid if we have finalised magnetic field initialisation and magnetic field learning is not prohibited
-    if (secondMagYawInit && (_magCal != 2)) {
-        magOffsets = _ahrs->get_compass()->get_offsets() - state.body_magfield*1000.0f;
+    // compass offsets are valid if we have finalised magnetic field initialisation and magnetic field learning is not prohibited and primary compass is valid
+    if (secondMagYawInit && (_magCal != 2) && _ahrs->get_compass()->healthy(0)) {
+        magOffsets = _ahrs->get_compass()->get_offsets(0) - state.body_magfield*1000.0f;
         return true;
     } else {
-        magOffsets = _ahrs->get_compass()->get_offsets();
+        magOffsets = _ahrs->get_compass()->get_offsets(0);
         return false;
     }
 }
@@ -4080,6 +4080,19 @@ void NavEKF::readMagData()
 
         // let other processes know that new compass data has arrived
         newDataMag = true;
+
+        // check if compass offsets have ben changed and adjust EKF bias states to maintain consistent innovations
+        if (_ahrs->get_compass()->healthy(0)) {
+            Vector3f nowMagOffsets = _ahrs->get_compass()->get_offsets(0);
+            bool changeDetected = ((nowMagOffsets.x != lastMagOffsets.x) || (nowMagOffsets.y != lastMagOffsets.y) || (nowMagOffsets.z != lastMagOffsets.z));
+            // Ignore bias changes before final mag field and yaw initialisation, as there may have been a compass calibration
+            if (changeDetected && secondMagYawInit) {
+                state.body_magfield.x += (nowMagOffsets.x - lastMagOffsets.x) * 0.001f;
+                state.body_magfield.y += (nowMagOffsets.y - lastMagOffsets.y) * 0.001f;
+                state.body_magfield.z += (nowMagOffsets.z - lastMagOffsets.z) * 0.001f;
+            }
+        lastMagOffsets = nowMagOffsets;
+        }
     } else {
         newDataMag = false;
     }

diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h
@@ -625,6 +625,7 @@ class NavEKF
     uint32_t startTimeLAND_ms;      // time in msec that the landing condition started - used to compensate for ground effect on baro height
     float gpsSpdAccuracy;           // estimated speed accuracy in m/s returned by the UBlox GPS receiver
     uint32_t lastGpsVelFail_ms;     // time of last GPS vertical velocity consistency check fail
+    Vector3f lastMagOffsets;        // magnetometer offsets returned by compass object from previous update
 
     // Used by smoothing of state corrections
     Vector10 gpsIncrStateDelta;    // vector of corrections to attitude, velocity and position to be applied over the period between the current and next GPS measurement