From 53b8a78500a68d0a0619b2de6ec0b583d5cbe340 Mon Sep 17 00:00:00 2001 From: Ewoud Smeur Date: Mon, 30 May 2016 23:15:33 +0200 Subject: [PATCH] added the possibility to add a yaw rate limit to stabilization indi (#1684) the yaw of a typical quadrotor easily saturates. One of the problems that occur when asking for a large change in yaw angle, is that the quadrotor can build up so much angular velocity in the yaw axis, that it can not physically slow down before reaching the yaw setpoint due to saturating. This will result in overshoot. This can be helped by adding a yaw rate limit, which this PR does for the INDI controller. --- conf/airframes/TUDELFT/tudelft_bebop_indi.xml | 5 ++- conf/settings/control/stabilization_indi.xml | 3 +- .../stabilization/stabilization_indi.c | 40 ++++++++++++------- .../stabilization/stabilization_indi.h | 3 +- 4 files changed, 33 insertions(+), 18 deletions(-) diff --git a/conf/airframes/TUDELFT/tudelft_bebop_indi.xml b/conf/airframes/TUDELFT/tudelft_bebop_indi.xml index bea1065a23..74bd47bd05 100644 --- a/conf/airframes/TUDELFT/tudelft_bebop_indi.xml +++ b/conf/airframes/TUDELFT/tudelft_bebop_indi.xml @@ -100,7 +100,7 @@ - + @@ -139,6 +139,9 @@ + + + diff --git a/conf/settings/control/stabilization_indi.xml b/conf/settings/control/stabilization_indi.xml index bd73eb2301..4a7e550136 100644 --- a/conf/settings/control/stabilization_indi.xml +++ b/conf/settings/control/stabilization_indi.xml @@ -15,7 +15,8 @@ - + + diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c index dc283914c4..6eb4076e90 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c @@ -71,6 +71,10 @@ #warning "Use caution with adaptive indi. See the wiki for more info" #endif +#ifndef STABILIZATION_INDI_MAX_R +#define STABILIZATION_INDI_MAX_R STABILIZATION_ATTITUDE_SP_MAX_R +#endif + struct Int32Eulers stab_att_sp_euler; struct Int32Quat stab_att_sp_quat; @@ -83,6 +87,7 @@ static inline void lms_estimation(void); #define INDI_EST_SCALE 0.001 //The G values are scaled to avoid numerical problems during the estimation struct IndiVariables indi = { .max_rate = STABILIZATION_INDI_MAX_RATE, + .attitude_max_yaw_rate = STABILIZATION_INDI_MAX_R, .g1 = {STABILIZATION_INDI_G1_P, STABILIZATION_INDI_G1_Q, STABILIZATION_INDI_G1_R}, .g2 = STABILIZATION_INDI_G2_R, @@ -206,28 +211,33 @@ static inline void stabilization_indi_calc_cmd(int32_t indi_commands[], struct I struct FloatRates *body_rates = stateGetBodyRates_f(); stabilization_indi_second_order_filter(&indi.rate, body_rates); + //The rates used for feedback are by default the measured rates. If needed they can be filtered (see below) + struct FloatRates rates_for_feedback; + RATES_COPY(rates_for_feedback, (*body_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 can not be as aggressive. #if STABILIZATION_INDI_FILTER_ROLL_RATE - indi.angular_accel_ref.p = indi.reference_acceleration.err_p * QUAT1_FLOAT_OF_BFP(att_err->qx) - - indi.reference_acceleration.rate_p * indi.rate.x.p; -#else - indi.angular_accel_ref.p = indi.reference_acceleration.err_p * QUAT1_FLOAT_OF_BFP(att_err->qx) - - indi.reference_acceleration.rate_p * body_rates->p; + rates_for_feedback.p = indi.rate.x.p; #endif #if STABILIZATION_INDI_FILTER_PITCH_RATE - indi.angular_accel_ref.q = indi.reference_acceleration.err_q * QUAT1_FLOAT_OF_BFP(att_err->qy) - - indi.reference_acceleration.rate_q * indi.rate.x.q; -#else - indi.angular_accel_ref.q = indi.reference_acceleration.err_q * QUAT1_FLOAT_OF_BFP(att_err->qy) - - indi.reference_acceleration.rate_q * body_rates->q; + rates_for_feedback.q = indi.rate.x.q; #endif #if STABILIZATION_INDI_FILTER_YAW_RATE - indi.angular_accel_ref.r = indi.reference_acceleration.err_r * QUAT1_FLOAT_OF_BFP(att_err->qz) - - indi.reference_acceleration.rate_r * indi.rate.x.r; -#else - indi.angular_accel_ref.r = indi.reference_acceleration.err_r * QUAT1_FLOAT_OF_BFP(att_err->qz) - - indi.reference_acceleration.rate_r * body_rates->r; + rates_for_feedback.r = indi.rate.x.r; #endif + indi.angular_accel_ref.p = indi.reference_acceleration.err_p * QUAT1_FLOAT_OF_BFP(att_err->qx) + - indi.reference_acceleration.rate_p * rates_for_feedback.p; + + indi.angular_accel_ref.q = indi.reference_acceleration.err_q * QUAT1_FLOAT_OF_BFP(att_err->qy) + - indi.reference_acceleration.rate_q * rates_for_feedback.q; + + //This separates the P and D controller and lets you impose a maximum yaw rate. + float rate_ref_r = indi.reference_acceleration.err_r * QUAT1_FLOAT_OF_BFP(att_err->qz)/indi.reference_acceleration.rate_r; + BoundAbs(rate_ref_r, indi.attitude_max_yaw_rate); + indi.angular_accel_ref.r = indi.reference_acceleration.rate_r * (rate_ref_r - rates_for_feedback.r); + /* Check if we are running the rate controller and overwrite */ if(rate_control) { indi.angular_accel_ref.p = indi.reference_acceleration.rate_p * ((float)radio_control.values[RADIO_ROLL] / MAX_PPRZ * indi.max_rate - body_rates->p); diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.h b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.h index c703f50de8..1b211888f5 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.h +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.h @@ -82,7 +82,8 @@ struct IndiVariables { struct ReferenceSystem reference_acceleration; bool adaptive; ///< Enable adataptive estimation - float max_rate; ///< Maximum rate in rate control + float max_rate; ///< Maximum rate in rate control in rad/s + float attitude_max_yaw_rate; ///< Maximum yaw rate in atttiude control in rad/s struct IndiEstimation est; ///< Estimation parameters for adaptive INDI };