AP_NavEKF: Enforce published magnetic declination during field state …

…reset
3drobotics · Nov 17, 2015 · 331103b · 331103b
1 parent c082c8e
commit 331103b
Show file tree

Hide file tree

Showing 2 changed files with 21 additions and 0 deletions.
diff --git a/libraries/AP_NavEKF/AP_NavEKF.cpp b/libraries/AP_NavEKF/AP_NavEKF.cpp
@@ -4213,6 +4213,8 @@ void NavEKF::readGpsData()
         // Set the EKF origin and magnetic field declination if not previously set  and GPS checks have passed
         if (!validOrigin && gpsGoodToAlign) {
             setOrigin();
+            // Now we know the location we have an estimate for the magnetic field declination and adjust the earth field accordingly
+            alignMagStateDeclination();
             // Set the height of the NED origin to ‘height of baro height datum relative to GPS height datum'
             EKF_origin.alt = gpsloc.alt - hgtMea;
         }
@@ -4470,6 +4472,9 @@ Quaternion NavEKF::calcQuatAndFieldStates(float roll, float pitch)
         // write to earth magnetic field state vector
         state.earth_magfield = initMagNED;
 
+        // align the NE earth magnetic field states with the published declination
+        alignMagStateDeclination();
+
         // clear bad magnetometer status
         badMag = false;
     } else {
@@ -5437,4 +5442,17 @@ uint32_t NavEKF::getLastYawResetAngle(float &yawAng)
     return lastYawReset_ms;
 }
 
+// align the NE earth magnetic field states with the published declination
+void NavEKF::alignMagStateDeclination()
+{
+    // get the magnetic declination
+    float magDecAng = use_compass() ? _ahrs->get_compass()->get_declination() : 0;
+
+    // rotate the NE values so that the declination matches the published value
+    Vector3f initMagNED = state.earth_magfield;
+    float magLengthNE = pythagorous2(initMagNED.x,initMagNED.y);
+    state.earth_magfield.x = magLengthNE * cosf(magDecAng);
+    state.earth_magfield.y = magLengthNE * sinf(magDecAng);
+}
+
 #endif // HAL_CPU_CLASS
diff --git a/libraries/AP_NavEKF/AP_NavEKF.h b/libraries/AP_NavEKF/AP_NavEKF.h
@@ -464,6 +464,9 @@ class NavEKF
     // update inflight calculaton that determines if GPS data is good enough for reliable navigation
     void calcGpsGoodForFlight(void);
 
+    // align the NE earth magnetic field states with the published declination
+    void alignMagStateDeclination();
+
     // EKF Mavlink Tuneable Parameters
     AP_Float _gpsHorizVelNoise;     // GPS horizontal velocity measurement noise : m/s
     AP_Float _gpsVertVelNoise;      // GPS vertical velocity measurement noise : m/s