diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c
index 8670c7e4b6..1aec849712 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c
@@ -184,10 +184,41 @@ void stabilization_attitude_read_rc(bool_t in_flight) {
 #if USE_SETPOINTS_WITH_TRANSITIONS
   stabilization_attitude_read_rc_absolute(in_flight);
 #else
-  stabilization_attitude_read_rc_setpoint_eulers(&stab_att_sp_euler, in_flight);
-  /* update quaternion setpoint from euler setpoint */
-  INT32_QUAT_OF_EULERS(stab_att_sp_quat, stab_att_sp_euler);
-  INT32_QUAT_WRAP_SHORTEST(stab_att_sp_quat);
-#endif
 
+  //FIXME: remove me, do in quaternion directly
+  stabilization_attitude_read_rc_setpoint_eulers(&stab_att_sp_euler, in_flight);
+
+  struct FloatQuat q_rp_cmd;
+  stabilization_attitude_read_rc_roll_pitch_quat(&q_rp_cmd);
+
+  /* get current heading */
+  const struct FloatVect3 zaxis = {0., 0., 1.};
+  struct FloatQuat q_yaw;
+  FLOAT_QUAT_OF_AXIS_ANGLE(q_yaw, zaxis, ANGLE_FLOAT_OF_BFP(ahrs.ltp_to_body_euler.psi));
+
+  /* apply roll and pitch commands with respect to current heading */
+  struct FloatQuat q_sp;
+  FLOAT_QUAT_COMP(q_sp, q_yaw, q_rp_cmd);
+  FLOAT_QUAT_NORMALIZE(q_sp);
+
+  if (in_flight)
+  {
+    /* get current heading setpoint */
+    struct FloatQuat q_yaw_sp;
+    FLOAT_QUAT_OF_AXIS_ANGLE(q_yaw_sp, zaxis, ANGLE_FLOAT_OF_BFP(stab_att_sp_euler.psi));
+
+    /* rotation between current yaw and yaw setpoint */
+    struct FloatQuat q_yaw_diff;
+    FLOAT_QUAT_COMP_INV(q_yaw_diff, q_yaw_sp, q_yaw);
+
+    /* temporary copy with roll/pitch command and current heading */
+    struct FloatQuat q_rp_sp;
+    QUAT_COPY(q_rp_sp, q_sp);
+
+    /* compute final setpoint with yaw */
+    FLOAT_QUAT_COMP_NORM_SHORTEST(q_sp, q_rp_sp, q_yaw_diff);
+  }
+
+  QUAT_BFP_OF_REAL(stab_att_sp_quat, q_sp);
+#endif
 }
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h
index f68db9d153..03ce96df9b 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h
@@ -85,5 +85,16 @@ static inline void stabilization_attitude_read_rc_setpoint_eulers(struct Int32Eu
 
 }
 
+static inline void stabilization_attitude_read_rc_roll_pitch_quat(struct FloatQuat* q) {
+  q->qx = radio_control.values[RADIO_ROLL] * STABILIZATION_ATTITUDE_SP_MAX_PHI / MAX_PPRZ / 2;
+  q->qy = radio_control.values[RADIO_PITCH] * STABILIZATION_ATTITUDE_SP_MAX_THETA / MAX_PPRZ / 2;
+  q->qz = 0.0;
+
+  /* normalize */
+  float norm = sqrtf(1.0 + SQUARE(q->qx)+ SQUARE(q->qy));
+  q->qi = 1.0 / norm;
+  q->qx /= norm;
+  q->qy /= norm;
+}
 
 #endif /* STABILISATION_ATTITUDE_RC_SETPOINT_H */