Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-01 04:46:51 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

Indi updates (#2731)

Browse Source 
* same filtering for indi as for indi simple

* Use feed forward from the reference model

* Added XML doc for new defines
This commit is contained in:
Ewoud Smeur
2021-07-12 10:36:21 +02:00
committed by GitHub
parent 726c44682e
commit b24389655d
3 changed files with 59 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/modules/stabilization_indi.xml
+4 -1
View File
@@ -28,7 +28,10 @@
<define name="REF_RATE_Q" value="28.0" description="INDI gains linear controller"/>
<define name="REF_RATE_R" value="28.0" description="INDI gains linear controller"/>
<define name="MAX_R" value="120.0" description="max yaw rate" unit="deg/s"/>
<define name="FILT_CUTOFF" value="8.0" description="second order cutoff parameter"/>
<define name="FILT_CUTOFF" value="8.0" description="second order cutoff frequency for angular accelerations in Hz"/>
<define name="STABILIZATION_INDI_FILT_CUTOFF_P" value="20.0" description="First order cutoff freq for angular rate in Hz"/>
<define name="STABILIZATION_INDI_FILT_CUTOFF_Q" value="20.0" description="First order cutoff freq for angular rate in Hz"/>
<define name="STABILIZATION_INDI_FILT_CUTOFF_R" value="20.0" description="First order cutoff freq for angular rate in Hz"/>
<define name="ESTIMATION_FILT_CUTOFF" value="8.0" description="second order cutoff parameter"/>
<define name="ACT_DYN" value="{0.1, 0.1, 0.1, 0.1}" description="actuator dynamics"/>
<define name="ACT_IS_SERVO" value="{0,0,0,0}" description="1 for every actuator that is a servo"/>
sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.c
+7 -6
View File
@@ -165,9 +165,10 @@ void guidance_indi_run(float *heading_sp)
float pos_y_err = POS_FLOAT_OF_BFP(guidance_h.ref.pos.y) - stateGetPositionNed_f()->y;
float pos_z_err = POS_FLOAT_OF_BFP(guidance_v_z_ref - stateGetPositionNed_i()->z);
float speed_sp_x = pos_x_err * guidance_indi_pos_gain;
float speed_sp_y = pos_y_err * guidance_indi_pos_gain;
float speed_sp_z = pos_z_err * guidance_indi_pos_gain;
// Use feed forward part from reference model
float speed_sp_x = pos_x_err * guidance_indi_pos_gain + SPEED_FLOAT_OF_BFP(guidance_h.ref.speed.x);
float speed_sp_y = pos_y_err * guidance_indi_pos_gain + SPEED_FLOAT_OF_BFP(guidance_h.ref.speed.y);
float speed_sp_z = pos_z_err * guidance_indi_pos_gain + SPEED_FLOAT_OF_BFP(guidance_v_zd_ref);
// If the acceleration setpoint is set over ABI message
if (indi_accel_sp_set_2d) {
@@ -190,9 +191,9 @@ void guidance_indi_run(float *heading_sp)
indi_accel_sp_set_3d = false;
}
} else {
sp_accel.x = (speed_sp_x - stateGetSpeedNed_f()->x) * guidance_indi_speed_gain;
sp_accel.y = (speed_sp_y - stateGetSpeedNed_f()->y) * guidance_indi_speed_gain;
sp_accel.z = (speed_sp_z - stateGetSpeedNed_f()->z) * guidance_indi_speed_gain;
sp_accel.x = (speed_sp_x - stateGetSpeedNed_f()->x) * guidance_indi_speed_gain + ACCEL_FLOAT_OF_BFP(guidance_h.ref.accel.x);
sp_accel.y = (speed_sp_y - stateGetSpeedNed_f()->y) * guidance_indi_speed_gain + ACCEL_FLOAT_OF_BFP(guidance_h.ref.accel.y);
sp_accel.z = (speed_sp_z - stateGetSpeedNed_f()->z) * guidance_indi_speed_gain + ACCEL_FLOAT_OF_BFP(guidance_v_zdd_ref);
}
#if GUIDANCE_INDI_RC_DEBUG
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
+48 -6
View File
@@ -48,6 +48,24 @@
// Factor that the estimated G matrix is allowed to deviate from initial one
#define INDI_ALLOWED_G_FACTOR 2.0
#ifdef STABILIZATION_INDI_FILT_CUTOFF_P
#define STABILIZATION_INDI_FILTER_ROLL_RATE TRUE
#else
#define STABILIZATION_INDI_FILT_CUTOFF_P 20.0
#endif
#ifdef STABILIZATION_INDI_FILT_CUTOFF_Q
#define STABILIZATION_INDI_FILTER_PITCH_RATE TRUE
#else
#define STABILIZATION_INDI_FILT_CUTOFF_Q 20.0
#endif
#ifdef STABILIZATION_INDI_FILT_CUTOFF_R
#define STABILIZATION_INDI_FILTER_YAW_RATE TRUE
#else
#define STABILIZATION_INDI_FILT_CUTOFF_R 20.0
#endif
float du_min[INDI_NUM_ACT];
float du_max[INDI_NUM_ACT];
float du_pref[INDI_NUM_ACT];
@@ -169,6 +187,7 @@ Butterworth2LowPass estimation_input_lowpass_filters[INDI_NUM_ACT];
Butterworth2LowPass measurement_lowpass_filters[3];
Butterworth2LowPass estimation_output_lowpass_filters[3];
Butterworth2LowPass acceleration_lowpass_filter;
static struct FirstOrderLowPass rates_filt_fo[3];
struct FloatVect3 body_accel_f;
@@ -286,6 +305,13 @@ void init_filters(void)
// Filtering of the accel body z
init_butterworth_2_low_pass(&acceleration_lowpass_filter, tau_est, sample_time, 0.0);
// 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);
}
/**
@@ -364,14 +390,30 @@ void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight)
estimation_rate_dd[i] = (estimation_rate_d[i] - estimation_rate_d_prev) * PERIODIC_FREQUENCY;
}
// FIXME: this should be configurable!
q_filt = (q_filt*3+body_rates->q)/4;
r_filt = (r_filt*3+body_rates->r)/4;
//The rates used for feedback are by default the measured 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 may need relaxed gains.
struct FloatRates rates_filt;
#if STABILIZATION_INDI_FILTER_ROLL_RATE
rates_filt.p = update_first_order_low_pass(&rates_filt_fo[0], body_rates->p);
#else
rates_filt.p = body_rates->p;
#endif
#if STABILIZATION_INDI_FILTER_PITCH_RATE
rates_filt.q = update_first_order_low_pass(&rates_filt_fo[1], body_rates->q);
#else
rates_filt.q = body_rates->q;
#endif
#if STABILIZATION_INDI_FILTER_YAW_RATE
rates_filt.r = update_first_order_low_pass(&rates_filt_fo[2], body_rates->r);
#else
rates_filt.r = body_rates->r;
#endif
//calculate the virtual control (reference acceleration) based on a PD controller
angular_accel_ref.p = (rate_sp.p - body_rates->p) * indi_gains.rate.p;
angular_accel_ref.q = (rate_sp.q - q_filt) * indi_gains.rate.q;
angular_accel_ref.r = (rate_sp.r - r_filt) * indi_gains.rate.r;
angular_accel_ref.p = (rate_sp.p - rates_filt.p) * indi_gains.rate.p;
angular_accel_ref.q = (rate_sp.q - rates_filt.q) * indi_gains.rate.q;
angular_accel_ref.r = (rate_sp.r - rates_filt.r) * indi_gains.rate.r;
g2_times_du = 0.0;
for (i = 0; i < INDI_NUM_ACT; i++) {