Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: Payload Place enhancements

This commit is contained in:
Leonard Hall 2022-12-31 12:41:31 +10:30 committed by Randy Mackay
parent 7857bb2210
commit dbe6a1e319
5 changed files with 272 additions and 188 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
ArduCopter/Parameters.cpp
View File

@ -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);
}
/*

7
ArduCopter/Parameters.h
View File

@ -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[];

14
ArduCopter/defines.h
View File

@ -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,
};

17
ArduCopter/mode.h
View File

@ -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

377
ArduCopter/mode_auto.cpp
View File

@ -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<double>(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 <stdio.h>
#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<double>(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()
{