[imu] imu_SetBodyToImuCurrent: only adjust by current roll/pitch

so you can use it if initial body_to_imu is not zero and also use it repeatedly
paparazzi · Aug 2, 2014 · fca2cd7 · fca2cd7
1 parent 519c4fd
commit fca2cd7
Showing 1 changed file with 3 additions and 3 deletions.
diff --git a/sw/airborne/subsystems/imu.c b/sw/airborne/subsystems/imu.c
@@ -187,9 +187,9 @@ void imu_SetBodyToImuCurrent(float set) {
   if (imu.b2i_set_current) {
     struct FloatEulers imu_to_body_eulers;
     memcpy(&imu_to_body_eulers, orientationGetEulers_f(&imu.body_to_imu), sizeof(struct FloatEulers));
-    // set to current roll and pitch
-    imu_to_body_eulers.phi = stateGetNedToBodyEulers_f()->phi;
-    imu_to_body_eulers.theta = stateGetNedToBodyEulers_f()->theta;
+    // adjust imu_to_body roll and pitch by current NedToBody roll and pitch
+    imu_to_body_eulers.phi += stateGetNedToBodyEulers_f()->phi;
+    imu_to_body_eulers.theta += stateGetNedToBodyEulers_f()->theta;
     orientationSetEulers_f(&imu.body_to_imu, &imu_to_body_eulers);
   }
 }