Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

rotorcraft stabilization/supervision commands with standard PPRZ range #169

Merged
merged 2 commits into from
Mar 26, 2012
Merged

rotorcraft stabilization/supervision commands with standard PPRZ range #169

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
a1ba648
rotorcraft commands and supervision: stabilization_cmd and inputs to …
flixr Mar 22, 2012
1905e35
rotorcraft stabilization: bound stabilization_cmd to MAX_PPRZ
flixr Mar 23, 2012
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
24 changes: 7 additions & 17 deletions sw/airborne/firmwares/rotorcraft/actuators/actuators_pwm_supervision.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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();

Expand Down
13 changes: 10 additions & 3 deletions sw/airborne/firmwares/rotorcraft/actuators/supervision.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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)
Expand All @@ -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();
Expand Down
10 changes: 7 additions & 3 deletions sw/airborne/firmwares/rotorcraft/guidance/guidance_v.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand Down Expand Up @@ -273,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);

}
14 changes: 8 additions & 6 deletions sw/airborne/firmwares/rotorcraft/guidance/guidance_v_adpt.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand Down Expand Up @@ -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
Expand All @@ -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) {
Expand All @@ -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) {
Expand Down
7 changes: 7 additions & 0 deletions sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_euler_int.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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);

}
6 changes: 3 additions & 3 deletions sw/airborne/firmwares/rotorcraft/stabilization/stabilization_attitude_quat_int.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand Down
21 changes: 4 additions & 17 deletions sw/airborne/firmwares/rotorcraft/stabilization/stabilization_none.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand All @@ -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;
}
5 changes: 5 additions & 0 deletions sw/airborne/firmwares/rotorcraft/stabilization/stabilization_rate.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);

}