diff --git a/ArduCopter/control_acro.pde b/ArduCopter/control_acro.pde index 87abe402d1..9dac7566a6 100644 --- a/ArduCopter/control_acro.pde +++ b/ArduCopter/control_acro.pde @@ -72,15 +72,15 @@ static void get_pilot_desired_angle_rates(int16_t roll_in, int16_t pitch_in, int // 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); + rate_ef_level.x -= g.acro_rp_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); + rate_ef_level.x -= g.acro_rp_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); + rate_ef_level.y -= g.acro_rp_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); + rate_ef_level.y -= g.acro_rp_p*(pitch_angle+aparm.angle_max); } } diff --git a/ArduCopter/control_sport.pde b/ArduCopter/control_sport.pde index f30cdba823..f79bd519f7 100644 --- a/ArduCopter/control_sport.pde +++ b/ArduCopter/control_sport.pde @@ -28,8 +28,30 @@ static void sport_run() update_simple_mode(); // get pilot's desired roll and pitch rates - target_roll_rate = g.rc_1.control_in; - target_pitch_rate = g.rc_2.control_in; + + // calculate rate requests + target_roll_rate = g.rc_1.control_in * g.acro_rp_p; + target_pitch_rate = g.rc_2.control_in * g.acro_rp_p; + + int32_t roll_angle = wrap_180_cd(ahrs.roll_sensor); + target_roll_rate = -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); + target_pitch_rate = -constrain_int32(pitch_angle, -ACRO_LEVEL_MAX_ANGLE, ACRO_LEVEL_MAX_ANGLE) * g.acro_balance_pitch; + + + if (roll_angle > aparm.angle_max){ + target_roll_rate -= g.acro_rp_p*(roll_angle-aparm.angle_max); + }else if (roll_angle < -aparm.angle_max) { + target_roll_rate -= g.acro_rp_p*(roll_angle+aparm.angle_max); + } + + if (pitch_angle > aparm.angle_max){ + target_pitch_rate -= g.acro_rp_p*(pitch_angle-aparm.angle_max); + }else if (pitch_angle < -aparm.angle_max) { + target_pitch_rate -= g.acro_rp_p*(pitch_angle+aparm.angle_max); + } // get pilot's desired yaw rate target_yaw_rate = get_pilot_desired_yaw_rate(g.rc_4.control_in);