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

rotorcraft supervision: do the mixing in float to avoid overflows

Browse Source
This commit is contained in:
Felix Ruess
2012-04-01 14:57:57 +02:00
parent 420214a003
commit bdb8d682dd
1 changed files with 7 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/actuators/supervision.c
+7 -13
View File
@@ -58,14 +58,6 @@
*/
#endif
/** total supervision command scale.
* scales a command input [-MAX_PPRZ,MAX_PPRZ]
* to the final supervision motor command with range of
* [SUPERVISION_MIN_MOTOR,SUPERVISION_MAX_MOTOR]
* or sets it to SUPERVISION_STOP_MOTOR
*/
#define SUPERVISION_CMD_SCALE (SUPERVISION_MAX_MOTOR - SUPERVISION_MIN_MOTOR) / (MAX_PPRZ * SUPERVISION_SCALE)
static const int32_t roll_coef[SUPERVISION_NB_MOTOR] = SUPERVISION_ROLL_COEF;
static const int32_t pitch_coef[SUPERVISION_NB_MOTOR] = SUPERVISION_PITCH_COEF;
static const int32_t yaw_coef[SUPERVISION_NB_MOTOR] = SUPERVISION_YAW_COEF;
@@ -152,13 +144,15 @@ void supervision_run(bool_t motors_on, bool_t override_on, int32_t in_cmd[] ) {
if (motors_on) {
int32_t min_cmd = INT32_MAX;
int32_t max_cmd = INT32_MIN;
/* do the mixing in float to avoid overflows, implicitly casted back to int32_t */
for (i=0; i<SUPERVISION_NB_MOTOR; i++) {
supervision.commands[i] = SUPERVISION_MIN_MOTOR +
(thrust_coef[i] * in_cmd[COMMAND_THRUST] +
roll_coef[i] * in_cmd[COMMAND_ROLL] +
pitch_coef[i] * in_cmd[COMMAND_PITCH] +
yaw_coef[i] * in_cmd[COMMAND_YAW] +
supervision.trim[i]) * SUPERVISION_CMD_SCALE;
(float)(thrust_coef[i] * in_cmd[COMMAND_THRUST] +
roll_coef[i] * in_cmd[COMMAND_ROLL] +
pitch_coef[i] * in_cmd[COMMAND_PITCH] +
yaw_coef[i] * in_cmd[COMMAND_YAW] +
supervision.trim[i]) / SUPERVISION_SCALE *
(SUPERVISION_MAX_MOTOR - SUPERVISION_MIN_MOTOR) / MAX_PPRZ;
if (supervision.commands[i] < min_cmd)
min_cmd = supervision.commands[i];
if (supervision.commands[i] > max_cmd)