[stabilization_indi] second order rate filters and disabling pseudoin…

…verse

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

@@ -66,6 +66,14 @@
 #define STABILIZATION_INDI_FILT_CUTOFF_R 20.0
 #endif
 
+#ifndef STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
+#define STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE TRUE
+#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 +91,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 +213,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);
@@ -283,7 +296,10 @@ void stabilization_indi_init(void)
 
   //Calculate G1G2_PSEUDO_INVERSE
   sum_g1_g2();
+
+  #if STABILIZATION_INDI_ALLOCATION_PSEUDO_INVERSE
   calc_g1g2_pseudo_inv();
+  #endif
 
   int8_t i;
   // Initialize the array of pointers to the rows of g1g2
@@ -353,12 +369,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 +488,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.p = 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.p = 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 +874,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 +918,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