diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 84abb729f4..10f1dc442e 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1415,8 +1415,8 @@ void update_roll_pitch_mode(void) switch(roll_pitch_mode){ case ROLL_PITCH_ACRO: // ACRO does not get SIMPLE mode ability - g.rc_1.servo_out = get_rate_roll(g.rc_1.control_in); - g.rc_2.servo_out = get_rate_pitch(g.rc_2.control_in); + g.rc_1.servo_out = get_acro_roll(g.rc_1.control_in); + g.rc_2.servo_out = get_acro_pitch(g.rc_2.control_in); break; case ROLL_PITCH_STABLE: diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 5942e69583..ed4ea10023 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -80,6 +80,22 @@ get_stabilize_yaw(int32_t target_angle) #endif } +static int +get_acro_roll(int32_t target_rate) +{ + target_rate = target_rate * g.pi_stabilize_roll.kP(); + target_rate = constrain(target_rate, -10000, 10000); + return get_rate_roll(target_rate); +} + +static int +get_acro_pitch(int32_t target_rate) +{ + target_rate = target_rate * g.pi_stabilize_pitch.kP(); + target_rate = constrain(target_rate, -10000, 10000); + return get_rate_pitch(target_rate); +} + static int get_rate_roll(int32_t target_rate) {