zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Copter: Leonard's control_acro fixes 

get_pilot_desired_angle_rates returns bf rate targets as floats
master
Randy Mackay 11 years ago committed by Andrew Tridgell
parent
2c6470f87b
commit
bf6bb59cb4
1 changed files with 56 additions and 56 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 112
      ArduCopter/control_acro.pde

112
ArduCopter/control_acro.pde
Unescape View File

@ -16,7 +16,7 @@ static bool acro_init(bool ignore_checks)
// should be called at 100hz or more // should be called at 100hz or more
static void acro_run() static void acro_run()
{ {
int16_t target_roll, target_pitch, target_yaw; float target_roll, target_pitch, target_yaw;
int16_t pilot_throttle_scaled; int16_t pilot_throttle_scaled;
// if motors not running reset angle targets // if motors not running reset angle targets
@ -42,7 +42,7 @@ static void acro_run()
// get_pilot_desired_angle_rates - transform pilot's roll pitch and yaw input into a desired lean angle rates // get_pilot_desired_angle_rates - transform pilot's roll pitch and yaw input into a desired lean angle rates
// returns desired angle rates in centi-degrees-per-second // returns desired angle rates in centi-degrees-per-second
static void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, int16_t &roll_out, int16_t &pitch_out, int16_t &yaw_out) static void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int16_t yaw_in, float &roll_out, float &pitch_out, float &yaw_out)
{ {
// Calculate trainer mode earth frame rate command for roll // Calculate trainer mode earth frame rate command for roll
@ -57,62 +57,62 @@ static void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int
// calculate earth frame rate corrections to pull the copter back to level while in ACRO mode // calculate earth frame rate corrections to pull the copter back to level while in ACRO mode
// Calculate trainer mode earth frame rate command for roll if (g.acro_trainer != ACRO_TRAINER_DISABLED) {
int32_t roll_angle = wrap_180_cd(ahrs.roll_sensor); // Calculate trainer mode earth frame rate command for roll
roll_angle = constrain_int32(roll_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE); int32_t roll_angle = wrap_180_cd(ahrs.roll_sensor);
rate_ef_level.x = -roll_angle * g.acro_balance_roll; rate_ef_level.x = -constrain_int32(roll_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_roll;
// Calculate trainer mode earth frame rate command for pitch // Calculate trainer mode earth frame rate command for pitch
int32_t pitch_angle = wrap_180_cd(ahrs.pitch_sensor); int32_t pitch_angle = wrap_180_cd(ahrs.pitch_sensor);
pitch_angle = constrain_int32(pitch_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE); rate_ef_level.y = -constrain_int32(pitch_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_pitch;
rate_ef_level.y = -pitch_angle * g.acro_balance_pitch;
// Calculate trainer mode earth frame rate command for yaw
// Calculate trainer mode earth frame rate command for yaw rate_ef_level.z = 0;
rate_ef_level.z = 0;
// Calculate angle limiting earth frame rate commands
// Calculate angle limiting earth frame rate commands if (g.acro_trainer == ACRO_TRAINER_LIMITED) {
if (g.acro_trainer == ACRO_TRAINER_LIMITED) { if (roll_angle > aparm.angle_max){
if (roll_angle > aparm.angle_max){ rate_ef_level.x -= g.pi_stabilize_roll.get_p(roll_angle-aparm.angle_max);
rate_ef_level.x += g.pi_stabilize_roll.get_p(aparm.angle_max-roll_angle); }else if (roll_angle < -aparm.angle_max) {
}else if (roll_angle < -aparm.angle_max) { rate_ef_level.x -= g.pi_stabilize_roll.get_p(roll_angle+aparm.angle_max);
rate_ef_level.x += g.pi_stabilize_roll.get_p(-aparm.angle_max-roll_angle); }
}
if (pitch_angle > aparm.angle_max){
if (pitch_angle > aparm.angle_max){ rate_ef_level.y -= g.pi_stabilize_pitch.get_p(pitch_angle-aparm.angle_max);
rate_ef_level.y += g.pi_stabilize_pitch.get_p(aparm.angle_max-pitch_angle); }else if (pitch_angle < -aparm.angle_max) {
}else if (pitch_angle < -aparm.angle_max) { rate_ef_level.y -= g.pi_stabilize_pitch.get_p(pitch_angle+aparm.angle_max);
rate_ef_level.y += g.pi_stabilize_pitch.get_p(-aparm.angle_max-pitch_angle); }
} }
}
// convert earth-frame level rates to body-frame level rates // convert earth-frame level rates to body-frame level rates
attitude_control.rate_ef_targets_to_bf(rate_ef_level, rate_bf_level); attitude_control.rate_ef_targets_to_bf(rate_ef_level, rate_bf_level);
// combine earth frame rate corrections with rate requests // combine earth frame rate corrections with rate requests
if (g.acro_trainer == ACRO_TRAINER_LIMITED) { if (g.acro_trainer == ACRO_TRAINER_LIMITED) {
rate_bf_request.x += rate_bf_level.x; rate_bf_request.x += rate_bf_level.x;
rate_bf_request.y += rate_bf_level.y; rate_bf_request.y += rate_bf_level.y;
rate_bf_request.z += rate_bf_level.z; rate_bf_request.z += rate_bf_level.z;
}else{ }else{
acro_level_mix = constrain_float(1-max(max(abs(roll_in), abs(pitch_in)), abs(yaw_in))/4500.0, 0, 1)*ahrs.cos_pitch(); acro_level_mix = constrain_float(1-max(max(abs(roll_in), abs(pitch_in)), abs(yaw_in))/4500.0, 0, 1)*ahrs.cos_pitch();
// Scale leveling rates by stick input // Scale leveling rates by stick input
rate_bf_level = rate_bf_level*acro_level_mix; rate_bf_level = rate_bf_level*acro_level_mix;
// Calculate rate limit to prevent change of rate through inverted // Calculate rate limit to prevent change of rate through inverted
rate_limit = fabs(fabs(rate_bf_request.x)-fabs(rate_bf_level.x)); rate_limit = fabs(fabs(rate_bf_request.x)-fabs(rate_bf_level.x));
rate_bf_request.x += rate_bf_level.x; rate_bf_request.x += rate_bf_level.x;
rate_bf_request.x = constrain_float(rate_bf_request.x, -rate_limit, rate_limit); rate_bf_request.x = constrain_float(rate_bf_request.x, -rate_limit, rate_limit);
// Calculate rate limit to prevent change of rate through inverted // Calculate rate limit to prevent change of rate through inverted
rate_limit = fabs(fabs(rate_bf_request.y)-fabs(rate_bf_level.y)); rate_limit = fabs(fabs(rate_bf_request.y)-fabs(rate_bf_level.y));
rate_bf_request.y += rate_bf_level.y; rate_bf_request.y += rate_bf_level.y;
rate_bf_request.y = constrain_float(rate_bf_request.y, -rate_limit, rate_limit); rate_bf_request.y = constrain_float(rate_bf_request.y, -rate_limit, rate_limit);
// Calculate rate limit to prevent change of rate through inverted // Calculate rate limit to prevent change of rate through inverted
rate_limit = fabs(fabs(rate_bf_request.z)-fabs(rate_bf_level.z)); rate_limit = fabs(fabs(rate_bf_request.z)-fabs(rate_bf_level.z));
rate_bf_request.z += rate_bf_level.z; rate_bf_request.z += rate_bf_level.z;
rate_bf_request.z = constrain_float(rate_bf_request.z, -rate_limit, rate_limit); rate_bf_request.z = constrain_float(rate_bf_request.z, -rate_limit, rate_limit);
}
} }
// hand back rate request // hand back rate request

Write Preview
Loading…
Cancel
Save