diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
index 14ab2564d1..349007a63a 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_h.c
@@ -515,11 +515,11 @@ static void guidance_h_traj_run(bool in_flight)
     ((guidance_h.gains.p * guidance_h_pos_err.y) >> (INT32_POS_FRAC - GH_GAIN_SCALE)) +
     ((guidance_h.gains.d * (guidance_h_speed_err.y >> 2)) >> (INT32_SPEED_FRAC - GH_GAIN_SCALE - 2));
   guidance_h_cmd_earth.x = pd_x +
-    ((guidance_h.gains.v * guidance_h.ref.speed.x) >> 17) + /* speed feedforward gain */
-    ((guidance_h.gains.a * guidance_h.ref.accel.x) >> 8);   /* acceleration feedforward gain */
+    ((guidance_h.gains.v * guidance_h.ref.speed.x) >> (INT32_SPEED_FRAC - GH_GAIN_SCALE)) + /* speed feedforward gain */
+    ((guidance_h.gains.a * guidance_h.ref.accel.x) >> (INT32_ACCEL_FRAC - GH_GAIN_SCALE));   /* acceleration feedforward gain */
   guidance_h_cmd_earth.y = pd_y +
-    ((guidance_h.gains.v * guidance_h.ref.speed.y) >> 17) + /* speed feedforward gain */
-    ((guidance_h.gains.a * guidance_h.ref.accel.y) >> 8);   /* acceleration feedforward gain */
+    ((guidance_h.gains.v * guidance_h.ref.speed.y) >> (INT32_SPEED_FRAC - GH_GAIN_SCALE)) + /* speed feedforward gain */
+    ((guidance_h.gains.a * guidance_h.ref.accel.y) >> (INT32_ACCEL_FRAC - GH_GAIN_SCALE));   /* acceleration feedforward gain */
 
   /* trim max bank angle from PD */
   VECT2_STRIM(guidance_h_cmd_earth, -traj_max_bank, traj_max_bank);
@@ -533,10 +533,10 @@ static void guidance_h_traj_run(bool in_flight)
     guidance_h_trim_att_integrator.x += (guidance_h.gains.i * pd_x);
     guidance_h_trim_att_integrator.y += (guidance_h.gains.i * pd_y);
     /* saturate it  */
-    VECT2_STRIM(guidance_h_trim_att_integrator, -(traj_max_bank << 16), (traj_max_bank << 16));
+    VECT2_STRIM(guidance_h_trim_att_integrator, -(traj_max_bank << (INT32_ANGLE_FRAC + GH_GAIN_SCALE*2)), (traj_max_bank << (INT32_ANGLE_FRAC + GH_GAIN_SCALE*2)));
     /* add it to the command */
-    guidance_h_cmd_earth.x += (guidance_h_trim_att_integrator.x >> 16);
-    guidance_h_cmd_earth.y += (guidance_h_trim_att_integrator.y >> 16);
+    guidance_h_cmd_earth.x += (guidance_h_trim_att_integrator.x >> (INT32_ANGLE_FRAC + GH_GAIN_SCALE*2));
+    guidance_h_cmd_earth.y += (guidance_h_trim_att_integrator.y >> (INT32_ANGLE_FRAC + GH_GAIN_SCALE*2));
   } else {
     INT_VECT2_ZERO(guidance_h_trim_att_integrator);
   }
diff --git a/sw/airborne/firmwares/rotorcraft/navigation.c b/sw/airborne/firmwares/rotorcraft/navigation.c
index 6c929147f0..d3476c649e 100644
--- a/sw/airborne/firmwares/rotorcraft/navigation.c
+++ b/sw/airborne/firmwares/rotorcraft/navigation.c
@@ -504,7 +504,7 @@ void compute_dist2_to_home(void)
   too_far_from_home = dist2_to_home > max_dist2_from_home;
 }
 
-/** Set nav_heading in degrees. */
+/** Set nav_heading in radians. */
 bool nav_set_heading_rad(float rad)
 {
   nav_heading = ANGLE_BFP_OF_REAL(rad);
diff --git a/sw/airborne/subsystems/ins.h b/sw/airborne/subsystems/ins.h
index 45d2f3791e..ffe0689d14 100644
--- a/sw/airborne/subsystems/ins.h
+++ b/sw/airborne/subsystems/ins.h
@@ -59,7 +59,7 @@ extern void ins_reset_local_origin(void);
 extern void ins_reset_altitude_ref(void);
 
 /** INS utm zone reset.
- *  Reset UTM zone according te the actual position.
+ *  Reset UTM zone according the the actual position.
  *  Only used with fixedwing firmware.
  *  Can be overwritte by specifc INS implementation.
  *  @param utm initial utm zone, returns the corrected utm position