attitude_estimator_ekf: mag declination parameter implemented

3 files changed, 25 insertions, 6 deletions

diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
index a70a14fe4..983ac7c8d 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
@@ -61,6 +61,8 @@
 #include <uORB/topics/parameter_update.h>
 #include <drivers/drv_hrt.h>
 
+#include <lib/mathlib/mathlib.h>
+
 #include <systemlib/systemlib.h>
 #include <systemlib/perf_counter.h>
 #include <systemlib/err.h>
@@ -433,10 +435,9 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 					/* send out */
 					att.timestamp = raw.timestamp;
 
-					// XXX Apply the same transformation to the rotation matrix
-					att.roll = euler[0] - ekf_params.roll_off;
-					att.pitch = euler[1] - ekf_params.pitch_off;
-					att.yaw = euler[2] - ekf_params.yaw_off;
+					att.roll = euler[0];
+					att.pitch = euler[1];
+					att.yaw = euler[2] + ekf_params.mag_decl;
 
 					att.rollspeed = x_aposteriori[0];
 					att.pitchspeed = x_aposteriori[1];
@@ -445,12 +446,21 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 					att.pitchacc = x_aposteriori[4];
 					att.yawacc = x_aposteriori[5];
 
-					//att.yawspeed =z_k[2] ;
 					/* copy offsets */
 					memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets));
 
+					/* magnetic declination */
+					math::EulerAngles eulerMagDecl(0.0f, 0.0f, ekf_params.mag_decl);
+					math::Dcm R(eulerMagDecl);
+					math::Dcm R_body(&Rot_matrix[0]);
+					R = R * R_body;
+
 					/* copy rotation matrix */
-					memcpy(&att.R, Rot_matrix, sizeof(Rot_matrix));
+					for (int i = 0; i < 3; i++) {
+						for (int j = 0; j < 3; j++) {
+							att.R[i][j] = R(i, j);
+						}
+					}
 					att.R_valid = true;
 
 					if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) {
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
index 52dac652b..a6a40b4a1 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c
@@ -63,6 +63,9 @@ PARAM_DEFINE_FLOAT(ATT_ROLL_OFF3, 0.0f);
 PARAM_DEFINE_FLOAT(ATT_PITCH_OFF3, 0.0f);
 PARAM_DEFINE_FLOAT(ATT_YAW_OFF3, 0.0f);
 
+/* magnetic declination, in degrees */
+PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f);
+
 int parameters_init(struct attitude_estimator_ekf_param_handles *h)
 {
 	/* PID parameters */
@@ -81,6 +84,8 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h)
 	h->pitch_off =	param_find("ATT_PITCH_OFF3");
 	h->yaw_off   =	param_find("ATT_YAW_OFF3");
 
+	h->mag_decl   =	param_find("ATT_MAG_DECL");
+
 	return OK;
 }
 
@@ -101,5 +106,7 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru
 	param_get(h->pitch_off, &(p->pitch_off));
 	param_get(h->yaw_off, &(p->yaw_off));
 
+	param_get(h->mag_decl, &(p->mag_decl));
+
 	return OK;
 }
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h
index 09817d58e..b4b3ca50d 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.h
@@ -47,12 +47,14 @@ struct attitude_estimator_ekf_params {
 	float roll_off;
 	float pitch_off;
 	float yaw_off;
+	float mag_decl;
 };
 
 struct attitude_estimator_ekf_param_handles {
 	param_t r0, r1, r2, r3;
 	param_t q0, q1, q2, q3, q4;
 	param_t roll_off, pitch_off, yaw_off;
+	param_t mag_decl;
 };
 
 /**