From 1de735441a2da796a12f928a5e0ef0c3c1c4e716 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Mon, 15 Jul 2024 17:42:57 +0930 Subject: [PATCH] Copter: Payload Place: Fix first run abort --- ArduCopter/mode_auto.cpp | 20 ++++++++++++-------- 1 file changed, 12 insertions(+), 8 deletions(-) diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index f6bd5d9377eb9..b5e9a62eec325 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -1255,13 +1255,21 @@ void PayloadPlace::run() const uint32_t descent_thrust_cal_duration_ms = 2000; // milliseconds const uint32_t placed_check_duration_ms = 500; // how long we have to be below a throttle threshold before considering placed - // Vertical thrust is taken from the attitude controller before angle boost is added + auto &g2 = copter.g2; + const auto &g = copter.g; + auto &inertial_nav = copter.inertial_nav; auto *attitude_control = copter.attitude_control; + const auto &pos_control = copter.pos_control; + const auto &wp_nav = copter.wp_nav; + + // Vertical thrust is taken from the attitude controller before angle boost is added const float thrust_level = attitude_control->get_throttle_in(); const uint32_t now_ms = AP_HAL::millis(); + // relax position target if we might be landed // if we discover we've landed then immediately release the load: if (copter.ap.land_complete || copter.ap.land_complete_maybe) { + pos_control->soften_for_landing_xy(); switch (state) { case State::FlyToLocation: // this is handled in wp_run() @@ -1289,7 +1297,9 @@ void PayloadPlace::run() switch (state) { case State::FlyToLocation: case State::Descent_Start: - gcs().send_text(MAV_SEVERITY_INFO, "%s Manual release", prefix_str); + gcs().send_text(MAV_SEVERITY_INFO, "%s Abort: Gripper Open", prefix_str); + // Descent_Start has not run so we must also initalise descent_start_altitude_cm + descent_start_altitude_cm = inertial_nav.get_position_z_up_cm(); state = State::Done; break; case State::Descent: @@ -1307,12 +1317,6 @@ void PayloadPlace::run() } #endif - auto &inertial_nav = copter.inertial_nav; - auto &g2 = copter.g2; - const auto &g = copter.g; - const auto &wp_nav = copter.wp_nav; - const auto &pos_control = copter.pos_control; - switch (state) { case State::FlyToLocation: if (copter.wp_nav->reached_wp_destination()) {