Added offset parameters for roll, pitch and yaw

3 files changed, 19 insertions, 3 deletions

diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
index 6533390bc..e373c531b 100644
--- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
+++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c
@@ -413,9 +413,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[])
 
 					/* send out */
 					att.timestamp = raw.timestamp;
-					att.roll = euler[0];
-					att.pitch = euler[1];
-					att.yaw = euler[2];
+					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.rollspeed = x_aposteriori[0];
 					att.pitchspeed = x_aposteriori[1];
diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c
index 6879f9dc8..75a2479ed 100644
--- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c
+++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.c
@@ -72,6 +72,10 @@ PARAM_DEFINE_FLOAT(EKF_ATT_R5, 1e2f);
 PARAM_DEFINE_FLOAT(EKF_ATT_R6, 1e-1f);
 PARAM_DEFINE_FLOAT(EKF_ATT_R7, 1e-1f);
 PARAM_DEFINE_FLOAT(EKF_ATT_R8, 1e-1f);
+/* offsets in roll, pitch and yaw of sensor plane and body */
+PARAM_DEFINE_FLOAT(ATT_ROLL_OFFS, 0.0f);
+PARAM_DEFINE_FLOAT(ATT_PITCH_OFFS, 0.0f);
+PARAM_DEFINE_FLOAT(ATT_YAW_OFFS, 0.0f);
 
 int parameters_init(struct attitude_estimator_ekf_param_handles *h)
 {
@@ -99,6 +103,10 @@ int parameters_init(struct attitude_estimator_ekf_param_handles *h)
 	h->r7 	=	param_find("EKF_ATT_R7");
 	h->r8 	=	param_find("EKF_ATT_R8");
 
+	h->roll_off  =	param_find("ATT_ROLL_OFFS");
+	h->pitch_off =	param_find("ATT_PITCH_OFFS");
+	h->yaw_off   =	param_find("ATT_YAW_OFFS");
+
 	return OK;
 }
 
@@ -127,5 +135,9 @@ int parameters_update(const struct attitude_estimator_ekf_param_handles *h, stru
 	param_get(h->r7, &(p->r[7]));
 	param_get(h->r8, &(p->r[8]));
 
+	param_get(h->roll_off, &(p->roll_off));
+	param_get(h->pitch_off, &(p->pitch_off));
+	param_get(h->yaw_off, &(p->yaw_off));
+
 	return OK;
 }
diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h
index 6a63f9767..ad775002b 100644
--- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h
+++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_params.h
@@ -44,11 +44,15 @@
 struct attitude_estimator_ekf_params {
 	float r[9];
 	float q[12];
+	float roll_off;
+	float pitch_off;
+	float yaw_off;
 };
 
 struct attitude_estimator_ekf_param_handles {
 	param_t r0, r1, r2, r3, r4, r5, r6, r7, r8;
 	param_t q0, q1, q2, q3, q4, q5, q6, q7, q8, q9, q10, q11;
+	param_t roll_off, pitch_off, yaw_off;
 };
 
 /**