diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
index 39995aad2f..e609a89624 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
@@ -253,12 +253,12 @@ void guidance_h_run(bool_t  in_flight) {
         guidance_h_nav_enter();
 
       if (horizontal_mode == HORIZONTAL_MODE_ATTITUDE) {
-        struct Int32Eulers sp_euler_i;
-        sp_euler_i.phi = nav_roll;
-        sp_euler_i.theta = nav_pitch;
+        struct Int32Eulers sp_cmd_i;
+        sp_cmd_i.phi = nav_roll;
+        sp_cmd_i.theta = nav_pitch;
         /* FIXME: heading can't be set via attitude block yet, use current heading for now */
-        sp_euler_i.psi = stateGetNedToBodyEulers_i()->psi;
-        stabilization_attitude_set_from_eulers_i(&sp_euler_i);
+        sp_cmd_i.psi = stateGetNedToBodyEulers_i()->psi;
+        stabilization_attitude_set_cmd_i(&sp_cmd_i);
       }
       else {
         INT32_VECT2_NED_OF_ENU(guidance_h_pos_sp, navigation_carrot);
@@ -362,8 +362,8 @@ static void guidance_h_traj_run(bool_t in_flight) {
   guidance_h_command_body.phi += guidance_h_rc_sp.phi;
   guidance_h_command_body.theta += guidance_h_rc_sp.theta;
 
-  /* Set attitude setpoint from pseudo-eulers */
-  stabilization_attitude_set_from_eulers_i(&guidance_h_command_body);
+  /* Set attitude setpoint from pseudo-euler commands */
+  stabilization_attitude_set_cmd_i(&guidance_h_command_body);
 }
 
 static void guidance_h_hover_enter(void) {
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h
index 7a0572f69e..94b47466e4 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude.h
@@ -34,7 +34,7 @@ extern void stabilization_attitude_init(void);
 extern void stabilization_attitude_read_rc(bool_t in_flight);
 extern void stabilization_attitude_enter(void);
 extern void stabilization_attitude_set_failsafe_setpoint(void);
-extern void stabilization_attitude_set_from_eulers_i(struct Int32Eulers *sp_euler);
+extern void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd);
 extern void stabilization_attitude_run(bool_t  in_flight);
 
 
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c
index b2f9f24178..3a5df19417 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c
@@ -86,8 +86,8 @@ void stabilization_attitude_set_failsafe_setpoint(void) {
   stab_att_sp_euler.psi = stateGetNedToBodyEulers_f()->psi;
 }
 
-void stabilization_attitude_set_from_eulers_i(struct Int32Eulers *sp_euler) {
-  EULERS_FLOAT_OF_BFP(stab_att_sp_euler, *sp_euler);
+void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) {
+  EULERS_FLOAT_OF_BFP(stab_att_sp_euler, *sp_cmd);
 }
 
 
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c
index f435e97a1f..a86f9c4c80 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c
@@ -102,8 +102,8 @@ void stabilization_attitude_set_failsafe_setpoint(void) {
   stab_att_sp_euler.psi = stateGetNedToBodyEulers_i()->psi;
 }
 
-void stabilization_attitude_set_from_eulers_i(struct Int32Eulers *sp_euler) {
-  memcpy(&stab_att_sp_euler, sp_euler, sizeof(struct Int32Eulers));
+void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) {
+  memcpy(&stab_att_sp_euler, sp_cmd, sizeof(struct Int32Eulers));
 }
 
 
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.c
index 69f7004aca..5ddcb0e7c3 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_passthrough.c
@@ -78,7 +78,7 @@ void stabilization_attitude_set_failsafe_setpoint(void) {
   stab_att_sp_euler.psi = stateGetNedToBodyEulers_i()->psi;
 }
 
-void stabilization_attitude_set_from_eulers_i(struct Int32Eulers *sp_euler) {
-  memcpy(&stab_att_sp_euler, sp_euler, sizeof(struct Int32Eulers));
+void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) {
+  memcpy(&stab_att_sp_euler, sp_cmd, sizeof(struct Int32Eulers));
 }
 
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c
index e546692a87..b99400c492 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_float.c
@@ -142,9 +142,25 @@ void stabilization_attitude_set_failsafe_setpoint(void) {
   stab_att_sp_quat.qz = sinf(heading2);
 }
 
-void stabilization_attitude_set_from_eulers_i(struct Int32Eulers *sp_euler) {
-  EULERS_FLOAT_OF_BFP(stab_att_sp_euler, *sp_euler);
-  FLOAT_QUAT_OF_EULERS(stab_att_sp_quat, stab_att_sp_euler);
+void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) {
+  EULERS_FLOAT_OF_BFP(stab_att_sp_euler, *sp_cmd);
+
+  /* orientation vector describing simultaneous rotation of roll/pitch */
+  struct FloatVect3 ov;
+  ov.x = stab_att_sp_euler.phi;
+  ov.y = stab_att_sp_euler.theta;
+  ov.z = 0.0;
+  /* quaternion from that orientation vector */
+  struct FloatQuat q_rp;
+  FLOAT_QUAT_OF_ORIENTATION_VECT(q_rp, ov);
+
+  /* quaternion with only heading setpoint */
+  const float yaw2 = stab_att_sp_euler.psi / 2.0;
+  struct FloatQuat q_yaw;
+  QUAT_ASSIGN(q_yaw, cosf(yaw2), 0.0, 0.0, sinf(yaw2));
+
+  /* final setpoint: apply roll/pitch, then yaw around resulting body z-axis */
+  FLOAT_QUAT_COMP(stab_att_sp_quat, q_yaw, q_rp);
   FLOAT_QUAT_WRAP_SHORTEST(stab_att_sp_quat);
 }
 
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 1bff46cd25..1c889af5d0 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c
@@ -92,11 +92,33 @@ void stabilization_attitude_set_failsafe_setpoint(void) {
   PPRZ_ITRIG_SIN(stab_att_sp_quat.qz, heading2);
 }
 
-void stabilization_attitude_set_from_eulers_i(struct Int32Eulers *sp_euler) {
+void stabilization_attitude_set_cmd_i(struct Int32Eulers *sp_cmd) {
   // copy euler setpoint for debugging
-  memcpy(&stab_att_sp_euler, sp_euler, sizeof(struct Int32Eulers));
-  INT32_QUAT_OF_EULERS(stab_att_sp_quat, *sp_euler);
-  INT32_QUAT_WRAP_SHORTEST(stab_att_sp_quat);
+  memcpy(&stab_att_sp_euler, sp_cmd, sizeof(struct Int32Eulers));
+
+  /// @todo calc sp_quat in fixed-point
+
+  /* orientation vector describing simultaneous rotation of roll/pitch */
+  struct FloatVect3 ov;
+  ov.x = ANGLE_FLOAT_OF_BFP(sp_cmd->phi);
+  ov.y = ANGLE_FLOAT_OF_BFP(sp_cmd->theta);
+  ov.z = 0.0;
+  /* quaternion from that orientation vector */
+  struct FloatQuat q_rp;
+  FLOAT_QUAT_OF_ORIENTATION_VECT(q_rp, ov);
+
+  /* quaternion with only heading setpoint */
+  const float yaw2 = ANGLE_FLOAT_OF_BFP(sp_cmd->psi) / 2.0;
+  struct FloatQuat q_yaw;
+  QUAT_ASSIGN(q_yaw, cosf(yaw2), 0.0, 0.0, sinf(yaw2));
+
+  /* final setpoint: apply roll/pitch, then yaw around resulting body z-axis */
+  struct FloatQuat q_sp;
+  FLOAT_QUAT_COMP(q_sp, q_yaw, q_rp);
+  FLOAT_QUAT_WRAP_SHORTEST(q_sp);
+
+  /* convert to fixed point */
+  QUAT_BFP_OF_REAL(stab_att_sp_quat, q_sp);
 }
 
 #define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b))