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

supervision scaling in fixedpoint, should work up to a supervision scale of 2^31/(4*9600)=55924

Browse Source
This commit is contained in:
Felix Ruess
2012-04-03 16:46:41 +02:00
parent 49d331f3ac
commit 2b0059a23f
1 changed files with 5 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/actuators/supervision.c
+5 -5
View File
@@ -147,11 +147,11 @@ void supervision_run(bool_t motors_on, bool_t override_on, int32_t in_cmd[] ) {
/* do the mixing in float to avoid overflows, implicitly casted back to int32_t */ /* do the mixing in float to avoid overflows, implicitly casted back to int32_t */
for (i=0; i<SUPERVISION_NB_MOTOR; i++) { for (i=0; i<SUPERVISION_NB_MOTOR; i++) {
supervision.commands[i] = SUPERVISION_MIN_MOTOR + supervision.commands[i] = SUPERVISION_MIN_MOTOR +
(float)(thrust_coef[i] * in_cmd[COMMAND_THRUST] + (thrust_coef[i] * in_cmd[COMMAND_THRUST] +
roll_coef[i] * in_cmd[COMMAND_ROLL] + roll_coef[i] * in_cmd[COMMAND_ROLL] +
pitch_coef[i] * in_cmd[COMMAND_PITCH] + pitch_coef[i] * in_cmd[COMMAND_PITCH] +
yaw_coef[i] * in_cmd[COMMAND_YAW] + yaw_coef[i] * in_cmd[COMMAND_YAW] +
supervision.trim[i]) / SUPERVISION_SCALE * supervision.trim[i]) / SUPERVISION_SCALE *
(SUPERVISION_MAX_MOTOR - SUPERVISION_MIN_MOTOR) / MAX_PPRZ; (SUPERVISION_MAX_MOTOR - SUPERVISION_MIN_MOTOR) / MAX_PPRZ;
if (supervision.commands[i] < min_cmd) if (supervision.commands[i] < min_cmd)
min_cmd = supervision.commands[i]; min_cmd = supervision.commands[i];