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

[stabilization] Float euler bounding and scaling fix

Browse Source 
- added BoundAbs to properly bound the commands
- removed gain denominators in the control loop (i.e. now the gains should be more consistent with other stabilization systems)

First, it is easier to set your gains now, as the values set by GCS/airframe config are the ones actually used (no division).
Second, the gains are in the same range as for other stabilization systems (no need to change xml settings files for GCS to get proper range).
This commit is contained in:
podhrmic
2013-11-12 15:06:16 -07:00
parent a0fcc68b7e
commit c3b41db698
1 changed files with 13 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_float.c
+13 -9
View File
@@ -160,11 +160,11 @@ void stabilization_attitude_run(bool_t in_flight) {
/* Compute feedforward */ /* Compute feedforward */
stabilization_att_ff_cmd[COMMAND_ROLL] = 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_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_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 */ /* Compute feedback */
/* attitude error */ /* attitude error */
@@ -192,24 +192,28 @@ void stabilization_attitude_run(bool_t in_flight) {
stabilization_att_fb_cmd[COMMAND_ROLL] = stabilization_att_fb_cmd[COMMAND_ROLL] =
stabilization_gains.p.x * att_err.phi + stabilization_gains.p.x * att_err.phi +
stabilization_gains.d.x * rate_err.p + 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_att_fb_cmd[COMMAND_PITCH] =
stabilization_gains.p.y * att_err.theta + stabilization_gains.p.y * att_err.theta +
stabilization_gains.d.y * rate_err.q + 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_att_fb_cmd[COMMAND_YAW] =
stabilization_gains.p.z * att_err.psi + stabilization_gains.p.z * att_err.psi +
stabilization_gains.d.z * rate_err.r + 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_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_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_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);
} }