From 313bde515791519a424686c032f73183483d4761 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Tue, 27 Mar 2012 21:43:01 +0200 Subject: [PATCH] rotorcraft stabilization: updated gain scales to get roughly the same gains as before the change of commands * attitude int_quat: multiply old gain scale by 48 (9600/200) * attitude int euler: changed CMD_SHIFT from 16 to 11, so a multiply of 32 to be a bit more conservative for a start * rate: changed the command shift by 5, so also a conservative 32 --- .../stabilization_attitude_euler_int.c | 12 +++++------- .../stabilization_attitude_quat_int.c | 14 +++++++------- .../rotorcraft/stabilization/stabilization_rate.c | 8 ++++---- 3 files changed, 16 insertions(+), 18 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c index 78cd52dda4..a0d42477d3 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c @@ -157,13 +157,11 @@ void stabilization_attitude_run(bool_t in_flight) { OFFSET_AND_ROUND2((stabilization_gains.i.z * stabilization_att_sum_err.psi), 10); - - //FIXME: still needed? for what? -#ifdef USE_HELI -#define CMD_SHIFT 12 -#else -#define CMD_SHIFT 16 -#endif + /* with P gain of 100, att_err of 180deg (3.14 rad) + * fb cmd: 100 * 3.14 * 2^12 / 2^CMD_SHIFT = 628 + * max possible command is 9600 + */ +#define CMD_SHIFT 11 /* sum feedforward and feedback */ stabilization_cmd[COMMAND_ROLL] = diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c index 2c4b8aefdd..8670c7e4b6 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c @@ -63,10 +63,10 @@ int32_t stabilization_att_fb_cmd[COMMANDS_NB]; int32_t stabilization_att_ff_cmd[COMMANDS_NB]; #define IERROR_SCALE 1024 -#define GAIN_PRESCALER_FF 1 -#define GAIN_PRESCALER_P 1 -#define GAIN_PRESCALER_D 1 -#define GAIN_PRESCALER_I 1 +#define GAIN_PRESCALER_FF 48 +#define GAIN_PRESCALER_P 48 +#define GAIN_PRESCALER_D 48 +#define GAIN_PRESCALER_I 48 void stabilization_attitude_init(void) { @@ -96,9 +96,9 @@ static void attitude_run_ff(int32_t ff_commands[], struct Int32AttitudeGains *ga { /* Compute feedforward based on reference acceleration */ - ff_commands[COMMAND_ROLL] = GAIN_PRESCALER_FF * gains->dd.x * RATE_FLOAT_OF_BFP(ref_accel->p) / (1 << 7); - ff_commands[COMMAND_PITCH] = GAIN_PRESCALER_FF * gains->dd.y * RATE_FLOAT_OF_BFP(ref_accel->q) / (1 << 7); - ff_commands[COMMAND_YAW] = GAIN_PRESCALER_FF * gains->dd.z * RATE_FLOAT_OF_BFP(ref_accel->r) / (1 << 7); + ff_commands[COMMAND_ROLL] = GAIN_PRESCALER_FF * gains->dd.x * RATE_FLOAT_OF_BFP(ref_accel->p) / (1 << 7); + ff_commands[COMMAND_PITCH] = GAIN_PRESCALER_FF * gains->dd.y * RATE_FLOAT_OF_BFP(ref_accel->q) / (1 << 7); + ff_commands[COMMAND_YAW] = GAIN_PRESCALER_FF * gains->dd.z * RATE_FLOAT_OF_BFP(ref_accel->r) / (1 << 7); } static void attitude_run_fb(int32_t fb_commands[], struct Int32AttitudeGains *gains, struct Int32Quat *att_err, diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c index 8cec7ab503..79c2328e3a 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c @@ -183,7 +183,7 @@ void stabilization_rate_run(bool_t in_flight) { RATES_ADD(stabilization_rate_ref, _delta_ref); /* compute feed-forward command */ - RATES_EWMULT_RSHIFT(stabilization_rate_ff_cmd, stabilization_rate_ddgain, stabilization_rate_refdot, 14); + RATES_EWMULT_RSHIFT(stabilization_rate_ff_cmd, stabilization_rate_ddgain, stabilization_rate_refdot, 9); /* compute feed-back command */ @@ -213,9 +213,9 @@ void stabilization_rate_run(bool_t in_flight) { stabilization_rate_fb_cmd.r = stabilization_rate_gain.r * _error.r + OFFSET_AND_ROUND2((stabilization_rate_igain.r * stabilization_rate_sum_err.r), 10); - stabilization_rate_fb_cmd.p = stabilization_rate_fb_cmd.p >> 16; - stabilization_rate_fb_cmd.q = stabilization_rate_fb_cmd.q >> 16; - stabilization_rate_fb_cmd.r = stabilization_rate_fb_cmd.r >> 16; + stabilization_rate_fb_cmd.p = stabilization_rate_fb_cmd.p >> 11; + stabilization_rate_fb_cmd.q = stabilization_rate_fb_cmd.q >> 11; + stabilization_rate_fb_cmd.r = stabilization_rate_fb_cmd.r >> 11; /* sum to final command */ stabilization_cmd[COMMAND_ROLL] = stabilization_rate_ff_cmd.p + stabilization_rate_fb_cmd.p;