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 28 Packages Projects Releases Wiki Activity

Merge pull request #171 from flixr/supervision_fix

Browse Source 
Supervision fix
This commit is contained in:
Felix Ruess
2012-03-27 11:00:58 -07:00
parent f91af0e886 a7e7d94e7a
commit 1255133b12
1 changed files with 5 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/actuators/supervision.c
+5 -4
View File
@@ -61,9 +61,10 @@
/** total supervision command scale.
* scales a command input [-MAX_PPRZ,MAX_PPRZ]
* to the final supervision motor command with range of
* [0,SUPERVISION_MAX_MOTOR-SUPERVISION_MIN_MOTOR]
* [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)
#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;
@@ -152,12 +153,12 @@ void supervision_run(bool_t motors_on, bool_t override_on, int32_t in_cmd[] ) {
int32_t min_cmd = INT32_MAX;
int32_t max_cmd = INT32_MIN;
for (i=0; i<SUPERVISION_NB_MOTOR; i++) {
supervision.commands[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_MIN_MOTOR) * SUPERVISION_CMD_SCALE;
supervision.trim[i]) * SUPERVISION_CMD_SCALE;
if (supervision.commands[i] < min_cmd)
min_cmd = supervision.commands[i];
if (supervision.commands[i] > max_cmd)