Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-10 06:59:54 +08:00
Code Issues Actions 31 Packages Projects Releases Wiki Activity

[stabilization_indi] 2nd order rate filters and disabling pseudoinverse (#3104)

Browse Source 
* [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
This commit is contained in:
Christophe De Wagter
2023-09-26 14:10:42 +02:00
committed by GitHub
parent f4f0cf2f3b
commit b033c90132
4 changed files with 49 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/autopilot_firmware.c
+2 -2
View File
@@ -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
+1 -1
View File
@@ -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
+45 -2
View File
@@ -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
+1 -1
View File
@@ -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