From 4b54887c6c55e47e5645736813a9505986dd59f8 Mon Sep 17 00:00:00 2001 From: Jason Short Date: Tue, 10 Jan 2012 23:40:39 -0800 Subject: [PATCH] Switched to set_next_WP for mode switches --- ArduCopter/system.pde | 14 +++++--------- 1 file changed, 5 insertions(+), 9 deletions(-) diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 2c4bc8f626..b518400297 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -321,7 +321,7 @@ static void init_ardupilot() #endif // initialise sonar - #if HIL_MODE != HIL_MODE_ATTITUDE && CONFIG_SONAR == ENABLED + #if CONFIG_SONAR == ENABLED init_sonar(); #endif @@ -490,9 +490,7 @@ static void set_mode(byte mode) roll_pitch_mode = ALT_HOLD_RP; throttle_mode = ALT_HOLD_THR; - next_WP = current_loc; - // 1m is the alt hold limit - next_WP.alt = max(next_WP.alt, 100); + set_next_WP(¤t_loc); break; case AUTO: @@ -508,9 +506,7 @@ static void set_mode(byte mode) yaw_mode = CIRCLE_YAW; roll_pitch_mode = CIRCLE_RP; throttle_mode = CIRCLE_THR; - next_WP = current_loc; - - // reset the desired circle angle + set_next_WP(¤t_loc); circle_angle = 0; break; @@ -518,14 +514,14 @@ static void set_mode(byte mode) yaw_mode = LOITER_YAW; roll_pitch_mode = LOITER_RP; throttle_mode = LOITER_THR; - next_WP = current_loc; + set_next_WP(¤t_loc); break; case POSITION: yaw_mode = YAW_HOLD; roll_pitch_mode = ROLL_PITCH_AUTO; throttle_mode = THROTTLE_MANUAL; - next_WP = current_loc; + set_next_WP(¤t_loc); break; case GUIDED: