Added pitch, roll, and yaw offsets to compensate for imperfect fmu placement. These were removed in 61a3177979838649af2a6e8e50bea7d15e2765f4

1 files changed, 5 insertions, 4 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 66a949af1..95685b407 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
@@ -512,10 +512,11 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 
 					/* send out */
 					att.timestamp = raw.timestamp;
-
-					att.roll = euler[0];
-					att.pitch = euler[1];
-					att.yaw = euler[2] + ekf_params.mag_decl;
+					
+					/* Compensate for fmu placement offsets and magnetic declination */
+					att.roll = euler[0] - ekf_params.roll_off;
+					att.pitch = euler[1] - ekf_params.pitch_off;
+					att.yaw = euler[2] + ekf_params.mag_decl - ekf_params.yaw_off;
 
 					att.rollspeed = x_aposteriori[0];
 					att.pitchspeed = x_aposteriori[1];