[INDI Rate Control] - Done (Needs to be checked/verified/validated)

sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c

@@ -35,6 +35,9 @@
 #include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h"
 #include "firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.h"
 
+// Added to access variable "stabilization_rate_sp" for direct rate control
+#include "firmwares/rotorcraft/stabilization/stabilization_rate_indi.h"
+
 #include "math/pprz_algebra_float.h"
 #include "state.h"
 #include "generated/airframe.h"
@@ -350,9 +353,9 @@ static void stabilization_indi_calc_cmd(struct Int32Quat *att_err, bool rate_con
 
   struct FloatRates rate_ref;
   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.p = stabilization_rate_sp.p;
-    rate_ref.q = (float)radio_control.values[RADIO_PITCH] / MAX_PPRZ * STABILIZATION_INDI_MAX_RATE;
+    rate_ref.q = stabilization_rate_sp.q;
-    rate_ref.r = (float)radio_control.values[RADIO_YAW]   / MAX_PPRZ * STABILIZATION_INDI_MAX_RATE;
+    rate_ref.r = stabilization_rate_sp.r;
   } else {
     //calculate the virtual control (reference acceleration) based on a PD controller
     rate_ref.p = reference_acceleration.err_p * QUAT1_FLOAT_OF_BFP(att_err->qx)

sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.c

@@ -35,6 +35,9 @@
 #include "firmwares/rotorcraft/stabilization/stabilization_attitude_rc_setpoint.h"
 #include "firmwares/rotorcraft/stabilization/stabilization_attitude_quat_transformations.h"
 
+// Added to access variable "stabilization_rate_sp" for direct rate control
+#include "firmwares/rotorcraft/stabilization/stabilization_rate_indi.h"
+
 #include "state.h"
 #include "generated/airframe.h"
 #include "paparazzi.h"
@@ -327,9 +330,9 @@ static inline void stabilization_indi_calc_cmd(int32_t indi_commands[], struct I
 
   /* Check if we are running the rate controller and overwrite */
   if (rate_control) {
-    indi.angular_accel_ref.p =  indi.reference_acceleration.rate_p * ((float)radio_control.values[RADIO_ROLL]  / MAX_PPRZ * indi.max_rate - body_rates->p);
+    indi.angular_accel_ref.p =  indi.reference_acceleration.rate_p * (stabilization_rate_sp.p - body_rates->p);
-    indi.angular_accel_ref.q =  indi.reference_acceleration.rate_q * ((float)radio_control.values[RADIO_PITCH] / MAX_PPRZ * indi.max_rate - body_rates->q);
+    indi.angular_accel_ref.q =  indi.reference_acceleration.rate_q * (stabilization_rate_sp.q - body_rates->q);
-    indi.angular_accel_ref.r =  indi.reference_acceleration.rate_r * ((float)radio_control.values[RADIO_YAW]   / MAX_PPRZ * indi.max_rate - body_rates->r);
+    indi.angular_accel_ref.r =  indi.reference_acceleration.rate_r * (stabilization_rate_sp.r - body_rates->r);
   }
 
   //Increment in angular acceleration requires increment in control input

sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate_indi.c

@@ -23,7 +23,7 @@
  *  Rate stabilization for rotorcrafts based on INDI by Ewoud Smeur.
  */
 #include "firmwares/rotorcraft/stabilization.h"
-#include "firmwares/rotorcraft/stabilization/stabilization_rate.h"
+#include "firmwares/rotorcraft/stabilization/stabilization_rate_indi.h"
 
 #ifdef STABILIZATION_ATTITUDE_INDI_SIMPLE
 #include "firmwares/rotorcraft/stabilization/stabilization_indi_simple.h"
@@ -31,21 +31,93 @@
 #include "firmwares/rotorcraft/stabilization/stabilization_indi.h"
 #endif
 
+struct FloatRates stabilization_rate_sp;
+
+#ifndef STABILIZATION_RATE_DEADBAND_P
+#define STABILIZATION_RATE_DEADBAND_P 0
+#endif
+#ifndef STABILIZATION_RATE_DEADBAND_Q
+#define STABILIZATION_RATE_DEADBAND_Q 0
+#endif
+#ifndef STABILIZATION_RATE_DEADBAND_R
+#define STABILIZATION_RATE_DEADBAND_R 200
+#endif
+
+#define ROLL_RATE_DEADBAND_EXCEEDED()                                         \
+  (radio_control.values[RADIO_ROLL] >  STABILIZATION_RATE_DEADBAND_P || \
+   radio_control.values[RADIO_ROLL] < -STABILIZATION_RATE_DEADBAND_P)
+
+#define PITCH_RATE_DEADBAND_EXCEEDED()                                         \
+  (radio_control.values[RADIO_PITCH] >  STABILIZATION_RATE_DEADBAND_Q || \
+   radio_control.values[RADIO_PITCH] < -STABILIZATION_RATE_DEADBAND_Q)
+
+#define YAW_RATE_DEADBAND_EXCEEDED()                                         \
+  (radio_control.values[RADIO_YAW] >  STABILIZATION_RATE_DEADBAND_R || \
+   radio_control.values[RADIO_YAW] < -STABILIZATION_RATE_DEADBAND_R)
+
+#if PERIODIC_TELEMETRY
+#include "subsystems/datalink/telemetry.h"
+
+static void send_rate(struct transport_tx *trans, struct link_device *dev)
+{
+  pprz_msg_send_RATE_LOOP(trans, dev, AC_ID,
+                          &stabilization_rate_sp.p,
+                          &stabilization_rate_sp.q,
+                          &stabilization_rate_sp.r,
+                          &stabilization_cmd[COMMAND_THRUST]);
+}
+#endif
+
 void stabilization_rate_init(void)
 {
   stabilization_indi_init();
-}
 
+#if PERIODIC_TELEMETRY
+  register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_RATE_LOOP, send_rate);
+#endif
+}
 
 void stabilization_rate_read_rc(void)
 {
-  //FIXME: make a new indi function
+  if (ROLL_RATE_DEADBAND_EXCEEDED()) {
+    stabilization_rate_sp.p = (float) radio_control.values[RADIO_ROLL] * STABILIZATION_INDI_MAX_RATE / MAX_PPRZ;
+  } else {
+    stabilization_rate_sp.p = 0;
+  }
+
+  if (PITCH_RATE_DEADBAND_EXCEEDED()) {
+    stabilization_rate_sp.q = (float) radio_control.values[RADIO_PITCH] * STABILIZATION_INDI_MAX_RATE / MAX_PPRZ;
+  } else {
+    stabilization_rate_sp.q = 0;
+  }
+
+  if (YAW_RATE_DEADBAND_EXCEEDED() && !THROTTLE_STICK_DOWN()) {
+    stabilization_rate_sp.r = (float) radio_control.values[RADIO_YAW] * STABILIZATION_INDI_MAX_RATE / MAX_PPRZ;
+  } else {
+    stabilization_rate_sp.r = 0;
+  }
 }
 
 //Read rc with roll and yaw sitcks switched if the default orientation is vertical but airplane sticks are desired
 void stabilization_rate_read_rc_switched_sticks(void)
 {
-  //FIXME: make a new indi function
+  if (ROLL_RATE_DEADBAND_EXCEEDED()) {
+    stabilization_rate_sp.r =  - (float) radio_control.values[RADIO_ROLL] * STABILIZATION_INDI_MAX_RATE / MAX_PPRZ;
+  } else {
+    stabilization_rate_sp.r = 0;
+  }
+
+  if (PITCH_RATE_DEADBAND_EXCEEDED()) {
+    stabilization_rate_sp.q = (float) radio_control.values[RADIO_PITCH] * STABILIZATION_INDI_MAX_RATE / MAX_PPRZ;
+  } else {
+    stabilization_rate_sp.q = 0;
+  }
+
+  if (YAW_RATE_DEADBAND_EXCEEDED() && !THROTTLE_STICK_DOWN()) {
+    stabilization_rate_sp.p = (float) radio_control.values[RADIO_YAW] * STABILIZATION_INDI_MAX_RATE / MAX_PPRZ;
+  } else {
+    stabilization_rate_sp.p = 0;
+  }
 }
 
 void stabilization_rate_enter(void)

sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate_indi.h

@@ -34,4 +34,6 @@ extern void stabilization_rate_read_rc_switched_sticks(void);
 extern void stabilization_rate_run(bool in_flight);
 extern void stabilization_rate_enter(void);
 
+extern struct FloatRates stabilization_rate_sp;
+
 #endif /* STABILIZATION_RATE_INDI */