diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 10a2de903c..e30c80c041 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1602,6 +1602,7 @@ bool set_roll_pitch_mode(uint8_t new_roll_pitch_mode) // if initialisation has been successful update the yaw mode if( roll_pitch_initialised ) { + exit_roll_pitch_mode(roll_pitch_mode); roll_pitch_mode = new_roll_pitch_mode; } @@ -1609,6 +1610,17 @@ bool set_roll_pitch_mode(uint8_t new_roll_pitch_mode) return roll_pitch_initialised; } +// exit_roll_pitch_mode - peforms any code required when exiting the current roll-pitch mode +void exit_roll_pitch_mode(uint8_t old_roll_pitch_mode) +{ +#if AUTOTUNE == ENABLED + if (old_roll_pitch_mode == ROLL_PITCH_AUTOTUNE) { + auto_tune_stop(); + } +#endif +} + + // update_roll_pitch_mode - run high level roll and pitch controllers // 100hz update rate void update_roll_pitch_mode(void)