From dbe6a1e319d54880da63dac8a053fed9cc214d67 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Sat, 31 Dec 2022 12:41:31 +1030 Subject: [PATCH] Copter: Payload Place enhancements --- ArduCopter/Parameters.cpp | 45 +++++ ArduCopter/Parameters.h | 7 + ArduCopter/defines.h | 14 +- ArduCopter/mode.h | 17 +- ArduCopter/mode_auto.cpp | 377 +++++++++++++++++++++----------------- 5 files changed, 272 insertions(+), 188 deletions(-) diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index 3ec8cd373d..a0e1ccbc84 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -1168,12 +1168,56 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // ID 60 is reserved for the SHIP_OPS + // extend to a new group + AP_SUBGROUPEXTENSION("", 61, ParametersG2, var_info2), + // ID 62 is reserved for the SHOW_... parameters from the Skybrush fork at // https://github.com/skybrush-io/ardupilot AP_GROUPEND }; +/* + extension to g2 parameters + */ +const AP_Param::GroupInfo ParametersG2::var_info2[] = { + + // @Param: PLDP_THRESH + // @DisplayName: Payload Place thrust ratio threshold + // @Description: Ratio of vertical thrust during decent below which payload touchdown will trigger. + // @Range: 0.5 0.9 + // @User: Standard + AP_GROUPINFO("PLDP_THRESH", 1, ParametersG2, pldp_thrust_placed_fraction, 0.9), + + // @Param: PLDP_RNG_MIN + // @DisplayName: Payload Place minimum range finder altitude + // @Description: Minimum range finder altitude in m to trigger payload touchdown, set to zero to disable. + // @Units: m + // @Range: 0 100 + // @User: Standard + AP_GROUPINFO("PLDP_RNG_MIN", 2, ParametersG2, pldp_range_finder_minimum_m, 0.0), + + // @Param: PLDP_DELAY + // @DisplayName: Payload Place climb delay + // @Description: Delay after release, in seconds, before aircraft starts to climb back to starting altitude. + // @Units: s + // @Range: 0 120 + // @User: Standard + AP_GROUPINFO("PLDP_DELAY", 3, ParametersG2, pldp_delay_s, 0.0), + + // @Param: PLDP_SPEED_DN + // @DisplayName: Payload Place decent speed + // @Description: The maximum vertical decent velocity in m/s. If 0 LAND_SPEED value is used. + // @Units: m/s + // @Range: 0 5 + // @User: Standard + AP_GROUPINFO("PLDP_SPEED_DN", 4, ParametersG2, pldp_descent_speed_ms, 0.0), + + // ID 62 is reserved for the AP_SUBGROUPEXTENSION + + AP_GROUPEND +}; + /* constructor for g2 object */ @@ -1231,6 +1275,7 @@ ParametersG2::ParametersG2(void) #endif { AP_Param::setup_object_defaults(this, var_info); + AP_Param::setup_object_defaults(this, var_info2); } /* diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 701699e6e8..1b931d83f6 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -492,6 +492,7 @@ public: // var_info for holding Parameter information static const struct AP_Param::GroupInfo var_info[]; + static const struct AP_Param::GroupInfo var_info2[]; // altitude at which nav control can start in takeoff AP_Float wp_navalt_min; @@ -681,6 +682,12 @@ public: #if WEATHERVANE_ENABLED == ENABLED AC_WeatherVane weathervane; #endif + + // payload place parameters + AP_Float pldp_thrust_placed_fraction; + AP_Float pldp_range_finder_minimum_m; + AP_Float pldp_delay_s; + AP_Float pldp_descent_speed_ms; }; extern const AP_Param::Info var_info[]; diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 708042713a..384fb8d97e 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -78,15 +78,13 @@ enum class AirMode { enum PayloadPlaceStateType { PayloadPlaceStateType_FlyToLocation, - PayloadPlaceStateType_Calibrating_Hover_Start, - PayloadPlaceStateType_Calibrating_Hover, - PayloadPlaceStateType_Descending_Start, - PayloadPlaceStateType_Descending, - PayloadPlaceStateType_Releasing_Start, + PayloadPlaceStateType_Descent_Start, + PayloadPlaceStateType_Descent, + PayloadPlaceStateType_Release, PayloadPlaceStateType_Releasing, - PayloadPlaceStateType_Released, - PayloadPlaceStateType_Ascending_Start, - PayloadPlaceStateType_Ascending, + PayloadPlaceStateType_Delay, + PayloadPlaceStateType_Ascent_Start, + PayloadPlaceStateType_Ascent, PayloadPlaceStateType_Done, }; diff --git a/ArduCopter/mode.h b/ArduCopter/mode.h index b7dad188f2..a6dcda9c85 100644 --- a/ArduCopter/mode.h +++ b/ArduCopter/mode.h @@ -553,7 +553,7 @@ private: void payload_place_run(); bool payload_place_run_should_run(); void payload_place_run_hover(); - void payload_place_run_descend(); + void payload_place_run_descent(); void payload_place_run_release(); SubMode _mode = SubMode::TAKEOFF; // controls which auto controller is run @@ -644,14 +644,13 @@ private: State state = State::FlyToLocation; struct { - PayloadPlaceStateType state = PayloadPlaceStateType_Calibrating_Hover_Start; // records state of place (descending, releasing, released, ...) - uint32_t hover_start_timestamp; // milliseconds - float hover_throttle_level; - uint32_t descend_start_timestamp; // milliseconds - uint32_t place_start_timestamp; // milliseconds - float descend_throttle_level; - float descend_start_altitude; - float descend_max; // centimetres + PayloadPlaceStateType state = PayloadPlaceStateType_Descent_Start; // records state of payload place + uint32_t descent_established_time_ms; // milliseconds + uint32_t place_start_time_ms; // milliseconds + float descent_thrust_level; + float descent_start_altitude_cm; + float descent_speed_cms; + float descent_max_cm; } nav_payload_place; bool waiting_to_start; // true if waiting for vehicle to be armed or EKF origin before starting mission diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index 3a195aac7b..ebe3be3132 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -537,8 +537,6 @@ bool ModeAuto::is_taking_off() const // auto_payload_place_start - initialises controller to implement a placing void ModeAuto::payload_place_start() { - nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover_Start; - // set horizontal speed and acceleration limits pos_control->set_max_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration()); pos_control->set_correction_speed_accel_xy(wp_nav->get_default_speed_xy(), wp_nav->get_wp_acceleration()); @@ -562,6 +560,8 @@ void ModeAuto::payload_place_start() // set submode set_submode(SubMode::NAV_PAYLOAD_PLACE); + + nav_payload_place.state = PayloadPlaceStateType_Descent_Start; } // returns true if pilot's yaw input should be used to adjust vehicle's heading @@ -1157,6 +1157,8 @@ void ModeAuto::nav_attitude_time_run() // called by auto_run at 100hz or more void ModeAuto::payload_place_run() { + const char* prefix_str = "PayloadPlace:"; + if (!payload_place_run_should_run()) { zero_throttle_and_relax_ac(); return; @@ -1165,21 +1167,192 @@ void ModeAuto::payload_place_run() // set motors to full range motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); + 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 + const float thrust_level = attitude_control->get_throttle_in(); + const uint32_t now_ms = AP_HAL::millis(); + + // if we discover we've landed then immediately release the load: + if (copter.ap.land_complete || copter.ap.land_complete_maybe) { + switch (nav_payload_place.state) { + case PayloadPlaceStateType_FlyToLocation: + // this is handled in wp_run() + break; + case PayloadPlaceStateType_Descent_Start: + // do nothing on this loop + break; + case PayloadPlaceStateType_Descent: + gcs().send_text(MAV_SEVERITY_INFO, "%s landed", prefix_str); + nav_payload_place.state = PayloadPlaceStateType_Release; + break; + case PayloadPlaceStateType_Release: + case PayloadPlaceStateType_Releasing: + case PayloadPlaceStateType_Delay: + case PayloadPlaceStateType_Ascent_Start: + case PayloadPlaceStateType_Ascent: + case PayloadPlaceStateType_Done: + break; + } + } + +#if AP_GRIPPER_ENABLED == ENABLED + // if pilot releases load manually: + if (g2.gripper.valid() && g2.gripper.released()) { + switch (nav_payload_place.state) { + case PayloadPlaceStateType_FlyToLocation: + case PayloadPlaceStateType_Descent_Start: + set_submode(SubMode::NAV_PAYLOAD_PLACE); + gcs().send_text(MAV_SEVERITY_INFO, "%s Manual release", prefix_str); + nav_payload_place.state = PayloadPlaceStateType_Done; + break; + case PayloadPlaceStateType_Descent: + gcs().send_text(MAV_SEVERITY_INFO, "%s Manual release", prefix_str); + nav_payload_place.state = PayloadPlaceStateType_Release; + break; + case PayloadPlaceStateType_Release: + case PayloadPlaceStateType_Releasing: + case PayloadPlaceStateType_Delay: + case PayloadPlaceStateType_Ascent_Start: + case PayloadPlaceStateType_Ascent: + case PayloadPlaceStateType_Done: + break; + } + } +#endif + switch (nav_payload_place.state) { case PayloadPlaceStateType_FlyToLocation: - return wp_run(); - case PayloadPlaceStateType_Calibrating_Hover_Start: - case PayloadPlaceStateType_Calibrating_Hover: - return payload_place_run_hover(); - case PayloadPlaceStateType_Descending_Start: - case PayloadPlaceStateType_Descending: - return payload_place_run_descend(); - case PayloadPlaceStateType_Releasing_Start: + if (copter.wp_nav->reached_wp_destination()) { + payload_place_start(); + } + break; + + case PayloadPlaceStateType_Descent_Start: + nav_payload_place.descent_established_time_ms = now_ms; + nav_payload_place.descent_start_altitude_cm = inertial_nav.get_position_z_up_cm(); + // limiting the decent rate to the limit set in wp_nav is not necessary but done for safety + nav_payload_place.descent_speed_cms = MIN((is_positive(g2.pldp_descent_speed_ms)) ? g2.pldp_descent_speed_ms * 100.0 : abs(g.land_speed), wp_nav->get_default_speed_down()); + nav_payload_place.descent_thrust_level = 1.0; + nav_payload_place.state = PayloadPlaceStateType_Descent; + FALLTHROUGH; + + case PayloadPlaceStateType_Descent: + // check maximum decent distance + if (!is_zero(nav_payload_place.descent_max_cm) && + nav_payload_place.descent_start_altitude_cm - inertial_nav.get_position_z_up_cm() > nav_payload_place.descent_max_cm) { + nav_payload_place.state = PayloadPlaceStateType_Ascent_Start; + gcs().send_text(MAV_SEVERITY_WARNING, "%s Reached maximum descent", prefix_str); + break; + } + // calibrate the decent thrust after aircraft has reached constant decent rate and release if threshold is reached + if (pos_control->get_vel_desired_cms().z > -0.95 * nav_payload_place.descent_speed_cms) { + // decent rate has not reached descent_speed_cms + nav_payload_place.descent_established_time_ms = now_ms; + break; + } else if (now_ms - nav_payload_place.descent_established_time_ms < descent_thrust_cal_duration_ms) { + // record minimum thrust for descent_thrust_cal_duration_ms + nav_payload_place.descent_thrust_level = MIN(nav_payload_place.descent_thrust_level, thrust_level); + nav_payload_place.place_start_time_ms = now_ms; + break; + } else if (thrust_level > g2.pldp_thrust_placed_fraction * nav_payload_place.descent_thrust_level) { + // thrust is above minimum threshold + nav_payload_place.place_start_time_ms = now_ms; + break; + } else if (is_positive(g2.pldp_range_finder_minimum_m)) { + if (!copter.rangefinder_state.enabled) { + // abort payload place because rangefinder is not enabled + nav_payload_place.state = PayloadPlaceStateType_Ascent_Start; + gcs().send_text(MAV_SEVERITY_WARNING, "%s PLDP_RNG_MIN set and rangefinder not enabled", prefix_str); + break; + } else if (copter.rangefinder_alt_ok() && (copter.rangefinder_state.glitch_count == 0) && (copter.rangefinder_state.alt_cm > g2.pldp_range_finder_minimum_m * 100.0)) { + // range finder altitude is above minimum + nav_payload_place.place_start_time_ms = now_ms; + break; + } + } + + // If we get here: + // 1. we have reached decent velocity + // 2. measured the thrust level required for decent + // 3. detected that our thrust requirements have reduced + // 4. rangefinder range has dropped below minimum if set + // 5. place_start_time_ms has been initialised + + // payload touchdown must be detected for 0.5 seconds + + if (now_ms - nav_payload_place.place_start_time_ms > placed_check_duration_ms) { + nav_payload_place.state = PayloadPlaceStateType_Release; + gcs().send_text(MAV_SEVERITY_INFO, "%s payload release thrust threshold: %f", prefix_str, static_cast(g2.pldp_thrust_placed_fraction * nav_payload_place.descent_thrust_level)); + } + break; + + case PayloadPlaceStateType_Release: + // Reinitialise vertical position controller to remove discontinuity due to touch down of payload + pos_control->init_z_controller_no_descent(); +#if AP_GRIPPER_ENABLED == ENABLED + if (g2.gripper.valid()) { + gcs().send_text(MAV_SEVERITY_INFO, "%s Releasing the gripper", prefix_str); + g2.gripper.release(); + nav_payload_place.state = PayloadPlaceStateType_Releasing; + } else { + nav_payload_place.state = PayloadPlaceStateType_Delay; + } +#else + nav_payload_place.state = PayloadPlaceStateType_Delay; +#endif + break; + case PayloadPlaceStateType_Releasing: - case PayloadPlaceStateType_Released: - case PayloadPlaceStateType_Ascending_Start: +#if AP_GRIPPER_ENABLED == ENABLED + if (g2.gripper.valid() && !g2.gripper.released()) { + break; + } +#endif + nav_payload_place.state = PayloadPlaceStateType_Delay; + FALLTHROUGH; + + case PayloadPlaceStateType_Delay: + // If we get here we have finished releasing the gripper + if (now_ms - nav_payload_place.place_start_time_ms < placed_check_duration_ms + g2.pldp_delay_s * 1000.0) { + break; + } + FALLTHROUGH; + + case PayloadPlaceStateType_Ascent_Start: { + auto_takeoff_start(nav_payload_place.descent_start_altitude_cm, false); + nav_payload_place.state = PayloadPlaceStateType_Ascent; + } + break; + + case PayloadPlaceStateType_Ascent: + if (auto_takeoff_complete) { + nav_payload_place.state = PayloadPlaceStateType_Done; + } + break; + + case PayloadPlaceStateType_Done: + break; + default: + // this should never happen + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + break; + } + + switch (nav_payload_place.state) { + case PayloadPlaceStateType_FlyToLocation: + // this should never happen + return wp_run(); + case PayloadPlaceStateType_Descent_Start: + case PayloadPlaceStateType_Descent: + return payload_place_run_descent(); + case PayloadPlaceStateType_Release: + case PayloadPlaceStateType_Releasing: + case PayloadPlaceStateType_Delay: + case PayloadPlaceStateType_Ascent_Start: return payload_place_run_hover(); - case PayloadPlaceStateType_Ascending: + case PayloadPlaceStateType_Ascent: case PayloadPlaceStateType_Done: return takeoff_run(); } @@ -1196,7 +1369,7 @@ bool ModeAuto::payload_place_run_should_run() return false; } // must not be landed - if (copter.ap.land_complete && (nav_payload_place.state == PayloadPlaceStateType_FlyToLocation || nav_payload_place.state == PayloadPlaceStateType_Calibrating_Hover_Start)) { + if (copter.ap.land_complete && (nav_payload_place.state == PayloadPlaceStateType_FlyToLocation || nav_payload_place.state == PayloadPlaceStateType_Descent_Start)) { return false; } // interlock must be enabled (i.e. unsafe) @@ -1210,13 +1383,17 @@ bool ModeAuto::payload_place_run_should_run() void ModeAuto::payload_place_run_hover() { land_run_horizontal_control(); - land_run_vertical_control(true); + // update altitude target and call position controller + pos_control->land_at_climb_rate_cm(0.0, false); + pos_control->update_z_controller(); } -void ModeAuto::payload_place_run_descend() +void ModeAuto::payload_place_run_descent() { land_run_horizontal_control(); - land_run_vertical_control(); + // update altitude target and call position controller + pos_control->land_at_climb_rate_cm(-nav_payload_place.descent_speed_cms, true); + pos_control->update_z_controller(); } // sets the target_loc's alt to the vehicle's current alt but does not change target_loc's frame @@ -1339,7 +1516,8 @@ bool ModeAuto::set_next_wp(const AP_Mission::Mission_Command& current_cmd, const switch (next_cmd.id) { case MAV_CMD_NAV_WAYPOINT: case MAV_CMD_NAV_LOITER_UNLIM: - case MAV_CMD_NAV_LOITER_TIME: { + case MAV_CMD_NAV_LOITER_TIME: + case MAV_CMD_NAV_PAYLOAD_PLACE: { const Location dest_loc = loc_from_cmd(current_cmd, default_loc); const Location next_dest_loc = loc_from_cmd(next_cmd, dest_loc); return wp_nav->set_wp_destination_next_loc(next_dest_loc); @@ -1754,19 +1932,18 @@ void ModeAuto::do_payload_place(const AP_Mission::Mission_Command& cmd) AP::logger().Write_Error(LogErrorSubsystem::TERRAIN, LogErrorCode::MISSING_TERRAIN_DATA); gcs().send_text(MAV_SEVERITY_CRITICAL, "PayloadPlace: no terrain data, using alt-above-home"); } - if (!wp_start(target_loc)) { // failure to set next destination can only be because of missing terrain data copter.failsafe_terrain_on_event(); return; } + // set submode + set_submode(SubMode::NAV_PAYLOAD_PLACE); } else { - nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover_Start; - // initialise placing controller payload_place_start(); } - nav_payload_place.descend_max = cmd.p1; + nav_payload_place.descent_max_cm = cmd.p1; } // do_RTL - start Return-to-Launch @@ -1837,167 +2014,25 @@ bool ModeAuto::verify_land() return retval; } -#define NAV_PAYLOAD_PLACE_DEBUGGING 0 - -#if NAV_PAYLOAD_PLACE_DEBUGGING -#include -#define debug(fmt, args ...) do {::fprintf(stderr,"%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0) -#else -#define debug(fmt, args ...) -#endif - // verify_payload_place - returns true if placing has been completed bool ModeAuto::verify_payload_place() { - const uint16_t hover_throttle_calibrate_time = 2000; // milliseconds - const uint16_t descend_throttle_calibrate_time = 2000; // milliseconds - const uint16_t measure_time = 1000; // milliseconds - const float hover_throttle_placed_fraction = 0.7; // i.e. if throttle is less than 70% of hover we have placed - const float descent_throttle_placed_fraction = 0.9; // i.e. if throttle is less than 90% of descent throttle we have placed - const uint16_t placed_time = 500; // how long we have to be below a throttle threshold before considering placed - - 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 (copter.ap.land_complete || copter.ap.land_complete_maybe) { - 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(MAV_SEVERITY_INFO, "PayloadPlace: 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 (!copter.wp_nav->reached_wp_destination()) { - return false; - } - payload_place_start(); - return false; - case PayloadPlaceStateType_Calibrating_Hover_Start: - // hover for 2 seconds to measure hover thrust - debug("Calibrate start"); - nav_payload_place.hover_start_timestamp = now; - nav_payload_place.hover_throttle_level = 1.0; - nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover; - FALLTHROUGH; - case PayloadPlaceStateType_Calibrating_Hover: { - if (now - nav_payload_place.hover_start_timestamp < hover_throttle_calibrate_time - measure_time) { - // waiting for aircraft to settle - return false; - } - if (now - nav_payload_place.hover_start_timestamp < hover_throttle_calibrate_time) { - // measure hover thrust - nav_payload_place.hover_throttle_level = MIN(nav_payload_place.hover_throttle_level, current_throttle_level); - return false; - } - // hover thrust has been measured - gcs().send_text(MAV_SEVERITY_INFO, "PayloadPlace: payload hover throttle: %f", static_cast(nav_payload_place.hover_throttle_level)); - nav_payload_place.state = PayloadPlaceStateType_Descending_Start; - } - FALLTHROUGH; - case PayloadPlaceStateType_Descending_Start: - nav_payload_place.descend_start_timestamp = now; - nav_payload_place.descend_start_altitude = inertial_nav.get_position_z_up_cm(); - nav_payload_place.descend_throttle_level = 1.0; - nav_payload_place.state = PayloadPlaceStateType_Descending; - FALLTHROUGH; - case PayloadPlaceStateType_Descending: - // make sure we don't descend too far - debug("descended: %f cm (%f cm max)", (nav_payload_place.descend_start_altitude - inertial_nav.get_position_z_up_cm()), nav_payload_place.descend_max); - if (!is_zero(nav_payload_place.descend_max) && - nav_payload_place.descend_start_altitude - inertial_nav.get_position_z_up_cm() > nav_payload_place.descend_max) { - nav_payload_place.state = PayloadPlaceStateType_Ascending_Start; - gcs().send_text(MAV_SEVERITY_WARNING, "PayloadPlace: Reached maximum descent"); - return false; // we'll do any cleanups required next time through the loop - } - // if the aircraft has been descending long enough, calibrate the decent thrust - if ((now - nav_payload_place.descend_start_timestamp > descend_throttle_calibrate_time - measure_time) && - (now - nav_payload_place.descend_start_timestamp < descend_throttle_calibrate_time)) { - // measure decent thrust - nav_payload_place.descend_throttle_level = MIN(nav_payload_place.descend_throttle_level, current_throttle_level); - } - // check for reduced thrust requirement indicating possible payload touch down - if (current_throttle_level > hover_throttle_placed_fraction * nav_payload_place.hover_throttle_level && - (now - nav_payload_place.descend_start_timestamp < descend_throttle_calibrate_time || - current_throttle_level > descent_throttle_placed_fraction * nav_payload_place.descend_throttle_level)) { - // throttle is above both threshold ratios (or above hover threshold ration and descent threshold ratio not yet valid) - nav_payload_place.place_start_timestamp = now; - return false; - } - if (now - nav_payload_place.place_start_timestamp < placed_time) { - // continue decent - debug("Place Timer: %d", now - nav_payload_place.place_start_timestamp); - return false; - } - nav_payload_place.state = PayloadPlaceStateType_Releasing_Start; - FALLTHROUGH; - case PayloadPlaceStateType_Releasing_Start: - // Reinitialise vertical position controller to remove discontinuity due to touch down of payload - pos_control->init_z_controller_no_descent(); -#if AP_GRIPPER_ENABLED - if (g2.gripper.valid()) { - gcs().send_text(MAV_SEVERITY_INFO, "PayloadPlace: Releasing the gripper"); - g2.gripper.release(); - } else { - gcs().send_text(MAV_SEVERITY_INFO, "PayloadPlace: Gripper not valid"); - nav_payload_place.state = PayloadPlaceStateType_Ascending_Start; - return false; - } -#else - gcs().send_text(MAV_SEVERITY_INFO, "Gripper code disabled"); - nav_payload_place.state = PayloadPlaceStateType_Ascending_Start; - return false; -#endif - nav_payload_place.state = PayloadPlaceStateType_Releasing; - FALLTHROUGH; + case PayloadPlaceStateType_Descent_Start: + case PayloadPlaceStateType_Descent: + case PayloadPlaceStateType_Release: case PayloadPlaceStateType_Releasing: -#if AP_GRIPPER_ENABLED - if (g2.gripper.valid() && !g2.gripper.released()) { - return false; - } -#endif - nav_payload_place.state = PayloadPlaceStateType_Released; - FALLTHROUGH; - case PayloadPlaceStateType_Released: { - nav_payload_place.state = PayloadPlaceStateType_Ascending_Start; - } - FALLTHROUGH; - case PayloadPlaceStateType_Ascending_Start: { - auto_takeoff_start(nav_payload_place.descend_start_altitude, false); - nav_payload_place.state = PayloadPlaceStateType_Ascending; - } - FALLTHROUGH; - case PayloadPlaceStateType_Ascending: - if (!auto_takeoff_complete) { - return false; - } - nav_payload_place.state = PayloadPlaceStateType_Done; - FALLTHROUGH; + case PayloadPlaceStateType_Delay: + case PayloadPlaceStateType_Ascent_Start: + case PayloadPlaceStateType_Ascent: + return false; case PayloadPlaceStateType_Done: return true; - default: - // this should never happen - INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); - return true; } // should never get here return true; } -#undef debug bool ModeAuto::verify_loiter_unlimited() {