Added support for automatic mag declination setup

3 files changed, 35 insertions, 18 deletions

diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp
index 8ac2d6154..1ca34ec30 100644
--- a/src/modules/ekf_att_pos_estimator/estimator.cpp
+++ b/src/modules/ekf_att_pos_estimator/estimator.cpp
@@ -158,7 +158,8 @@ AttPosEKF::AttPosEKF() :
     gpsHgt(0.0f),
     baroHgt(0.0f),
     GPSstatus(0),
-    VtasMeas(0.0f)
+    VtasMeas(0.0f),
+    magDeclination(0.0f)
 {
     velNED[0] = 0.0f;
     velNED[1] = 0.0f;
@@ -2341,7 +2342,7 @@ int AttPosEKF::CheckAndBound()
     if (StatesNaN(&last_ekf_error)) {
         ekf_debug("re-initializing dynamic");
 
-        InitializeDynamic(velNED);
+        InitializeDynamic(velNED, magDeclination);
 
         return 1;
     }
@@ -2374,7 +2375,7 @@ int AttPosEKF::CheckAndBound()
     return 0;
 }
 
-void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat)
+void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat)
 {
     float initialRoll, initialPitch;
     float cosRoll, sinRoll, cosPitch, sinPitch;
@@ -2394,6 +2395,8 @@ void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, f
     magY = my * cosRoll - mz * sinRoll;
 
     initialHdg = atan2f(-magY, magX);
+    /* true heading is the mag heading minus declination */
+    initialHdg -= declination;
 
     cosRoll = cosf(initialRoll * 0.5f);
     sinRoll = sinf(initialRoll * 0.5f);
@@ -2408,9 +2411,17 @@ void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, f
     initQuat[1] = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading;
     initQuat[2] = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading;
     initQuat[3] = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading;
+
+    /* normalize */
+    float norm = sqrtf(initQuat[0]*initQuat[0] + initQuat[1]*initQuat[1] + initQuat[2]*initQuat[2] + initQuat[3]*initQuat[3]);
+
+    initQuat[0] /= norm;
+    initQuat[1] /= norm;
+    initQuat[2] /= norm;
+    initQuat[3] /= norm;
 }
 
-void AttPosEKF::InitializeDynamic(float (&initvelNED)[3])
+void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination)
 {
 
     // Clear the init flag
@@ -2431,11 +2442,13 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3])
 
     ZeroVariables();
 
+    magDeclination = declination;
+
     // Calculate initial filter quaternion states from raw measurements
     float initQuat[4];
     Vector3f initMagXYZ;
     initMagXYZ   = magData - magBias;
-    AttitudeInit(accel.x, accel.y, accel.z, initMagXYZ.x, initMagXYZ.y, initMagXYZ.z, initQuat);
+    AttitudeInit(accel.x, accel.y, accel.z, initMagXYZ.x, initMagXYZ.y, initMagXYZ.z, declination, initQuat);
 
     // Calculate initial Tbn matrix and rotate Mag measurements into NED
     // to set initial NED magnetic field states
@@ -2451,9 +2464,9 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3])
     magstate.q1 = initQuat[1];
     magstate.q2 = initQuat[2];
     magstate.q3 = initQuat[3];
-    magstate.magN = magData.x;
-    magstate.magE = magData.y;
-    magstate.magD = magData.z;
+    magstate.magN = initMagNED.x;
+    magstate.magE = initMagNED.y;
+    magstate.magD = initMagNED.z;
     magstate.magXbias = magBias.x;
     magstate.magYbias = magBias.y;
     magstate.magZbias = magBias.z;
@@ -2493,7 +2506,7 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3])
     summedDelVel.z = 0.0f;
 }
 
-void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt)
+void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination)
 {
     //store initial lat,long and height
     latRef = referenceLat;
@@ -2503,7 +2516,7 @@ void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, do
 
     memset(&last_ekf_error, 0, sizeof(last_ekf_error));
 
-    InitializeDynamic(initvelNED);
+    InitializeDynamic(initvelNED, declination);
 }
 
 void AttPosEKF::ZeroVariables()
diff --git a/src/modules/ekf_att_pos_estimator/estimator.h b/src/modules/ekf_att_pos_estimator/estimator.h
index e118ae573..378107b69 100644
--- a/src/modules/ekf_att_pos_estimator/estimator.h
+++ b/src/modules/ekf_att_pos_estimator/estimator.h
@@ -208,6 +208,7 @@ public:
     float innovRng; ///< Range finder innovation
     float varInnovVtas; // innovation variance output
     float VtasMeas; // true airspeed measurement (m/s)
+    float magDeclination;       ///< magnetic declination
     double latRef; // WGS-84 latitude of reference point (rad)
     double lonRef; // WGS-84 longitude of reference point (rad)
     float hgtRef; // WGS-84 height of reference point (m)
@@ -309,7 +310,7 @@ void OnGroundCheck();
 
 void CovarianceInit();
 
-void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt);
+void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination);
 
 float ConstrainFloat(float val, float min, float max);
 
@@ -334,7 +335,7 @@ void GetLastErrorState(struct ekf_status_report *last_error);
 bool StatesNaN(struct ekf_status_report *err_report);
 void FillErrorReport(struct ekf_status_report *err);
 
-void InitializeDynamic(float (&initvelNED)[3]);
+void InitializeDynamic(float (&initvelNED)[3], float declination);
 
 protected:
 
@@ -342,7 +343,7 @@ bool FilterHealthy();
 
 void ResetHeight(void);
 
-void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat);
+void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat);
 
 };
 
diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp
index a82e8d704..23f9e80bd 100644
--- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp
@@ -994,11 +994,14 @@ FixedwingEstimator::task_main()
 					_ekf->velNED[1] = _gps.vel_e_m_s;
 					_ekf->velNED[2] = _gps.vel_d_m_s;
 
-					_ekf->gpsLat = math::radians(_gps.lat / (double)1e7);
-					_ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI;
-					_ekf->gpsHgt = _gps.alt / 1e3f;
+					_ekf->gpsLat = math::radians(lat);
+					_ekf->gpsLon = math::radians(lon) - M_PI;
+					_ekf->gpsHgt = gps_alt;
+
+					// Look up mag declination based on current position
+					float declination = math::radians(get_mag_declination(lat, lon));
 
-					_ekf->InitialiseFilter(_ekf->velNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt);
+					_ekf->InitialiseFilter(_ekf->velNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination);
 
 					// Initialize projection
 					_local_pos.ref_lat = _gps.lat;
@@ -1024,7 +1027,7 @@ FixedwingEstimator::task_main()
 
 					_ekf->posNE[0] = _ekf->posNED[0];
 					_ekf->posNE[1] = _ekf->posNED[1];
-					_ekf->InitialiseFilter(_ekf->velNED, 0.0, 0.0, 0.0f);
+					_ekf->InitialiseFilter(_ekf->velNED, 0.0, 0.0, 0.0f, 0.0f);
 				}
 			}