[stabilization_indi] 2nd order rate filters and disabling pseudoinver…

…se (#3104)

* [fix] MS45xx print MSG

* [stabilization_indi] second order rate filters and disabling pseudoinverse

* Apply suggestions from code review

* [fix] alternate in_flight detectors need motors_on.

* Default behaviour is WLS

sw/airborne/firmwares/rotorcraft/autopilot_firmware.c

@@ -89,7 +89,7 @@ bool WEAK autopilot_ground_detection(void) {
 
 
 /** Default end-of-in-flight detection estimation based on thrust and speed */
-bool WEAK autopilot_in_flight_end_detection(void) {
+bool WEAK autopilot_in_flight_end_detection(bool motors_on UNUSED) {
   if (autopilot_in_flight_counter > 0) {
     /* probably in_flight if thrust, speed and accel above IN_FLIGHT_MIN thresholds */
     if ((stabilization_cmd[COMMAND_THRUST] <= AUTOPILOT_IN_FLIGHT_MIN_THRUST) &&
@@ -282,7 +282,7 @@ void autopilot_reset_in_flight_counter(void)
 void autopilot_check_in_flight(bool motors_on)
 {
   if (autopilot.in_flight) {
-    if (autopilot_in_flight_end_detection()) {
+    if (autopilot_in_flight_end_detection(motors_on)) {
       autopilot.in_flight = false;
       autopilot_in_flight_counter = 0;
     }

sw/airborne/firmwares/rotorcraft/autopilot_firmware.h

@@ -37,7 +37,7 @@ extern uint8_t autopilot_mode_auto2;
 extern bool autopilot_ground_detection(void);
 
 // Detect the end of in_flight and stop integrators in control loops
-extern bool autopilot_in_flight_end_detection(void);
+extern bool autopilot_in_flight_end_detection(bool motors_on);
 
 extern void autopilot_firmware_init(void);
 

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

@@ -66,6 +66,15 @@
 #define STABILIZATION_INDI_FILT_CUTOFF_R 20.0
 #endif
 
+// Default is WLS
+#ifndef STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
+#define STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE FALSE
+#endif
+
+#ifndef INDI_FILTER_RATES_SECOND_ORDER
+#define INDI_FILTER_RATES_SECOND_ORDER FALSE
+#endif
+
 // Airspeed [m/s] at which the forward flight throttle limit is used instead of
 // the hover throttle limit.
 #ifndef INDI_HROTTLE_LIMIT_AIRSPEED_FWD
@@ -83,7 +92,9 @@ static void lms_estimation(void);
 static void get_actuator_state(void);
 static void calc_g1_element(float dx_error, int8_t i, int8_t j, float mu_extra);
 static void calc_g2_element(float dx_error, int8_t j, float mu_extra);
+#if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
 static void calc_g1g2_pseudo_inv(void);
+#endif
 static void bound_g_mat(void);
 
 int32_t stabilization_att_indi_cmd[COMMANDS_NB];
@@ -203,8 +214,11 @@ Butterworth2LowPass estimation_input_lowpass_filters[INDI_NUM_ACT];
 Butterworth2LowPass measurement_lowpass_filters[3];
 Butterworth2LowPass estimation_output_lowpass_filters[3];
 Butterworth2LowPass acceleration_lowpass_filter;
+#if INDI_FILTER_RATES_SECOND_ORDER
+Butterworth2LowPass rates_filt_so[3];
+#else
 static struct FirstOrderLowPass rates_filt_fo[3];
-
+#endif
 struct FloatVect3 body_accel_f;
 
 void init_filters(void);
@@ -281,9 +295,14 @@ void stabilization_indi_init(void)
   float_vect_zero(estimation_rate_dd, INDI_NUM_ACT);
   float_vect_zero(actuator_state_filt_vect, INDI_NUM_ACT);
 
-  //Calculate G1G2_PSEUDO_INVERSE
+  //Calculate G1G2
   sum_g1_g2();
+
+  // Do not compute if not needed
+  #if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
+  //Calculate G1G2_PSEUDO_INVERSE
   calc_g1g2_pseudo_inv();
+  #endif
 
   int8_t i;
   // Initialize the array of pointers to the rows of g1g2
@@ -353,12 +372,21 @@ void init_filters(void)
   // Filtering of the accel body z
   init_butterworth_2_low_pass(&acceleration_lowpass_filter, tau_est, sample_time, 0.0);
 
+#if INDI_FILTER_RATES_SECOND_ORDER
+  tau = 1.0 / (2.0 * M_PI * STABILIZATION_INDI_FILT_CUTOFF_P);
+  init_butterworth_2_low_pass(&rates_filt_so[0], tau, sample_time, 0.0);
+  tau = 1.0 / (2.0 * M_PI * STABILIZATION_INDI_FILT_CUTOFF_Q);
+  init_butterworth_2_low_pass(&rates_filt_so[1], tau, sample_time, 0.0);
+  tau = 1.0 / (2.0 * M_PI * STABILIZATION_INDI_FILT_CUTOFF_R);
+  init_butterworth_2_low_pass(&rates_filt_so[2], tau, sample_time, 0.0);
+#else
   // Init rate filter for feedback
   float time_constants[3] = {1.0 / (2 * M_PI * STABILIZATION_INDI_FILT_CUTOFF_P), 1.0 / (2 * M_PI * STABILIZATION_INDI_FILT_CUTOFF_Q), 1.0 / (2 * M_PI * STABILIZATION_INDI_FILT_CUTOFF_R)};
 
   init_first_order_low_pass(&rates_filt_fo[0], time_constants[0], sample_time, stateGetBodyRates_f()->p);
   init_first_order_low_pass(&rates_filt_fo[1], time_constants[1], sample_time, stateGetBodyRates_f()->q);
   init_first_order_low_pass(&rates_filt_fo[2], time_constants[2], sample_time, stateGetBodyRates_f()->r);
+#endif
 }
 
 /**
@@ -463,17 +491,29 @@ void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight)
   //Note that due to the delay, the PD controller may need relaxed gains.
   struct FloatRates rates_filt;
 #if STABILIZATION_INDI_FILTER_ROLL_RATE
+#if INDI_FILTER_RATES_SECOND_ORDER
+  rates_filt.p = update_butterworth_2_low_pass(&rates_filt_so[0], body_rates->p);
+#else
   rates_filt.p = update_first_order_low_pass(&rates_filt_fo[0], body_rates->p);
+#endif
 #else
   rates_filt.p = body_rates->p;
 #endif
 #if STABILIZATION_INDI_FILTER_PITCH_RATE
+#if INDI_FILTER_RATES_SECOND_ORDER
+  rates_filt.q = update_butterworth_2_low_pass(&rates_filt_so[1], body_rates->q);
+#else
   rates_filt.q = update_first_order_low_pass(&rates_filt_fo[1], body_rates->q);
+#endif
 #else
   rates_filt.q = body_rates->q;
 #endif
 #if STABILIZATION_INDI_FILTER_YAW_RATE
+#if INDI_FILTER_RATES_SECOND_ORDER
+  rates_filt.r = update_butterworth_2_low_pass(&rates_filt_so[2], body_rates->r);
+#else
   rates_filt.r = update_first_order_low_pass(&rates_filt_fo[2], body_rates->r);
+#endif
 #else
   rates_filt.r = body_rates->r;
 #endif
@@ -837,6 +877,7 @@ void sum_g1_g2(void) {
   }
 }
 
+#if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
 /**
  * Function that calculates the pseudo-inverse of (G1+G2).
  * Make sure to sum of G1 and G2 before running this!
@@ -880,10 +921,12 @@ void calc_g1g2_pseudo_inv(void)
     }
   }
 }
+#endif
 
 static void rpm_cb(uint8_t sender_id UNUSED, struct rpm_act_t *rpm_msg UNUSED, uint8_t num_act UNUSED)
 {
 #if INDI_RPM_FEEDBACK
+PRINT_CONFIG_MSG("INDI_RPM_FEEDBACK");
   int8_t i;
   for (i = 0; i < num_act; i++) {
     // Sanity check that index is valid

sw/airborne/modules/sensors/airspeed_ms45xx_i2c.c

@@ -140,7 +140,7 @@ PRINT_CONFIG_VAR(MS45XX_PRESSURE_OFFSET)
  * default airspeed scale is 2/1.225
  */
 #ifdef MS45XX_AIRSPEED_SCALE
-#PRINT_CONFIG("MS45XX changed air density. PS: Use MS45XX_PRESSURE_SCALE to calibrate the MS45XX.")
+PRINT_CONFIG_MSG("MS45XX changed air density. PS: Use MS45XX_PRESSURE_SCALE to calibrate the MS45XX.");
 #else
 #define MS45XX_AIRSPEED_SCALE 1.6327
 #endif