From a1ba6485d50c597c787e361bba1acdfd4f982fe0 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Thu, 22 Mar 2012 17:30:38 +0100 Subject: [PATCH 1/2] rotorcraft commands and supervision: stabilization_cmd and inputs to supervision_run with a range of [-MAX_PPRZ,MAX_PPRZ] --- .../actuators/actuators_pwm_supervision.c | 24 ++++++------------- .../rotorcraft/actuators/supervision.c | 13 +++++++--- .../rotorcraft/guidance/guidance_v.c | 7 +++--- .../rotorcraft/guidance/guidance_v_adpt.h | 14 ++++++----- .../stabilization/stabilization_none.c | 21 ++++------------ 5 files changed, 33 insertions(+), 46 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/actuators/actuators_pwm_supervision.c b/sw/airborne/firmwares/rotorcraft/actuators/actuators_pwm_supervision.c index 424f832ed9c..d794984c8b9 100644 --- a/sw/airborne/firmwares/rotorcraft/actuators/actuators_pwm_supervision.c +++ b/sw/airborne/firmwares/rotorcraft/actuators/actuators_pwm_supervision.c @@ -35,8 +35,9 @@ #define actuators actuators_pwm_values #define Actuator(_x) actuators_pwm_values[_x] -#define ActuatorsCommit() do { } while(0); +#define ActuatorsCommit() {} +/** actuator PWM values in usec. */ int32_t actuators_pwm_values[ACTUATORS_PWM_NB]; void actuators_init(void) @@ -45,27 +46,16 @@ void actuators_init(void) actuators_pwm_arch_init(); } -#define PWM_GAIN_SCALE 2 - void actuators_set(bool_t motors_on) { - int32_t pwm_commands[COMMANDS_NB]; - int32_t pwm_commands_pprz[COMMANDS_NB]; - - pwm_commands[COMMAND_ROLL] = commands[COMMAND_ROLL] * PWM_GAIN_SCALE; - pwm_commands[COMMAND_PITCH] = commands[COMMAND_PITCH] * PWM_GAIN_SCALE; - pwm_commands[COMMAND_YAW] = commands[COMMAND_YAW] * PWM_GAIN_SCALE; - pwm_commands[COMMAND_THRUST] = (commands[COMMAND_THRUST] * ((SUPERVISION_MAX_MOTOR - SUPERVISION_MIN_MOTOR) / 200)) + SUPERVISION_MIN_MOTOR; - - pwm_commands_pprz[COMMAND_ROLL] = commands[COMMAND_ROLL] * (MAX_PPRZ / 100); - pwm_commands_pprz[COMMAND_PITCH] = commands[COMMAND_PITCH] * (MAX_PPRZ / 100); - pwm_commands_pprz[COMMAND_YAW] = commands[COMMAND_YAW] * (MAX_PPRZ / 100); - supervision_run(motors_on, FALSE, pwm_commands); + /* set normal control surface actuators, i.e. servos */ + SetActuatorsFromCommands(commands); - SetActuatorsFromCommands(pwm_commands_pprz); + /* run supervision for actuators (motor controllers) that need mixing */ + supervision_run(motors_on, FALSE, commands); for (int i = 0; i < SUPERVISION_NB_MOTOR; i++) { - actuators_pwm_values[i] = supervision.commands[i]; + actuators_pwm_values[i] = supervision.commands[i]; } actuators_pwm_commit(); diff --git a/sw/airborne/firmwares/rotorcraft/actuators/supervision.c b/sw/airborne/firmwares/rotorcraft/actuators/supervision.c index a08ad59512b..41fb17c3cf5 100644 --- a/sw/airborne/firmwares/rotorcraft/actuators/supervision.c +++ b/sw/airborne/firmwares/rotorcraft/actuators/supervision.c @@ -58,6 +58,13 @@ */ #endif +/** 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] + */ +#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; @@ -150,7 +157,7 @@ void supervision_run(bool_t motors_on, bool_t override_on, int32_t in_cmd[] ) { 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.trim[i] + SUPERVISION_MIN_MOTOR) * SUPERVISION_CMD_SCALE; if (supervision.commands[i] < min_cmd) min_cmd = supervision.commands[i]; if (supervision.commands[i] > max_cmd) @@ -166,8 +173,8 @@ void supervision_run(bool_t motors_on, bool_t override_on, int32_t in_cmd[] ) { /* For testing motor failure */ if (motors_on && override_on) { for (i = 0; i < SUPERVISION_NB_MOTOR; i++) { - if (supervision.override_enabled[i]) - supervision.commands[i] = supervision.override_value[i]; + if (supervision.override_enabled[i]) + supervision.commands[i] = supervision.override_value[i]; } } bound_commands(); diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c index b8664b05fd3..e52b67351df 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c @@ -97,9 +97,10 @@ void guidance_v_init(void) { void guidance_v_read_rc(void) { - // used in RC_DIRECT directly and as saturation in CLIMB and HOVER - guidance_v_rc_delta_t = (int32_t)radio_control.values[RADIO_THROTTLE] * (SUPERVISION_MAX_MOTOR - SUPERVISION_MIN_MOTOR) / MAX_PPRZ; - // used in RC_CLIMB + /* used in RC_DIRECT directly and as saturation in CLIMB and HOVER */ + guidance_v_rc_delta_t = (int32_t)radio_control.values[RADIO_THROTTLE]; + + /* used in RC_CLIMB */ guidance_v_rc_zd_sp = ((MAX_PPRZ/2) - (int32_t)radio_control.values[RADIO_THROTTLE]) * GUIDANCE_V_RC_CLIMB_COEF; DeadBand(guidance_v_rc_zd_sp, GUIDANCE_V_RC_CLIMB_DEAD_BAND); diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adpt.h b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adpt.h index 80395279848..8b5cf0491c1 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adpt.h +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adpt.h @@ -30,6 +30,8 @@ #ifndef GUIDANCE_V_ADPT #define GUIDANCE_V_ADPT +#include "paparazzi.h" + /** Adapt noise factor. * Smaller values will make the filter to adapter faster * Bigger values (slower adaptation) make the filter more robust to external perturbations @@ -46,7 +48,7 @@ #define GUIDANCE_V_ADAPT_MAX_ACCEL 4.0 #endif -/** Filter is not fed if command values are out of a % of MIN/MAX_SUPERVISION +/** Filter is not fed if command values are out of a % of 0/MAX_PPRZ * MAX_CMD and MIN_CMD must be between 0 and 1 with MIN_CMD < MAX_CMD */ #ifndef GUIDANCE_V_ADAPT_MAX_CMD @@ -100,8 +102,8 @@ int32_t gv_adapt_Xmeas; #define GV_ADAPT_MAX_ACCEL ACCEL_BFP_OF_REAL(GUIDANCE_V_ADAPT_MAX_ACCEL) /* Command bounds */ -#define GV_ADAPT_MAX_CMD ((int32_t)Blend(SUPERVISION_MAX_MOTOR, SUPERVISION_MIN_MOTOR, GUIDANCE_V_ADAPT_MAX_CMD)) -#define GV_ADAPT_MIN_CMD ((int32_t)Blend(SUPERVISION_MAX_MOTOR, SUPERVISION_MIN_MOTOR, GUIDANCE_V_ADAPT_MIN_CMD)) +#define GV_ADAPT_MAX_CMD ((int32_t)(GUIDANCE_V_ADAPT_MAX_CMD*MAX_PPRZ)) +#define GV_ADAPT_MIN_CMD ((int32_t)(GUIDANCE_V_ADAPT_MIN_CMD*MAX_PPRZ)) /* Output bounds. * Don't let it climb over a value that would @@ -114,8 +116,8 @@ int32_t gv_adapt_Xmeas; * 9.81*2^18/1 = 2571632 */ // TODO Check this properly -#define GV_ADAPT_MAX_OUT (BFP_OF_REAL(9.81, GV_ADAPT_X_FRAC) / SUPERVISION_MIN_MOTOR) -#define GV_ADAPT_MIN_OUT (BFP_OF_REAL(9.81, GV_ADAPT_X_FRAC) / SUPERVISION_MAX_MOTOR) +#define GV_ADAPT_MAX_OUT (BFP_OF_REAL(9.81, GV_ADAPT_X_FRAC)) +#define GV_ADAPT_MIN_OUT (BFP_OF_REAL(9.81, GV_ADAPT_X_FRAC) / MAX_PPRZ) static inline void gv_adapt_init(void) { @@ -127,7 +129,7 @@ static inline void gv_adapt_init(void) { /** Adaptation function. * @param zdd_meas vert accel measurement in m/s^2 with #INT32_ACCEL_FRAC - * @param thrust_applied controller input [SUPERVISION_MIN_MOTOR, SUPERVISION_MAX_MOTOR] + * @param thrust_applied controller input [0 : MAX_PPRZ] * @param zd_ref vertical speed reference in m/s with #INT32_SPEED_FRAC */ static inline void gv_adapt_run(int32_t zdd_meas, int32_t thrust_applied, int32_t zd_ref) { diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_none.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_none.c index cd78cdb9b8b..e341501641d 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_none.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_none.c @@ -32,17 +32,6 @@ #include "subsystems/radio_control.h" #include "generated/airframe.h" -#define F_UPDATE_RES 9 -#define REF_DOT_FRAC 11 -#define REF_FRAC 16 - -#ifndef SUPERVISION_SCALE -#define SUPERVISION_SCALE MAX_PPRZ -#endif - -#define OFFSET_AND_ROUND(_a, _b) (((_a)+(1<<((_b)-1)))>>(_b)) -#define OFFSET_AND_ROUND2(_a, _b) (((_a)+(1<<((_b)-1))-((_a)<0?1:0))>>(_b)) - struct Int32Rates stabilization_none_rc_cmd; void stabilization_none_init(void) { @@ -60,10 +49,8 @@ void stabilization_none_enter(void) { } void stabilization_none_run(bool_t in_flight) { - - /* sum to final command */ - stabilization_cmd[COMMAND_ROLL] = stabilization_none_rc_cmd.p * SUPERVISION_SCALE / MAX_PPRZ; - stabilization_cmd[COMMAND_PITCH] = stabilization_none_rc_cmd.q * SUPERVISION_SCALE / MAX_PPRZ; - stabilization_cmd[COMMAND_YAW] = stabilization_none_rc_cmd.r * SUPERVISION_SCALE / MAX_PPRZ; - + /* just directly pass rc commands through */ + stabilization_cmd[COMMAND_ROLL] = stabilization_none_rc_cmd.p; + stabilization_cmd[COMMAND_PITCH] = stabilization_none_rc_cmd.q; + stabilization_cmd[COMMAND_YAW] = stabilization_none_rc_cmd.r; } From 1905e350c427e7c7aa7a821b73808371511b064d Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Fri, 23 Mar 2012 15:17:45 +0100 Subject: [PATCH 2/2] rotorcraft stabilization: bound stabilization_cmd to MAX_PPRZ --- sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c | 3 +++ .../stabilization/stabilization_attitude_euler_int.c | 7 +++++++ .../stabilization/stabilization_attitude_quat_int.c | 6 +++--- .../rotorcraft/stabilization/stabilization_rate.c | 5 +++++ 4 files changed, 18 insertions(+), 3 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c index e52b67351df..51a62ecd79c 100644 --- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c +++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c @@ -274,4 +274,7 @@ __attribute__ ((always_inline)) static inline void run_hover_loop(bool_t in_flig guidance_v_delta_t = guidance_v_ff_cmd + guidance_v_fb_cmd; + /* bound the result */ + Bound(guidance_v_delta_t, 0, MAX_PPRZ); + } diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c index 45249171248..e62f2d5a2f3 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c @@ -143,6 +143,8 @@ void stabilization_attitude_run(bool_t in_flight) { OFFSET_AND_ROUND2((stabilization_gains.i.z * stabilization_att_sum_err.psi), 10); + + //FIXME: still needed? for what? #ifdef USE_HELI #define CMD_SHIFT 12 #else @@ -159,4 +161,9 @@ void stabilization_attitude_run(bool_t in_flight) { stabilization_cmd[COMMAND_YAW] = OFFSET_AND_ROUND((stabilization_att_fb_cmd[COMMAND_YAW]+stabilization_att_ff_cmd[COMMAND_YAW]), CMD_SHIFT); + /* bound the result */ + BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ); + BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ); + BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ); + } diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c index fc8942161ad..75e130386c5 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c @@ -160,9 +160,9 @@ void stabilization_attitude_run(bool_t enable_integrator) { stabilization_cmd[COMMAND_YAW] = stabilization_att_fb_cmd[COMMAND_YAW] + stabilization_att_ff_cmd[COMMAND_YAW]; /* bound the result */ - Bound(stabilization_cmd[COMMAND_ROLL], -200, 200); - Bound(stabilization_cmd[COMMAND_PITCH], -200, 200); - Bound(stabilization_cmd[COMMAND_YAW], -200, 200); + BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ); + BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ); + BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ); } void stabilization_attitude_read_rc(bool_t in_flight) { diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c index 2a43e9ee7c7..6231d2a8944 100644 --- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c +++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c @@ -202,4 +202,9 @@ void stabilization_rate_run(bool_t in_flight) { stabilization_cmd[COMMAND_PITCH] = stabilization_rate_ff_cmd.q + stabilization_rate_fb_cmd.q; stabilization_cmd[COMMAND_YAW] = stabilization_rate_ff_cmd.r + stabilization_rate_fb_cmd.r; + /* bound the result */ + BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ); + BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ); + BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ); + }