From 1abb43905148ec4dfa341440e05deda0544e204b Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 9 Sep 2013 10:54:13 +0900 Subject: [PATCH] Copter: move acro's var initialisation to roll-pitch and yaw controller initialisation --- ArduCopter/ArduCopter.pde | 10 ++++++++++ ArduCopter/system.pde | 4 ---- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index f4741d5bbe..981cafd036 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1431,8 +1431,11 @@ bool set_yaw_mode(uint8_t new_yaw_mode) switch( new_yaw_mode ) { case YAW_HOLD: + yaw_initialised = true; + break; case YAW_ACRO: yaw_initialised = true; + acro_yaw_rate = 0; break; case YAW_LOOK_AT_NEXT_WP: if( ap.home_is_set ) { @@ -1660,7 +1663,14 @@ bool set_roll_pitch_mode(uint8_t new_roll_pitch_mode) switch( new_roll_pitch_mode ) { case ROLL_PITCH_STABLE: + roll_pitch_initialised = true; + break; case ROLL_PITCH_ACRO: + // reset acro level rates + acro_roll_rate = 0; + acro_pitch_rate = 0; + roll_pitch_initialised = true; + break; case ROLL_PITCH_AUTO: case ROLL_PITCH_STABLE_OF: case ROLL_PITCH_TOY: diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index b959e75916..874e4648bf 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -360,10 +360,6 @@ static bool set_mode(uint8_t mode) set_roll_pitch_mode(ACRO_RP); set_throttle_mode(ACRO_THR); set_nav_mode(NAV_NONE); - // reset acro level rates - acro_roll_rate = 0; - acro_pitch_rate = 0; - acro_yaw_rate = 0; break; case STABILIZE: