mirror of https://github.com/ArduPilot/ardupilot
ArduCopter: Added check to g.rtl_approach_alt, which if greater than zero will initiate an approach to the target alt rather than land.
This commit is contained in:
parent
cfe3b58248
commit
a24b7bcf14
|
@ -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;
|
||||
|
|
Loading…
Reference in New Issue