From 4aab4f72b64412b210267d50c4c56854f421d3c8 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Fri, 2 Dec 2016 13:25:08 +1100 Subject: [PATCH] Copter: move to releasing payload if we are landed --- ArduCopter/commands_logic.cpp | 22 ++++++++++++++++++++++ 1 file changed, 22 insertions(+) diff --git a/ArduCopter/commands_logic.cpp b/ArduCopter/commands_logic.cpp index 05738e21ef..f2b8e47a20 100644 --- a/ArduCopter/commands_logic.cpp +++ b/ArduCopter/commands_logic.cpp @@ -705,6 +705,28 @@ bool Copter::verify_payload_place() const float current_throttle_level = motors.get_throttle(); const uint32_t now = AP_HAL::millis(); + + // if we discover we've landed then immediately release the load: + if (ap.land_complete) { + switch (nav_payload_place.state) { + case PayloadPlaceStateType_FlyToLocation: + case PayloadPlaceStateType_Calibrating_Hover_Start: + case PayloadPlaceStateType_Calibrating_Hover: + case PayloadPlaceStateType_Descending_Start: + case PayloadPlaceStateType_Descending: + gcs_send_text_fmt(MAV_SEVERITY_INFO, "NAV_PLACE: landed"); + nav_payload_place.state = PayloadPlaceStateType_Releasing_Start; + break; + case PayloadPlaceStateType_Releasing_Start: + case PayloadPlaceStateType_Releasing: + case PayloadPlaceStateType_Released: + case PayloadPlaceStateType_Ascending_Start: + case PayloadPlaceStateType_Ascending: + case PayloadPlaceStateType_Done: + break; + } + } + switch (nav_payload_place.state) { case PayloadPlaceStateType_FlyToLocation: if (!wp_nav.reached_wp_destination()) {