diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
index 75c60529a6..8e72a4899f 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
@@ -288,15 +288,15 @@ static inline void guidance_h_traj_run(bool_t in_flight) {
 
   /* run PID */
   guidance_h_command_earth.x =
-    guidance_h_pgain * (guidance_h_pos_err.x >> (INT32_POS_FRAC - GH_GAIN_SCALE)) +
-    guidance_h_dgain * (guidance_h_speed_err.x >> (INT32_SPEED_FRAC - GH_GAIN_SCALE)) +
-    guidance_h_igain * (guidance_h_pos_err_sum.x >> (12 + INT32_POS_FRAC - GH_GAIN_SCALE)) +
-    guidance_h_again * (guidance_h_accel_ref.x >> 8); /* feedforward gain */
+    ((guidance_h_pgain * guidance_h_pos_err.x) >> (INT32_POS_FRAC - GH_GAIN_SCALE)) +
+    ((guidance_h_dgain * (guidance_h_speed_err.x >> 2)) >> (INT32_SPEED_FRAC - GH_GAIN_SCALE - 2)) +
+    ((guidance_h_igain * (guidance_h_pos_err_sum.x >> 12)) >> (INT32_POS_FRAC - GH_GAIN_SCALE)) +
+    ((guidance_h_again * guidance_h_accel_ref.x) >> 8); /* feedforward gain */
   guidance_h_command_earth.y =
-    guidance_h_pgain * (guidance_h_pos_err.y >> (INT32_POS_FRAC - GH_GAIN_SCALE)) +
-    guidance_h_dgain * (guidance_h_speed_err.y >> (INT32_SPEED_FRAC - GH_GAIN_SCALE)) +
-    guidance_h_igain * (guidance_h_pos_err_sum.y >> (12 + INT32_POS_FRAC - GH_GAIN_SCALE)) +
-    guidance_h_again * (guidance_h_accel_ref.y >> 8); /* feedforward gain */
+    ((guidance_h_pgain * guidance_h_pos_err.y) >> (INT32_POS_FRAC - GH_GAIN_SCALE)) +
+    ((guidance_h_dgain * (guidance_h_speed_err.y >> 2)) >> (INT32_SPEED_FRAC - GH_GAIN_SCALE - 2)) +
+    ((guidance_h_igain * (guidance_h_pos_err_sum.y >> 12)) >> (INT32_POS_FRAC - GH_GAIN_SCALE)) +
+    ((guidance_h_again * guidance_h_accel_ref.y) >> 8); /* feedforward gain */
 
   VECT2_STRIM(guidance_h_command_earth, -TRAJ_MAX_BANK, TRAJ_MAX_BANK);
 
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h
index aec9bcf361..7bcbdda2c4 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.h
@@ -49,9 +49,9 @@ extern uint8_t guidance_h_mode;
  */
 extern struct Int32Vect2 guidance_h_pos_sp;
 
-extern struct Int32Vect2 guidance_h_pos_ref;
-extern struct Int32Vect2 guidance_h_speed_ref;
-extern struct Int32Vect2 guidance_h_accel_ref;
+extern struct Int32Vect2 guidance_h_pos_ref;        ///< with #INT32_POS_FRAC
+extern struct Int32Vect2 guidance_h_speed_ref;      ///< with #INT32_SPEED_FRAC
+extern struct Int32Vect2 guidance_h_accel_ref;      ///< with #INT32_ACCEL_FRAC
 
 extern struct Int32Vect2 guidance_h_pos_err;
 extern struct Int32Vect2 guidance_h_speed_err;
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.h
index e3cfa1fa5c..868e2b4026 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.h
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h_ref.h
@@ -54,7 +54,8 @@ extern struct Int64Vect2 b2_gh_pos_ref;
 
 /* Saturations definition */
 #ifndef GUIDANCE_H_REF_MAX_ACCEL
-#define GUIDANCE_H_REF_MAX_ACCEL ( tanf(RadOfDeg(30.))*9.81 )
+/* tanf(RadOfDeg(30.))*9.81 = 5.66 */
+#define GUIDANCE_H_REF_MAX_ACCEL 5.66
 #endif
 #define B2_GH_MAX_ACCEL BFP_OF_REAL(GUIDANCE_H_REF_MAX_ACCEL, B2_GH_ACCEL_REF_FRAC)
 
diff --git a/sw/airborne/firmwares/rotorcraft/navigation.c b/sw/airborne/firmwares/rotorcraft/navigation.c
index 5166996b58..ba42fb0009 100644
--- a/sw/airborne/firmwares/rotorcraft/navigation.c
+++ b/sw/airborne/firmwares/rotorcraft/navigation.c
@@ -123,6 +123,7 @@ void nav_run(void) {
   VECT2_STRIM(path_to_waypoint, -(1<<15), (1<<15));
 
 #if !GUIDANCE_H_USE_REF
+  PRINT_CONFIG_MSG("NOT using horizontal guidance reference :-(")
   int32_t dist_to_waypoint;
   INT32_VECT2_NORM(dist_to_waypoint, path_to_waypoint);
 
@@ -136,6 +137,7 @@ void nav_run(void) {
     VECT2_SUM(navigation_carrot, path_to_carrot, *stateGetPositionEnu_i());
   }
 #else
+  PRINT_CONFIG_MSG("Using horizontal guidance reference :-)")
   // if H_REF is used, CARROT_DIST is not used
   VECT2_COPY(navigation_carrot, navigation_target);
 #endif