From b033c90132645820ce1ccd9cb90d7cbbad177e52 Mon Sep 17 00:00:00 2001 From: Christophe De Wagter Date: Tue, 26 Sep 2023 14:10:42 +0200 Subject: [PATCH] [stabilization_indi] 2nd order rate filters and disabling pseudoinverse (#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 --- .../firmwares/rotorcraft/autopilot_firmware.c | 4 +- .../firmwares/rotorcraft/autopilot_firmware.h | 2 +- .../stabilization/stabilization_indi.c | 47 ++++++++++++++++++- .../modules/sensors/airspeed_ms45xx_i2c.c | 2 +- 4 files changed, 49 insertions(+), 6 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_firmware.c b/sw/airborne/firmwares/rotorcraft/autopilot_firmware.c index f3a3771b73..ecff8ce228 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot_firmware.c +++ b/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; } diff --git a/sw/airborne/firmwares/rotorcraft/autopilot_firmware.h b/sw/airborne/firmwares/rotorcraft/autopilot_firmware.h index 870453bb3c..405db6680a 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot_firmware.h +++ b/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); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c index 5835090549..5a73927947 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c +++ b/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 diff --git a/sw/airborne/modules/sensors/airspeed_ms45xx_i2c.c b/sw/airborne/modules/sensors/airspeed_ms45xx_i2c.c index 566bc974be..290e8d13ac 100644 --- a/sw/airborne/modules/sensors/airspeed_ms45xx_i2c.c +++ b/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