From 6a9f07ca02473ee12420fb46332a5d9dbf163200 Mon Sep 17 00:00:00 2001 From: Tatsuya Yamaguchi Date: Wed, 26 Jan 2022 11:05:29 +0900 Subject: [PATCH] Copter: fix sanity checks for takeoff altitude --- ArduCopter/mode_auto.cpp | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) 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