From a24b7bcf14cc2923f4422e07ce9a463ff5e58550 Mon Sep 17 00:00:00 2001 From: Adam M Rivera Date: Sun, 15 Apr 2012 16:54:43 -0500 Subject: [PATCH] ArduCopter: Added check to g.rtl_approach_alt, which if greater than zero will initiate an approach to the target alt rather than land. --- ArduCopter/ArduCopter.pde | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index e477fdedef..9cab558b0e 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1814,8 +1814,11 @@ static void update_navigation() wp_control = LOITER_MODE; } + if(g.rtl_approach_alt > 0){ + set_mode(APPROACH); + } // Kick us out of loiter and begin landing if the auto_land_timer is set - if(auto_land_timer != 0 && (millis() - auto_land_timer) > (uint32_t)g.auto_land_timeout.get()){ + else if(auto_land_timer != 0 && (millis() - auto_land_timer) > (uint32_t)g.auto_land_timeout.get()){ // just to make sure we clear the timer auto_land_timer = 0; set_mode(LAND); @@ -1835,6 +1838,11 @@ static void update_navigation() update_nav_wp(); break; + case APPROACH: + // calculates the desired Roll and Pitch + update_nav_wp(); + break; + case CIRCLE: yaw_tracking = MAV_ROI_WPNEXT; wp_control = CIRCLE_MODE;