diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c
index 8c6d8cdc5a..02c565396c 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c
@@ -118,6 +118,8 @@ void stabilization_attitude_ref_enter()
  * Reference
  */
 #define DT_UPDATE (1./PERIODIC_FREQUENCY)
+// CAUTION! Periodic frequency is assumed to be 512 Hz
+// which is equal to >> 9
 #define F_UPDATE_RES 9
 
 void stabilization_attitude_ref_update() {
@@ -125,9 +127,11 @@ void stabilization_attitude_ref_update() {
   /* integrate reference attitude            */
   struct Int32Quat qdot;
   INT32_QUAT_DERIVATIVE(qdot, stab_att_ref_rate, stab_att_ref_quat);
-  //QUAT_SMUL(qdot, qdot, RATE_BFP_OF_REAL(DT_UPDATE));
-  QUAT_SMUL(qdot, qdot, 4);
-  QUAT_SMUL(qdot, qdot, DT_UPDATE);
+  //QUAT_SMUL(qdot, qdot, DT_UPDATE);
+  qdot.qi = qdot.qi >> F_UPDATE_RES;
+  qdot.qx = qdot.qx >> F_UPDATE_RES;
+  qdot.qy = qdot.qy >> F_UPDATE_RES;
+  qdot.qz = qdot.qz >> F_UPDATE_RES;
   QUAT_ADD(stab_att_ref_quat, qdot);
   INT32_QUAT_NORMALIZE(stab_att_ref_quat);
 
diff --git a/sw/airborne/math/pprz_algebra_int.h b/sw/airborne/math/pprz_algebra_int.h
index 59ac43d670..d482ad5565 100644
--- a/sw/airborne/math/pprz_algebra_int.h
+++ b/sw/airborne/math/pprz_algebra_int.h
@@ -605,11 +605,12 @@ struct Int64Vect3 {
 
 
 /* _qd = -0.5*omega(_r) * _q  */
+// mult with 0.5 is done by shifting one more bit to the right
 #define INT32_QUAT_DERIVATIVE(_qd, _r, _q) {				\
-    (_qd).qi = (-1*( (_r).p*(_q).qx/2 + (_r).q*(_q).qy + (_r).r*(_q).qz))>>INT32_RATE_FRAC; \
-    (_qd).qx = (-1*(-(_r).p*(_q).qi/2 - (_r).r*(_q).qy + (_r).q*(_q).qz))>>INT32_RATE_FRAC; \
-    (_qd).qy = (-1*(-(_r).q*(_q).qi/2 + (_r).r*(_q).qx - (_r).p*(_q).qz))>>INT32_RATE_FRAC; \
-    (_qd).qz = (-1*(-(_r).r*(_q).qi/2 - (_r).q*(_q).qx + (_r).p*(_q).qy))>>INT32_RATE_FRAC; \
+    (_qd).qi = (-( (_r).p*(_q).qx + (_r).q*(_q).qy + (_r).r*(_q).qz))>>(INT32_RATE_FRAC+1); \
+    (_qd).qx = (-(-(_r).p*(_q).qi - (_r).r*(_q).qy + (_r).q*(_q).qz))>>(INT32_RATE_FRAC+1); \
+    (_qd).qy = (-(-(_r).q*(_q).qi + (_r).r*(_q).qx - (_r).p*(_q).qz))>>(INT32_RATE_FRAC+1); \
+    (_qd).qz = (-(-(_r).r*(_q).qi - (_r).q*(_q).qx + (_r).p*(_q).qy))>>(INT32_RATE_FRAC+1); \
   }
 
 /** in place quaternion first order integration with constant rotational velocity. */