diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index af35251065..ca1ae9274e 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -207,12 +207,9 @@ void ModeAuto::takeoff_start(const Location& dest_loc) } // sanity check target - if (alt_target < copter.current_loc.alt) { - dest.set_alt_cm(copter.current_loc.alt, Location::AltFrame::ABOVE_HOME); - } - // Note: if taking off from below home this could cause a climb to an unexpectedly high altitude - if (alt_target < 100) { - dest.set_alt_cm(100, Location::AltFrame::ABOVE_HOME); + float alt_target_min_cm = copter.current_loc.alt + (copter.ap.land_complete ? 100 : 0); + if (alt_target < alt_target_min_cm ) { + dest.set_alt_cm(alt_target_min_cm , Location::AltFrame::ABOVE_HOME); } // set waypoint controller target