From 553ad877f6f34cf94e174b04b9a6eb4611dfad97 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 11 Mar 2022 13:09:16 +0900 Subject: [PATCH] Copter: fix takeoff to terrain alt --- ArduCopter/mode.h | 2 +- ArduCopter/takeoff.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index d16cf93fdf..721db3a025 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -213,7 +213,7 @@ protected: // auto takeoff variables static float auto_takeoff_start_alt_cm; // start altitude expressed as cm above ekf origin - static float auto_takeoff_complete_alt_cm; // completion altitude expressed as cm above ekf origin + static float auto_takeoff_complete_alt_cm; // completion altitude expressed in cm above ekf origin or above terrain (depending upon auto_takeoff_terrain_alt) static bool auto_takeoff_terrain_alt; // true if altitudes are above terrain static bool auto_takeoff_complete; // true when takeoff is complete static Vector3p auto_takeoff_complete_pos; // target takeoff position as offset from ekf origin in cm diff --git a/ArduCopter/takeoff.cpp b/ArduCopter/takeoff.cpp index b7df4ea755..136ff31ef6 100644 --- a/ArduCopter/takeoff.cpp +++ b/ArduCopter/takeoff.cpp @@ -179,7 +179,7 @@ void Mode::auto_takeoff_run() } // handle takeoff completion - bool reached_altitude = (copter.pos_control->get_pos_target_z_cm() - auto_takeoff_start_alt_cm) >= ((auto_takeoff_complete_alt_cm - auto_takeoff_start_alt_cm) * 0.90); + bool reached_altitude = (copter.pos_control->get_pos_target_z_cm() - auto_takeoff_start_alt_cm) >= ((auto_takeoff_complete_alt_cm + terr_offset - auto_takeoff_start_alt_cm) * 0.90); bool reached_climb_rate = copter.pos_control->get_vel_desired_cms().z < copter.pos_control->get_max_speed_up_cms() * 0.1; auto_takeoff_complete = reached_altitude && reached_climb_rate;