Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-09 22:49:53 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

Reorganize INDI simple for better takeoff (#2688)

Browse Source
This commit is contained in:
Ewoud Smeur
2021-04-08 20:24:46 +02:00
committed by GitHub
parent 2cd3c38d80
commit 029947494a
1 changed files with 24 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.c
+24 -22
View File
@@ -295,6 +295,12 @@ static inline void finite_difference(float output[3], float new[3], float old[3]
*/
void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight __attribute__((unused)))
{
//Propagate input filters
//first order actuator dynamics
indi.u_act_dyn.p = indi.u_act_dyn.p + STABILIZATION_INDI_ACT_DYN_P * (indi.u_in.p - indi.u_act_dyn.p);
indi.u_act_dyn.q = indi.u_act_dyn.q + STABILIZATION_INDI_ACT_DYN_Q * (indi.u_in.q - indi.u_act_dyn.q);
indi.u_act_dyn.r = indi.u_act_dyn.r + STABILIZATION_INDI_ACT_DYN_R * (indi.u_in.r - indi.u_act_dyn.r);
// Propagate the filter on the gyroscopes and actuators
struct FloatRates *body_rates = stateGetBodyRates_f();
filter_pqr(indi.u, &indi.u_act_dyn);
@@ -339,10 +345,24 @@ void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight __att
indi.du.q = 1.0 / indi.g1.q * (indi.angular_accel_ref.q - indi.rate_d[1]);
indi.du.r = 1.0 / (indi.g1.r + indi.g2) * (indi.angular_accel_ref.r - indi.rate_d[2] + indi.g2 * indi.du.r);
//add the increment to the total control input
indi.u_in.p = indi.u[0].o[0] + indi.du.p;
indi.u_in.q = indi.u[1].o[0] + indi.du.q;
indi.u_in.r = indi.u[2].o[0] + indi.du.r;
//Don't increment if thrust is off and on the ground
//without this the inputs will increment to the maximum before even getting in the air.
if (stabilization_cmd[COMMAND_THRUST] < 300 && !in_flight) {
FLOAT_RATES_ZERO(indi.u_in);
// If on the gournd, no increments, just proportional control
indi.u_in.p = indi.du.p;
indi.u_in.q = indi.du.q;
indi.u_in.r = indi.du.r;
} else {
//add the increment to the total control input
indi.u_in.p = indi.u[0].o[0] + indi.du.p;
indi.u_in.q = indi.u[1].o[0] + indi.du.q;
indi.u_in.r = indi.u[2].o[0] + indi.du.r;
// only run the estimation if the commands are not zero.
lms_estimation();
}
//bound the total control input
#if STABILIZATION_INDI_FULL_AUTHORITY
@@ -357,24 +377,6 @@ void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight __att
Bound(indi.u_in.r, -4500, 4500);
#endif
//Propagate input filters
//first order actuator dynamics
indi.u_act_dyn.p = indi.u_act_dyn.p + STABILIZATION_INDI_ACT_DYN_P * (indi.u_in.p - indi.u_act_dyn.p);
indi.u_act_dyn.q = indi.u_act_dyn.q + STABILIZATION_INDI_ACT_DYN_Q * (indi.u_in.q - indi.u_act_dyn.q);
indi.u_act_dyn.r = indi.u_act_dyn.r + STABILIZATION_INDI_ACT_DYN_R * (indi.u_in.r - indi.u_act_dyn.r);
//Don't increment if thrust is off
//TODO: this should be something more elegant, but without this the inputs
//will increment to the maximum before even getting in the air.
if (stabilization_cmd[COMMAND_THRUST] < 300) {
FLOAT_RATES_ZERO(indi.du);
FLOAT_RATES_ZERO(indi.u_act_dyn);
FLOAT_RATES_ZERO(indi.u_in);
} else {
// only run the estimation if the commands are not zero.
lms_estimation();
}
/* INDI feedback */
stabilization_cmd[COMMAND_ROLL] = indi.u_in.p;
stabilization_cmd[COMMAND_PITCH] = indi.u_in.q;