diff --git a/ArduCopter/heli_control_acro.pde b/ArduCopter/heli_control_acro.pde index e71746323f..9aae2fd1a4 100644 --- a/ArduCopter/heli_control_acro.pde +++ b/ArduCopter/heli_control_acro.pde @@ -7,6 +7,8 @@ // heli_acro_init - initialise acro controller static bool heli_acro_init(bool ignore_checks) { + // clear stabilized rate errors + attitude_control.init_targets(); return true; } @@ -16,6 +18,12 @@ static void heli_acro_run() { int16_t target_roll, target_pitch, target_yaw; + // if not armed or main rotor not up to full speed clear stabilized rate errors + // unlike multicopters we do not set throttle (i.e. collective pitch) to zero so the swash servos move + if(!motors.armed() || !motors.motor_runup_complete()) { + attitude_control.init_targets(); + } + // To-Do: add support for flybarred helis // convert the input to the desired body frame rate