diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.c
index 5ddcb0e7c3..16337e5853 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.c
@@ -28,6 +28,7 @@
  * software onboard or just does not need it at all.
  */
 
+#include "state.h"
 #include "firmwares/rotorcraft/stabilization.h"
 #include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h"
 #include "paparazzi.h"
@@ -78,7 +79,17 @@ void stabilization_attitude_set_failsafe_setpoint(void) {
   stab_att_sp_euler.psi = stateGetNedToBodyEulers_i()->psi;
 }
 
-void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) {
-  memcpy(&stab_att_sp_euler, sp_cmd, sizeof(struct Int32Eulers));
+void stabilization_attitude_set_rpy_setpoint_i(struct Int32Eulers *rpy) {
+  memcpy(&stab_att_sp_euler, rpy, sizeof(struct Int32Eulers));
 }
 
+void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading) {
+  /* Rotate horizontal commands to body frame by psi */
+  int32_t psi = stateGetNedToBodyEulers_i()->psi;
+  int32_t s_psi, c_psi;
+  PPRZ_ITRIG_SIN(s_psi, psi);
+  PPRZ_ITRIG_COS(c_psi, psi);
+  stab_att_sp_euler.phi = (-s_psi * cmd->x + c_psi * cmd->y) >> INT32_TRIG_FRAC;
+  stab_att_sp_euler.theta = -(c_psi * cmd->x + s_psi * cmd->y) >> INT32_TRIG_FRAC;
+  stab_att_sp_euler.psi = heading;
+}
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.h
index 4c0209674f..01755a987e 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.h
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.h
@@ -25,4 +25,6 @@
 #include "math/pprz_algebra_int.h"
 #include "firmwares/rotorcraft/stabilization/stabilization_attitude_common_int.h"
 
+extern struct Int32Eulers stab_att_sp_euler;
+
 #endif /* STABILIZATION_ATTITUDE_PASSTHROUGH_H */
diff --git a/sw/airborne/subsystems/ins/ins_ardrone2.h b/sw/airborne/subsystems/ins/ins_ardrone2.h
index 617c2845f3..0558ab4e10 100644
--- a/sw/airborne/subsystems/ins/ins_ardrone2.h
+++ b/sw/airborne/subsystems/ins/ins_ardrone2.h
@@ -40,8 +40,8 @@ struct InsArdrone2 {
 
   /* output LTP NED */
   struct NedCoor_i ltp_pos;
-  struct NedCoor_i ltp_speed;
-  struct NedCoor_i ltp_accel;
+  struct NedCoor_f ltp_speed;
+  struct NedCoor_f ltp_accel;
 };
 
 extern struct InsArdrone2 ins_impl;