INDI code cleanup

Renaming confusing variables
Fixing rate interface

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

@@ -35,7 +35,7 @@
 void stabilization_attitude_init(void)
 {
   // Check if the indi init is already done for rate control
-#ifndef STABILIZATION_RATE_INDI
+#ifndef USE_STABILIZATION_RATE_INDI
   stabilization_indi_init();
 #endif
 }
@@ -62,9 +62,7 @@ void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t head
 
 void stabilization_attitude_run(bool in_flight)
 {
-  struct Int32Quat quat_sp = {1, 0, 0, 0}; // always hover
-
-  stabilization_indi_attitude_run(in_flight, quat_sp);
+  stabilization_indi_attitude_run(stab_att_sp_quat, in_flight);
 }
 
 void stabilization_attitude_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn)

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

@@ -63,13 +63,17 @@ static void calc_g1g2_pseudo_inv(void);
 static void bound_g_mat(void);
 
 int32_t stabilization_att_indi_cmd[COMMANDS_NB];
-struct ReferenceSystem reference_acceleration = {
-  STABILIZATION_INDI_REF_ERR_P,
-  STABILIZATION_INDI_REF_ERR_Q,
-  STABILIZATION_INDI_REF_ERR_R,
-  STABILIZATION_INDI_REF_RATE_P,
-  STABILIZATION_INDI_REF_RATE_Q,
-  STABILIZATION_INDI_REF_RATE_R,
+struct Indi_gains indi_gains = {
+  .att = {
+    STABILIZATION_INDI_REF_ERR_P,
+    STABILIZATION_INDI_REF_ERR_Q,
+    STABILIZATION_INDI_REF_ERR_R
+  },
+  .rate = {
+    STABILIZATION_INDI_REF_RATE_P,
+    STABILIZATION_INDI_REF_RATE_Q,
+    STABILIZATION_INDI_REF_RATE_R
+  },
 };
 
 #if STABILIZATION_INDI_USE_ADAPTIVE
@@ -98,11 +102,6 @@ float act_pref[INDI_NUM_ACT] = {0.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
-
 #ifdef STABILIZATION_INDI_WLS_PRIORITIES
 static float Wv[INDI_OUTPUTS] = STABILIZATION_INDI_WLS_PRIORITIES;
 #else
@@ -345,20 +344,36 @@ void stabilization_indi_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading)
  *
  * Function that calculates the INDI commands
  */
-void stabilization_indi_calc_cmd(struct FloatRates rate_ref, bool in_flight)
+void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight)
 {
+  /* Propagate the filter on the gyroscopes */
   struct FloatRates *body_rates = stateGetBodyRates_f();
+  float rate_vect[3] = {body_rates->p, body_rates->q, body_rates->r};
+  int8_t i;
+  for (i = 0; i < 3; i++) {
+    update_butterworth_2_low_pass(&measurement_lowpass_filters[i], rate_vect[i]);
+    update_butterworth_2_low_pass(&estimation_output_lowpass_filters[i], rate_vect[i]);
 
+    //Calculate the angular acceleration via finite difference
+    angular_acceleration[i] = (measurement_lowpass_filters[i].o[0]
+                               - measurement_lowpass_filters[i].o[1]) * PERIODIC_FREQUENCY;
+
+    // Calculate derivatives for estimation
+    float estimation_rate_d_prev = estimation_rate_d[i];
+    estimation_rate_d[i] = (estimation_output_lowpass_filters[i].o[0] - estimation_output_lowpass_filters[i].o[1]) * PERIODIC_FREQUENCY;
+    estimation_rate_dd[i] = (estimation_rate_d[i] - estimation_rate_d_prev) * PERIODIC_FREQUENCY;
+  }
+
+  // FIXME: this should be configurable!
   q_filt = (q_filt*3+body_rates->q)/4;
   r_filt = (r_filt*3+body_rates->r)/4;
 
   //calculate the virtual control (reference acceleration) based on a PD controller
-  angular_accel_ref.p = (rate_ref.p - body_rates->p) * reference_acceleration.rate_p;
-  angular_accel_ref.q = (rate_ref.q - q_filt) * reference_acceleration.rate_q;
-  angular_accel_ref.r = (rate_ref.r - r_filt) * reference_acceleration.rate_r;
+  angular_accel_ref.p = (rate_sp.p - body_rates->p) * indi_gains.rate.p;
+  angular_accel_ref.q = (rate_sp.q - q_filt) * indi_gains.rate.q;
+  angular_accel_ref.r = (rate_sp.r - r_filt) * indi_gains.rate.r;
 
   g2_times_du = 0.0;
-  int8_t i;
   for (i = 0; i < INDI_NUM_ACT; i++) {
     g2_times_du += g2[i] * indi_du[i];
   }
@@ -478,50 +493,29 @@ void stabilization_indi_calc_cmd(struct FloatRates rate_ref, bool in_flight)
  *
  * Function that should be called to run the INDI controller
  */
-void stabilization_indi_attitude_run(bool in_flight, struct Int32Quat quat_sp)
+void stabilization_indi_attitude_run(struct Int32Quat quat_sp, bool in_flight)
 {
-  /* Propagate the filter on the gyroscopes */
-  struct FloatRates *body_rates = stateGetBodyRates_f();
-  float rate_vect[3] = {body_rates->p, body_rates->q, body_rates->r};
-  int8_t i;
-  for (i = 0; i < 3; i++) {
-    update_butterworth_2_low_pass(&measurement_lowpass_filters[i], rate_vect[i]);
-    update_butterworth_2_low_pass(&estimation_output_lowpass_filters[i], rate_vect[i]);
-
-    //Calculate the angular acceleration via finite difference
-    angular_acceleration[i] = (measurement_lowpass_filters[i].o[0]
-                               - measurement_lowpass_filters[i].o[1]) * PERIODIC_FREQUENCY;
-
-    // Calculate derivatives for estimation
-    float estimation_rate_d_prev = estimation_rate_d[i];
-    estimation_rate_d[i] = (estimation_output_lowpass_filters[i].o[0] - estimation_output_lowpass_filters[i].o[1]) * PERIODIC_FREQUENCY;
-    estimation_rate_dd[i] = (estimation_rate_d[i] - estimation_rate_d_prev) * PERIODIC_FREQUENCY;
-  }
-
   /* attitude error                          */
   struct Int32Quat att_err;
   struct Int32Quat *att_quat = stateGetNedToBodyQuat_i();
-  int32_quat_inv_comp(&att_err, att_quat, &quat_sp); 
+  int32_quat_inv_comp(&att_err, att_quat, &quat_sp);
   /* wrap it in the shortest direction       */
   int32_quat_wrap_shortest(&att_err);
   int32_quat_normalize(&att_err);
 
   // local variable to compute rate setpoints based on attitude error
-  struct FloatRates rate_ref; 
+  struct FloatRates rate_sp;
 
   // 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)
-                / 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;
+  rate_sp.p = indi_gains.att.p * QUAT1_FLOAT_OF_BFP(att_err.qx) / indi_gains.rate.p;
+  rate_sp.q = indi_gains.att.q * QUAT1_FLOAT_OF_BFP(att_err.qy) / indi_gains.rate.q;
+  rate_sp.r = indi_gains.att.r * QUAT1_FLOAT_OF_BFP(att_err.qz) / indi_gains.rate.r;
 
   // Possibly we can use some bounding here
-  /*BoundAbs(rate_ref.r, 5.0);*/
+  /*BoundAbs(rate_sp.r, 5.0);*/
 
   /* compute the INDI command */
-  stabilization_indi_calc_cmd(rate_ref, in_flight);
+  stabilization_indi_rate_run(rate_sp, in_flight);
 
   // Reset thrust increment boolean
   indi_thrust_increment_set = false;

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

@@ -38,24 +38,20 @@ extern bool indi_use_adaptive;
 
 extern float *Bwls[INDI_OUTPUTS];
 
-struct ReferenceSystem {
-  float err_p;
-  float err_q;
-  float err_r;
-  float rate_p;
-  float rate_q;
-  float rate_r;
+struct Indi_gains {
+  struct FloatRates att;
+  struct FloatRates rate;
 };
 
-extern struct ReferenceSystem reference_acceleration;
+extern struct Indi_gains indi_gains;
 
 extern void stabilization_indi_init(void);
 extern void stabilization_indi_enter(void);
 extern void stabilization_indi_set_failsafe_setpoint(void);
 extern void stabilization_indi_set_rpy_setpoint_i(struct Int32Eulers *rpy);
 extern void stabilization_indi_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading);
-extern void stabilization_indi_calc_cmd(struct FloatRates rate_ref, bool in_flight);
-extern void stabilization_indi_attitude_run(bool in_flight, struct Int32Quat quat_sp);
+extern void stabilization_indi_rate_run(struct FloatRates rate_ref, bool in_flight);
+extern void stabilization_indi_attitude_run(struct Int32Quat quat_sp, bool in_flight);
 extern void stabilization_indi_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn);
 
 #endif /* STABILIZATION_INDI */

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

@@ -86,13 +86,17 @@ struct IndiVariables indi = {
 
   .g1 = {STABILIZATION_INDI_G1_P, STABILIZATION_INDI_G1_Q, STABILIZATION_INDI_G1_R},
   .g2 = STABILIZATION_INDI_G2_R,
-  .reference_acceleration = {
-    STABILIZATION_INDI_REF_ERR_P,
-    STABILIZATION_INDI_REF_ERR_Q,
-    STABILIZATION_INDI_REF_ERR_R,
-    STABILIZATION_INDI_REF_RATE_P,
-    STABILIZATION_INDI_REF_RATE_Q,
-    STABILIZATION_INDI_REF_RATE_R
+  .gains = {
+    .att = {
+      STABILIZATION_INDI_REF_ERR_P,
+      STABILIZATION_INDI_REF_ERR_Q,
+      STABILIZATION_INDI_REF_ERR_R
+    },
+    .rate = {
+      STABILIZATION_INDI_REF_RATE_P,
+      STABILIZATION_INDI_REF_RATE_Q,
+      STABILIZATION_INDI_REF_RATE_R
+    },
   },
 
   /* Estimation parameters for adaptive INDI */
@@ -207,7 +211,6 @@ void stabilization_indi_set_failsafe_setpoint(void)
  *
  * @param rpy roll pitch yaw input
  */
-
 void stabilization_indi_set_rpy_setpoint_i(struct Int32Eulers *rpy)
 {
   // stab_att_sp_euler.psi still used in ref..
@@ -215,7 +218,7 @@ void stabilization_indi_set_rpy_setpoint_i(struct Int32Eulers *rpy)
 
   int32_quat_of_eulers(&stab_att_sp_quat, &stab_att_sp_euler);
 }
- 
+
 /**
  * @brief Set attitude setpoint from command in earth axes
  *
@@ -285,9 +288,8 @@ static inline void finite_difference(float output[3], float new[3], float old[3]
  *
  * @param indi_commands[] Array of commands that the function will write to
  * @param att_err quaternion attitude error
- * @param rate_control rate control enabled, otherwise attitude control
  */
-void stabilization_indi_calc_cmd(struct FloatRates rate_ref, bool in_flight __attribute__((unused)), bool rate_control)
+void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight __attribute__((unused)))
 {
   // Propagate the filter on the gyroscopes and actuators
   struct FloatRates *body_rates = stateGetBodyRates_f();
@@ -298,37 +300,28 @@ void stabilization_indi_calc_cmd(struct FloatRates rate_ref, bool in_flight __at
   finite_difference_from_filter(indi.rate_d, indi.rate);
 
   //The rates used for feedback are by default the measured rates. If needed they can be filtered (see below)
-  struct FloatRates rates_for_feedback;
-  RATES_COPY(rates_for_feedback, (*body_rates));
+  struct FloatRates rates_filt_fo;
+  RATES_COPY(rates_filt_fo, (*body_rates));
 
   //If there is a lot of noise on the gyroscope, it might be good to use the filtered value for feedback.
   //Note that due to the delay, the PD controller can not be as aggressive.
 #if STABILIZATION_INDI_FILTER_ROLL_RATE
-  rates_for_feedback.p = indi.rate[0].o[0];
+  rates_filt_fo.p = (rates_filt_fo.p*3+body_rates->p)/4;
 #endif
 #if STABILIZATION_INDI_FILTER_PITCH_RATE
-  rates_for_feedback.q = indi.rate[1].o[0];
+  rates_filt_fo.q = (rates_filt_fo.q*3+body_rates->q)/4;
 #endif
 #if STABILIZATION_INDI_FILTER_YAW_RATE
-  rates_for_feedback.r = indi.rate[2].o[0];
+  rates_filt_fo.r = (rates_filt_fo.r*3+body_rates->r)/4;
 #endif
 
-  // Compute reference angular acceleration:
-  indi.angular_accel_ref.p = rate_ref.p - indi.reference_acceleration.rate_p * rates_for_feedback.p;
-
-  indi.angular_accel_ref.q = rate_ref.q - indi.reference_acceleration.rate_q * rates_for_feedback.q;
-
   //This separates the P and D controller and lets you impose a maximum yaw rate.
-  float rate_ref_r = rate_ref.r / indi.reference_acceleration.rate_r;
-  BoundAbs(rate_ref_r, indi.attitude_max_yaw_rate);
-  indi.angular_accel_ref.r = indi.reference_acceleration.rate_r * (rate_ref_r - rates_for_feedback.r);
+  BoundAbs(rate_sp.r, indi.attitude_max_yaw_rate);
 
-  /* Check if we are running the rate controller and overwrite */
-  if (rate_control) {
-    indi.angular_accel_ref.p =  indi.reference_acceleration.rate_p * (rate_ref.p - body_rates->p);
-    indi.angular_accel_ref.q =  indi.reference_acceleration.rate_q * (rate_ref.q - body_rates->q);
-    indi.angular_accel_ref.r =  indi.reference_acceleration.rate_r * (rate_ref.r - body_rates->r);
-  }
+  // Compute reference angular acceleration:
+  indi.angular_accel_ref.p = (rate_sp.p - rates_filt_fo.p) * indi.gains.rate.p;
+  indi.angular_accel_ref.q = (rate_sp.q - rates_filt_fo.q) * indi.gains.rate.q;
+  indi.angular_accel_ref.r = (rate_sp.r - rates_filt_fo.r) * indi.gains.rate.r;
 
   //Increment in angular acceleration requires increment in control input
   //G1 is the control effectiveness. In the yaw axis, we need something additional: G2.
@@ -391,7 +384,7 @@ void stabilization_indi_calc_cmd(struct FloatRates rate_ref, bool in_flight __at
  * @param in_flight not used
  * @param rate_control rate control enabled, otherwise attitude control
  */
-void stabilization_indi_attitude_run(bool in_flight __attribute__((unused)), struct Int32Quat quat_sp)
+void stabilization_indi_attitude_run(struct Int32Quat quat_sp, bool in_flight __attribute__((unused)))
 {
   /* attitude error                          */
   struct Int32Quat att_err;
@@ -404,12 +397,12 @@ void stabilization_indi_attitude_run(bool in_flight __attribute__((unused)), str
   struct FloatRates rate_sp;
 
   // indi.reference_acceleration.err_p * QUAT1_FLOAT_OF_BFP(att_err.qx) - rate setpoint
-  rate_sp.p = indi.reference_acceleration.err_p * QUAT1_FLOAT_OF_BFP(att_err.qx);
-  rate_sp.q = indi.reference_acceleration.err_q * QUAT1_FLOAT_OF_BFP(att_err.qy);
-  rate_sp.r = indi.reference_acceleration.err_r * QUAT1_FLOAT_OF_BFP(att_err.qz);
+  rate_sp.p = indi.gains.att.p * QUAT1_FLOAT_OF_BFP(att_err.qx) / indi.gains.rate.p;
+  rate_sp.q = indi.gains.att.q * QUAT1_FLOAT_OF_BFP(att_err.qy) / indi.gains.rate.q;
+  rate_sp.r = indi.gains.att.r * QUAT1_FLOAT_OF_BFP(att_err.qz) / indi.gains.rate.r;
 
   /* compute the INDI command */
-  stabilization_indi_calc_cmd(rate_sp, in_flight, FALSE);
+  stabilization_indi_rate_run(rate_sp, in_flight);
 }
 
 /**

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

@@ -40,13 +40,9 @@
 extern struct Int32Quat   stab_att_sp_quat;  ///< with #INT32_QUAT_FRAC
 extern struct Int32Eulers stab_att_sp_euler; ///< with #INT32_ANGLE_FRAC
 
-struct ReferenceSystem {
-  float err_p;
-  float err_q;
-  float err_r;
-  float rate_p;
-  float rate_q;
-  float rate_r;
+struct Indi_gains {
+  struct FloatRates att;
+  struct FloatRates rate;
 };
 
 struct IndiEstimation {
@@ -73,7 +69,7 @@ struct IndiVariables {
   struct FloatRates g1;
   float g2;
 
-  struct ReferenceSystem reference_acceleration;
+  struct Indi_gains gains;
 
   bool adaptive;             ///< Enable adataptive estimation
   float max_rate;            ///< Maximum rate in rate control in rad/s
@@ -88,8 +84,8 @@ extern void stabilization_indi_enter(void);
 extern void stabilization_indi_set_failsafe_setpoint(void);
 extern void stabilization_indi_set_rpy_setpoint_i(struct Int32Eulers *rpy);
 extern void stabilization_indi_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading);
-extern void stabilization_indi_calc_cmd(struct FloatRates rates_ref, bool in_flight, bool rate_control);
-extern void stabilization_indi_attitude_run(bool in_flight, struct Int32Quat quat_sp);
+extern void stabilization_indi_rate_run(struct FloatRates rates_sp, bool in_flight);
+extern void stabilization_indi_attitude_run(struct Int32Quat quat_sp, bool in_flight);
 extern void stabilization_indi_read_rc(bool in_flight, bool in_carefree, bool coordinated_turn);
 
 #endif /* STABILIZATION_INDI_SIMPLE_H */

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

@@ -1,6 +1,6 @@
 /*
  * Copyright (C) 2016 Freek van Tienen <freek.v.tienen@gmail.com>
- *               2020 Rohan Chotalal 
+ *               2020 Rohan Chotalal
  *
  * This file is part of paparazzi.
  *
@@ -33,10 +33,17 @@
 #include "firmwares/rotorcraft/stabilization/stabilization_indi.h"
 #endif
 
+#include "firmwares/rotorcraft/autopilot_rc_helpers.h"
+
 /* -- define variables */
 struct FloatRates stabilization_rate_sp;
 
-/* -- RC deadbands */ 
+/** Maximum rate you can request in RC rate mode (rad/s)*/
+#ifndef STABILIZATION_INDI_MAX_RATE
+#define STABILIZATION_INDI_MAX_RATE 6.0
+#endif
+
+/* -- RC deadbands */
 #ifndef STABILIZATION_RATE_DEADBAND_P
 #define STABILIZATION_RATE_DEADBAND_P 0
 #endif
@@ -65,14 +72,20 @@ struct FloatRates stabilization_rate_sp;
 
 static void send_rate(struct transport_tx *trans, struct link_device *dev)
 {
-  float dummy = 0; 
+  float dummy = 0;
+  float fb_p = stabilization_cmd[COMMAND_ROLL];
+  float fb_q = stabilization_cmd[COMMAND_PITCH];
+  float fb_r = stabilization_cmd[COMMAND_YAW];
 
   pprz_msg_send_RATE_LOOP(trans, dev, AC_ID,
                           &stabilization_rate_sp.p,
                           &stabilization_rate_sp.q,
                           &stabilization_rate_sp.r,
-                          &dummy, &dummy, &dummy, // [CHECK THIS!] 
-                          &stabilization_cmd[COMMAND_THRUST]); [// CHECK WITH EWOUD]
+                          &dummy, &dummy, &dummy,
+                          &fb_p,
+                          &fb_q,
+                          &fb_r,
+                          &stabilization_cmd[COMMAND_THRUST]);
 }
 #endif
 
@@ -88,6 +101,14 @@ void stabilization_rate_init(void)
 #endif
 }
 
+/**
+ * @brief Reset rate controller
+ */
+void stabilization_rate_enter(void)
+{
+  stabilization_indi_enter();
+}
+
 /**
  * @brief Read RC comands with roll and yaw sticks
  */
@@ -113,7 +134,7 @@ void stabilization_rate_read_rc(void)
 }
 
 /**
- * @brief Read rc with roll and yaw sitcks switched if the default orientation is vertical 
+ * @brief 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)
@@ -137,38 +158,11 @@ void stabilization_rate_read_rc_switched_sticks(void)
   }
 }
 
-void stabilization_rate_indi_cmd(bool in_flight, struct Int32Rates rate_sp)
-{
-#ifdef STABILIZATION_ATTITUDE_INDI_SIMPLE
-stabilization_indi_calc_cmd(rate_sp, in_flight, TRUE)
-#else
-stabilization_indi_calc_cmd(rate_sp, in_flight)
-#endif
-}
-
 /**
- * @brief Run this indi rate interface (when module mode!) 
- */
-void stabilization_rate_indi_run(bool in_flight, struct Int32Rates rates_sp)
-{
-  /* compute the INDI rate command */
-  stabilization_indi_calc_cmd(rates_sp, in_flight);
-}
-
-/**
- * @brief Run this indi rate interface from the "stabilization_rate_run" function 
- * (according to Gautier, when in RC mode)
+ * @brief Run indi rate interface from the "stabilization_rate_run" function
  */
 void stabilization_rate_run(bool in_flight)
 {
   /* compute the INDI rate command */
-  stabilization_rate_indi_calc_run(in_flight, stabilization_rate_sp); 
-}
-
-/**
- * @brief Enter this indi rate module 
- */
-void stabilization_rate_indi_enter(void)
-{
-  stabilization_indi_enter();
+  stabilization_indi_rate_run(stabilization_rate_sp, in_flight);
 }

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

@@ -30,12 +30,4 @@
 /* access declarations of basic functions from "stabilization_rate.h" file */
 #include "firmwares/rotorcraft/stabilization/stabilization_rate.h"
 
-#include "math/pprz_algebra_int.h"
-
-extern struct FloatRates stabilization_rate_sp;
-
-extern void stabilization_rate_indi_cmd(bool in_flight, struct Int32Rates rate_sp);
-extern void stabilization_rate_indi_run(bool in_flight, struct Int32Rates rates_sp);
-extern void stabilization_rate_indi_enter(void);
-
 #endif /* STABILIZATION_RATE_INDI */