From bf6bb59cb498d6e4a03fb7af85b056924ab622e6 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 10 Feb 2014 10:26:49 +0900 Subject: [PATCH] Copter: Leonard's control_acro fixes get_pilot_desired_angle_rates returns bf rate targets as floats --- ArduCopter/control_acro.pde | 108 ++++++++++++++++++------------------ 1 file changed, 54 insertions(+), 54 deletions(-) diff --git a/ArduCopter/control_acro.pde b/ArduCopter/control_acro.pde index 4f98697d07..87abe402d1 100644 --- a/ArduCopter/control_acro.pde +++ b/ArduCopter/control_acro.pde @@ -16,7 +16,7 @@ static bool acro_init(bool ignore_checks) // should be called at 100hz or more static void acro_run() { - int16_t target_roll, target_pitch, target_yaw; + float target_roll, target_pitch, target_yaw; int16_t pilot_throttle_scaled; // 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 // 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 @@ -57,64 +57,64 @@ 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 trainer mode earth frame rate command for roll - int32_t roll_angle = wrap_180_cd(ahrs.roll_sensor); - roll_angle = constrain_int32(roll_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE); - rate_ef_level.x = -roll_angle * g.acro_balance_roll; + if (g.acro_trainer != ACRO_TRAINER_DISABLED) { + // Calculate trainer mode earth frame rate command for roll + int32_t roll_angle = wrap_180_cd(ahrs.roll_sensor); + 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 - 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 = -pitch_angle * g.acro_balance_pitch; + // Calculate trainer mode earth frame rate command for pitch + int32_t pitch_angle = wrap_180_cd(ahrs.pitch_sensor); + rate_ef_level.y = -constrain_int32(pitch_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_pitch; - // Calculate trainer mode earth frame rate command for yaw - rate_ef_level.z = 0; - - // Calculate angle limiting earth frame rate commands - if (g.acro_trainer == ACRO_TRAINER_LIMITED) { - if (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) { - rate_ef_level.x += g.pi_stabilize_roll.get_p(-aparm.angle_max-roll_angle); + // Calculate trainer mode earth frame rate command for yaw + rate_ef_level.z = 0; + + // Calculate angle limiting earth frame rate commands + if (g.acro_trainer == ACRO_TRAINER_LIMITED) { + if (roll_angle > aparm.angle_max){ + rate_ef_level.x -= g.pi_stabilize_roll.get_p(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); + } + + if (pitch_angle > aparm.angle_max){ + rate_ef_level.y -= g.pi_stabilize_pitch.get_p(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); + } } - - if (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) { - 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 + attitude_control.rate_ef_targets_to_bf(rate_ef_level, rate_bf_level); + + // combine earth frame rate corrections with rate requests + if (g.acro_trainer == ACRO_TRAINER_LIMITED) { + rate_bf_request.x += rate_bf_level.x; + rate_bf_request.y += rate_bf_level.y; + rate_bf_request.z += rate_bf_level.z; + }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(); + + // Scale leveling rates by stick input + rate_bf_level = rate_bf_level*acro_level_mix; + + // Calculate rate limit to prevent change of rate through inverted + 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 = constrain_float(rate_bf_request.x, -rate_limit, rate_limit); + + // Calculate rate limit to prevent change of rate through inverted + 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 = constrain_float(rate_bf_request.y, -rate_limit, rate_limit); + + // Calculate rate limit to prevent change of rate through inverted + 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 = constrain_float(rate_bf_request.z, -rate_limit, rate_limit); } } - // convert earth-frame level rates to body-frame level rates - attitude_control.rate_ef_targets_to_bf(rate_ef_level, rate_bf_level); - - // combine earth frame rate corrections with rate requests - if (g.acro_trainer == ACRO_TRAINER_LIMITED) { - rate_bf_request.x += rate_bf_level.x; - rate_bf_request.y += rate_bf_level.y; - rate_bf_request.z += rate_bf_level.z; - }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(); - - // Scale leveling rates by stick input - rate_bf_level = rate_bf_level*acro_level_mix; - - // Calculate rate limit to prevent change of rate through inverted - 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 = constrain_float(rate_bf_request.x, -rate_limit, rate_limit); - - // Calculate rate limit to prevent change of rate through inverted - 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 = constrain_float(rate_bf_request.y, -rate_limit, rate_limit); - - // Calculate rate limit to prevent change of rate through inverted - 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 = constrain_float(rate_bf_request.z, -rate_limit, rate_limit); - } - // hand back rate request roll_out = rate_bf_request.x; pitch_out = rate_bf_request.y;