diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
index d422926a77..4765686d09 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
@@ -87,6 +87,11 @@ bool act_is_servo[INDI_NUM_ACT] = {0};
 
 float act_dyn[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_DYN;
 
+/** Maximum rate you can request in RC rate mode (rad/s)*/
+#ifndef STABILIZATION_INDI_MAX_RATE
+#define STABILIZATION_INDI_MAX_RATE 6.0
+#endif
+
 // variables needed for control
 float actuator_state_filt_vect[INDI_NUM_ACT];
 struct FloatRates angular_accel_ref = {0., 0., 0.};
@@ -303,13 +308,31 @@ void stabilization_indi_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)
  */
 static void stabilization_indi_calc_cmd(struct Int32Quat *att_err, bool rate_control, bool in_flight)
 {
+
+  if(rate_control) {//Check if we are running the rate controller
+    rate_ref.p = (float)radio_control.values[RADIO_ROLL]  / MAX_PPRZ * STABILIZATION_INDI_MAX_RATE;
+    rate_ref.q = (float)radio_control.values[RADIO_PITCH] / MAX_PPRZ * STABILIZATION_INDI_MAX_RATE;
+    rate_ref.r = (float)radio_control.values[RADIO_YAW]   / MAX_PPRZ * STABILIZATION_INDI_MAX_RATE;
+  } else {
+    //calculate the virtual control (reference acceleration) based on a PD controller
+    struct FloatRates rate_ref;
+    rate_ref.p = reference_acceleration.err_p * QUAT1_FLOAT_OF_BFP(att_err->qx)
+      /reference_acceleration.rate_p;
+    rate_ref.q = reference_acceleration.err_q * QUAT1_FLOAT_OF_BFP(att_err->qy)
+      /reference_acceleration.rate_q;
+    rate_ref.r = reference_acceleration.err_r * QUAT1_FLOAT_OF_BFP(att_err->qz)
+      /reference_acceleration.rate_r;
+
+    // Possibly we can use some bounding here
+    /*BoundAbs(rate_ref.r, 5.0);*/
+  }
+
+  struct FloatRates *body_rates = stateGetBodyRates_f();
+
   //calculate the virtual control (reference acceleration) based on a PD controller
-  angular_accel_ref.p = reference_acceleration.err_p * QUAT1_FLOAT_OF_BFP(att_err->qx)
-                        - reference_acceleration.rate_p * stateGetBodyRates_f()->p;
-  angular_accel_ref.q = reference_acceleration.err_q * QUAT1_FLOAT_OF_BFP(att_err->qy)
-                        - reference_acceleration.rate_q * stateGetBodyRates_f()->q;
-  angular_accel_ref.r = reference_acceleration.err_r * QUAT1_FLOAT_OF_BFP(att_err->qz)
-                        - reference_acceleration.rate_r * stateGetBodyRates_f()->r;
+  angular_accel_ref.p = (rate_ref.p - body_rates->p) * reference_acceleration.rate_p;
+  angular_accel_ref.q = (rate_ref.q - body_rates->q) * reference_acceleration.rate_q;
+  angular_accel_ref.r = (rate_ref.r - body_rates->r) * reference_acceleration.rate_r;
 
   g2_times_du = 0.0;
   int8_t i;
@@ -652,7 +675,7 @@ static void rpm_cb(uint8_t __attribute__((unused)) sender_id, uint16_t UNUSED *r
 /**
  * ABI callback that obtains the thrust increment from guidance INDI
  */
-static void thrust_cb(uint8_t sender_id, float thrust_increment)
+static void thrust_cb(uint8_t UNUSED sender_id, float thrust_increment)
 {
   indi_thrust_increment = thrust_increment;
   indi_thrust_increment_set = true;