added the possibility to add a yaw rate limit to stabilization indi (#1684)

the yaw of a typical quadrotor easily saturates. One of the problems that occur when asking for a large change in yaw angle, is that the quadrotor can build up so much angular velocity in the yaw axis, that it can not physically slow down before reaching the yaw setpoint due to saturating. This will result in overshoot.

This can be helped by adding a yaw rate limit, which this PR does for the INDI controller.

conf/airframes/TUDELFT/tudelft_bebop_indi.xml

@@ -100,7 +100,7 @@
     <!-- setpoint limits for attitude stabilization rc flight -->
     <define name="SP_MAX_PHI" value="45" unit="deg"/>
     <define name="SP_MAX_THETA" value="45" unit="deg"/>
-    <define name="SP_MAX_R" value="300" unit="deg/s"/>
+    <define name="SP_MAX_R" value="120" unit="deg/s"/>
     <define name="DEADBAND_A" value="0"/>
     <define name="DEADBAND_E" value="0"/>
     <define name="DEADBAND_R" value="50"/>
@@ -139,6 +139,9 @@
     <define name="REF_RATE_Q" value="28.0"/>
     <define name="REF_RATE_R" value="28.0"/>
 
+    <!--Maxium yaw rate, to avoid instability-->
+    <define name="MAX_R" value="120.0" unit="deg/s"/>
+
     <!-- second order filter parameters -->
     <define name="FILT_OMEGA" value="50.0"/>
     <define name="FILT_ZETA" value="0.55"/>

conf/settings/control/stabilization_indi.xml

@@ -15,7 +15,8 @@
       <dl_setting var="indi.g1.r" min="0" step="0.001" max="10" module="stabilization/stabilization_indi" shortname="ctl_eff_r" param="STABILIZATION_INDI_G1_R" persistent="true"/>
       <dl_setting var="indi.g2" min="0" step="0.01" max="10" module="stabilization/stabilization_indi" shortname="g2" param="STABILIZATION_INDI_G2_R" persistent="true"/>
       <dl_setting var="indi.adaptive" min="0" step="1" max="1" module="stabilization/stabilization_indi" shortname="use_adaptive" values="FALSE|TRUE" param="STABILIZATION_INDI_USE_ADAPTIVE" type="uint8" persistent="true"/>
-      <dl_setting var="indi.max_rate" min="0" step="0.0001" max="80.0" module="stabilization/stabilization_indi" shortname="max_rate"/>
+      <dl_setting var="indi.max_rate" min="0" step="0.01" max="80.0" module="stabilization/stabilization_indi" shortname="max_rate" param="STABILIZATION_INDI_MAX_RATE" unit="rad/s" alt_unit="deg/s"/>
+      <dl_setting var="indi.attitude_max_yaw_rate" min="0" step="0.01" max="80.0" module="stabilization/stabilization_indi" shortname="max_yaw_rate_attitude" param="STABILIZATION_INDI_MAX_R" unit="rad/s" alt_unit="deg/s"/>
     </dl_settings>
 
   </dl_settings>

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

@@ -71,6 +71,10 @@
 #warning "Use caution with adaptive indi. See the wiki for more info"
 #endif
 
+#ifndef STABILIZATION_INDI_MAX_R
+#define STABILIZATION_INDI_MAX_R STABILIZATION_ATTITUDE_SP_MAX_R
+#endif
+
 struct Int32Eulers stab_att_sp_euler;
 struct Int32Quat   stab_att_sp_quat;
 
@@ -83,6 +87,7 @@ static inline void lms_estimation(void);
 #define INDI_EST_SCALE 0.001 //The G values are scaled to avoid numerical problems during the estimation
 struct IndiVariables indi = {
   .max_rate = STABILIZATION_INDI_MAX_RATE,
+  .attitude_max_yaw_rate = STABILIZATION_INDI_MAX_R,
 
   .g1 = {STABILIZATION_INDI_G1_P, STABILIZATION_INDI_G1_Q, STABILIZATION_INDI_G1_R},
   .g2 = STABILIZATION_INDI_G2_R,
@@ -206,28 +211,33 @@ static inline void stabilization_indi_calc_cmd(int32_t indi_commands[], struct I
   struct FloatRates *body_rates = stateGetBodyRates_f();
   stabilization_indi_second_order_filter(&indi.rate, body_rates);
 
+  //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));
+
+  //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
-  indi.angular_accel_ref.p = indi.reference_acceleration.err_p * QUAT1_FLOAT_OF_BFP(att_err->qx)
-                             - indi.reference_acceleration.rate_p * indi.rate.x.p;
-#else
-  indi.angular_accel_ref.p = indi.reference_acceleration.err_p * QUAT1_FLOAT_OF_BFP(att_err->qx)
-                             - indi.reference_acceleration.rate_p * body_rates->p;
+  rates_for_feedback.p = indi.rate.x.p;
 #endif
 #if STABILIZATION_INDI_FILTER_PITCH_RATE
-  indi.angular_accel_ref.q = indi.reference_acceleration.err_q * QUAT1_FLOAT_OF_BFP(att_err->qy)
-                             - indi.reference_acceleration.rate_q * indi.rate.x.q;
-#else
-  indi.angular_accel_ref.q = indi.reference_acceleration.err_q * QUAT1_FLOAT_OF_BFP(att_err->qy)
-                             - indi.reference_acceleration.rate_q * body_rates->q;
+  rates_for_feedback.q = indi.rate.x.q;
 #endif
 #if STABILIZATION_INDI_FILTER_YAW_RATE
-  indi.angular_accel_ref.r = indi.reference_acceleration.err_r * QUAT1_FLOAT_OF_BFP(att_err->qz)
-                             - indi.reference_acceleration.rate_r * indi.rate.x.r;
-#else
-  indi.angular_accel_ref.r = indi.reference_acceleration.err_r * QUAT1_FLOAT_OF_BFP(att_err->qz)
-                             - indi.reference_acceleration.rate_r * body_rates->r;
+  rates_for_feedback.r = indi.rate.x.r;
 #endif
 
+  indi.angular_accel_ref.p = indi.reference_acceleration.err_p * QUAT1_FLOAT_OF_BFP(att_err->qx)
+                             - indi.reference_acceleration.rate_p * rates_for_feedback.p;
+
+  indi.angular_accel_ref.q = indi.reference_acceleration.err_q * QUAT1_FLOAT_OF_BFP(att_err->qy)
+                             - 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 = indi.reference_acceleration.err_r * QUAT1_FLOAT_OF_BFP(att_err->qz)/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);
+
   /* 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);

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

@@ -82,7 +82,8 @@ struct IndiVariables {
   struct ReferenceSystem reference_acceleration;
 
   bool adaptive;             ///< Enable adataptive estimation
-  float max_rate;            ///< Maximum rate in rate control
+  float max_rate;            ///< Maximum rate in rate control in rad/s
+  float attitude_max_yaw_rate; ///< Maximum yaw rate in atttiude control in rad/s
   struct IndiEstimation est; ///< Estimation parameters for adaptive INDI
 };