From e144e1d2e8f7817445a422083b4c23a4b77f0df8 Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Fri, 22 Mar 2024 17:51:45 +0100 Subject: [PATCH] Copter: reload takeoff cmd on arm when using AllowTakeOffWithoutRaisingThrottle USING AUTO_OPTIONS 135 we set auto mode and arm from it if we got a takeoff cmd as first nav cmd. Doing this, the takeoff cmd is loaded when we enter the auto mode and set the target alt relatively to origin https://github.com/ArduPilot/ardupilot/blob/master/ArduCopter/mode_auto.cpp#L343 This is never called again. So when we reset the home from the first arming, we don't update the target alt relativly to origin. This leads to a wrong takeoff alt settle due to the alt drift on ground. --- ArduCopter/mode.h | 2 ++ ArduCopter/mode_auto.cpp | 14 ++++++++------ 2 files changed, 10 insertions(+), 6 deletions(-) diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index 9d6526b46d545d..1766a3711d85e2 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -725,6 +725,8 @@ class ModeAuto : public Mode { float up; // desired speed upwards in m/s. 0 if unset float down; // desired speed downwards in m/s. 0 if unset } desired_speed_override; + + Location takeoff_loc; // location of takeoff command }; #if AUTOTUNE_ENABLED == ENABLED diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 12233d9f4877bb..8aa14df29b8fee 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -335,19 +335,18 @@ void ModeAuto::takeoff_start(const Location& dest_loc) alt_target_terrain = true; } else { // set horizontal target - Location dest(dest_loc); - dest.lat = copter.current_loc.lat; - dest.lng = copter.current_loc.lng; + takeoff_loc = Location(copter.current_loc.lat, copter.current_loc.lng, dest_loc.alt, dest_loc.get_alt_frame()); // get altitude target above EKF origin - if (!dest.get_alt_cm(Location::AltFrame::ABOVE_ORIGIN, alt_target_cm)) { + if (!takeoff_loc.get_alt_cm(Location::AltFrame::ABOVE_ORIGIN, alt_target_cm)) { // this failure could only happen if take-off alt was specified as an alt-above terrain and we have no terrain data LOGGER_WRITE_ERROR(LogErrorSubsystem::TERRAIN, LogErrorCode::MISSING_TERRAIN_DATA); // fall back to altitude above current altitude - alt_target_cm = current_alt_cm + dest.alt; + alt_target_cm = current_alt_cm + takeoff_loc.alt; } } + // sanity check target int32_t alt_target_min_cm = current_alt_cm + (copter.ap.land_complete ? 100 : 0); alt_target_cm = MAX(alt_target_cm, alt_target_min_cm); @@ -992,7 +991,10 @@ void ModeAuto::takeoff_run() // if the user doesn't want to raise the throttle we can set it automatically // note that this can defeat the disarm check on takeoff if ((copter.g2.auto_options & (int32_t)Options::AllowTakeOffWithoutRaisingThrottle) != 0) { - copter.set_auto_armed(true); + if (motors->armed() && copter.ap.auto_armed == false) { + takeoff_start(takeoff_loc); // reload the takeoff command after arming home update and restart takeoff start sequence + copter.set_auto_armed(true); + } } auto_takeoff.run(); }