Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-06 07:53:43 +08:00
Code Issues Actions 26 Packages Projects Releases Wiki Activity

rotorcraft stabilization: updated gain scales to get roughly the same gains as before the change of commands

Browse Source 
* 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
This commit is contained in:
Felix Ruess
2012-03-27 21:43:01 +02:00
parent 1255133b12
commit 313bde5157
3 changed files with 16 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c
+5 -7
View File
@@ -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] =
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c
+7 -7
View File
@@ -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,
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c
+4 -4
View File
@@ -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;