Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-04 13:55:40 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

Merge pull request #588 from podhrmic/master

Browse Source 
[stabilization] Float euler bounding and scaling fix

you need to divide the old dd-gain by 512 and the old i-gain by 1024.
This commit is contained in:
Felix Ruess
2013-11-18 02:45:26 -08:00
parent 96d7347f4a 6e5469d0cd
commit f6a720f18c
1 changed files with 14 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c
+14 -10
View File
@@ -152,7 +152,7 @@ void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t head
stab_att_sp_euler.psi = ANGLE_FLOAT_OF_BFP(heading);
}
#define MAX_SUM_ERR RadOfDeg(56000)
#define MAX_SUM_ERR 200
void stabilization_attitude_run(bool_t in_flight) {
@@ -160,11 +160,11 @@ void stabilization_attitude_run(bool_t in_flight) {
/* Compute feedforward */
stabilization_att_ff_cmd[COMMAND_ROLL] =
stabilization_gains.dd.x * stab_att_ref_accel.p / 32.;
stabilization_gains.dd.x * stab_att_ref_accel.p;
stabilization_att_ff_cmd[COMMAND_PITCH] =
stabilization_gains.dd.y * stab_att_ref_accel.q / 32.;
stabilization_gains.dd.y * stab_att_ref_accel.q;
stabilization_att_ff_cmd[COMMAND_YAW] =
stabilization_gains.dd.z * stab_att_ref_accel.r / 32.;
stabilization_gains.dd.z * stab_att_ref_accel.r;
/* Compute feedback */
/* attitude error */
@@ -192,24 +192,28 @@ void stabilization_attitude_run(bool_t in_flight) {
stabilization_att_fb_cmd[COMMAND_ROLL] =
stabilization_gains.p.x * att_err.phi +
stabilization_gains.d.x * rate_err.p +
stabilization_gains.i.x * stabilization_att_sum_err.phi / 1024.;
stabilization_gains.i.x * stabilization_att_sum_err.phi;
stabilization_att_fb_cmd[COMMAND_PITCH] =
stabilization_gains.p.y * att_err.theta +
stabilization_gains.d.y * rate_err.q +
stabilization_gains.i.y * stabilization_att_sum_err.theta / 1024.;
stabilization_gains.i.y * stabilization_att_sum_err.theta;
stabilization_att_fb_cmd[COMMAND_YAW] =
stabilization_gains.p.z * att_err.psi +
stabilization_gains.d.z * rate_err.r +
stabilization_gains.i.z * stabilization_att_sum_err.psi / 1024.;
stabilization_gains.i.z * stabilization_att_sum_err.psi;
stabilization_cmd[COMMAND_ROLL] =
(stabilization_att_fb_cmd[COMMAND_ROLL]+stabilization_att_ff_cmd[COMMAND_ROLL])/16.;
(stabilization_att_fb_cmd[COMMAND_ROLL]+stabilization_att_ff_cmd[COMMAND_ROLL]);
stabilization_cmd[COMMAND_PITCH] =
(stabilization_att_fb_cmd[COMMAND_PITCH]+stabilization_att_ff_cmd[COMMAND_PITCH])/16.;
(stabilization_att_fb_cmd[COMMAND_PITCH]+stabilization_att_ff_cmd[COMMAND_PITCH]);
stabilization_cmd[COMMAND_YAW] =
(stabilization_att_fb_cmd[COMMAND_YAW]+stabilization_att_ff_cmd[COMMAND_YAW])/16.;
(stabilization_att_fb_cmd[COMMAND_YAW]+stabilization_att_ff_cmd[COMMAND_YAW]);
/* bound the result */
BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ);
BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ);
BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ);
}