Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-21 23:33:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: encapsulate PayLoadPlace state and methods into existing struct

Browse Source 
... and make it a class
This commit is contained in:
Peter Barker 2023-09-28 11:24:36 +10:00 committed by Peter Barker
parent ba72b7edfd
commit 5820a9bc2f
5 changed files with 176 additions and 148 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/Copter.h
View File

@ -237,6 +237,8 @@ public:
friend class _AutoTakeoff;
friend class PayloadPlace;
Copter(void);
private:

12
ArduCopter/defines.h
View File

@ -76,18 +76,6 @@ enum class AirMode {
AIRMODE_ENABLED,
};
enum PayloadPlaceStateType {
PayloadPlaceStateType_FlyToLocation,
PayloadPlaceStateType_Descent_Start,
PayloadPlaceStateType_Descent,
PayloadPlaceStateType_Release,
PayloadPlaceStateType_Releasing,
PayloadPlaceStateType_Delay,
PayloadPlaceStateType_Ascent_Start,
PayloadPlaceStateType_Ascent,
PayloadPlaceStateType_Done,
};
// bit options for DEV_OPTIONS parameter
enum DevOptions {
DevOptionADSBMAVLink = 1,

2
ArduCopter/mode.cpp
View File

@ -25,6 +25,8 @@ Mode::Mode(void) :
G_Dt(copter.G_Dt)
{ };
PayloadPlace Mode::payload_place;
// return the static controller object corresponding to supplied mode
Mode *Copter::mode_from_mode_num(const Mode::Number mode)
{

58
ArduCopter/mode.h
View File

@ -29,7 +29,43 @@ private:
Vector3p complete_pos; // target takeoff position as offset from ekf origin in cm
};
class PayloadPlace {
public:
void run();
void start_descent();
bool verify();
enum class State : uint8_t {
FlyToLocation,
Descent_Start,
Descent,
Release,
Releasing,
Delay,
Ascent_Start,
Ascent,
Done,
};
// these are set by the Mission code:
State state = State::Descent_Start; // records state of payload place
float descent_max_cm;
private:
bool run_should_run();
void run_hover();
void run_descent();
void run_release();
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;
};
class Mode {
friend class PayloadPlace;
public:
@ -167,6 +203,9 @@ protected:
land_run_vertical_control(pause_descent);
}
// payload place flight behaviour:
static PayloadPlace payload_place;
// run normal or precision landing (if enabled)
// pause_descent is true if vehicle should not descend
void land_run_normal_or_precland(bool pause_descent = false);
@ -432,10 +471,11 @@ private:
};
class ModeAuto : public Mode {
public:
friend class PayloadPlace; // in case wp_run is accidentally required
// inherit constructor
using Mode::Mode;
Number mode_number() const override { return auto_RTL? Number::AUTO_RTL : Number::AUTO; }
@ -556,12 +596,6 @@ private:
Location loc_from_cmd(const AP_Mission::Mission_Command& cmd, const Location& default_loc) const;
void payload_place_run();
bool payload_place_run_should_run();
void payload_place_run_hover();
void payload_place_run_descent();
void payload_place_run_release();
SubMode _mode = SubMode::TAKEOFF; // controls which auto controller is run
bool shift_alt_to_current_alt(Location& target_loc) const;
@ -649,16 +683,6 @@ private:
};
State state = State::FlyToLocation;
struct {
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
// True if we have entered AUTO to perform a DO_LAND_START landing sequence and we should report as AUTO RTL mode

250
ArduCopter/mode_auto.cpp
View File

@ -150,7 +150,7 @@ void ModeAuto::run()
break;
case SubMode::NAV_PAYLOAD_PLACE:
payload_place_run();
payload_place.run();
break;
case SubMode::NAV_ATTITUDE_TIME:
@ -540,8 +540,11 @@ bool ModeAuto::is_taking_off() const
}
// auto_payload_place_start - initialises controller to implement a placing
void ModeAuto::payload_place_start()
void PayloadPlace::start_descent()
{
auto *pos_control = copter.pos_control;
auto *wp_nav = copter.wp_nav;
// 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());
@ -561,12 +564,9 @@ void ModeAuto::payload_place_start()
}
// initialise yaw
auto_yaw.set_mode(AutoYaw::Mode::HOLD);
copter.flightmode->auto_yaw.set_mode(Mode::AutoYaw::Mode::HOLD);
// set submode
set_submode(SubMode::NAV_PAYLOAD_PLACE);
nav_payload_place.state = PayloadPlaceStateType_Descent_Start;
state = PayloadPlace::State::Descent_Start;
}
// returns true if pilot's yaw input should be used to adjust vehicle's heading
@ -866,7 +866,7 @@ bool ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd)
break;
case MAV_CMD_NAV_PAYLOAD_PLACE:
cmd_complete = verify_payload_place();
cmd_complete = payload_place.verify();
break;
case MAV_CMD_NAV_LOITER_UNLIM:
@ -1160,120 +1160,127 @@ void ModeAuto::nav_attitude_time_run()
// auto_payload_place_run - places an object in auto mode
// called by auto_run at 100hz or more
void ModeAuto::payload_place_run()
void PayloadPlace::run()
{
const char* prefix_str = "PayloadPlace:";
if (!payload_place_run_should_run()) {
zero_throttle_and_relax_ac();
if (!run_should_run()) {
copter.flightmode->zero_throttle_and_relax_ac();
return;
}
// set motors to full range
motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
copter.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
auto *attitude_control = copter.attitude_control;
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:
switch (state) {
case State::FlyToLocation:
// this is handled in wp_run()
break;
case PayloadPlaceStateType_Descent_Start:
case State::Descent_Start:
// do nothing on this loop
break;
case PayloadPlaceStateType_Descent:
case State::Descent:
gcs().send_text(MAV_SEVERITY_INFO, "%s landed", prefix_str);
nav_payload_place.state = PayloadPlaceStateType_Release;
state = State::Release;
break;
case PayloadPlaceStateType_Release:
case PayloadPlaceStateType_Releasing:
case PayloadPlaceStateType_Delay:
case PayloadPlaceStateType_Ascent_Start:
case PayloadPlaceStateType_Ascent:
case PayloadPlaceStateType_Done:
case State::Release:
case State::Releasing:
case State::Delay:
case State::Ascent_Start:
case State::Ascent:
case State::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);
if (AP::gripper() != nullptr &&
AP::gripper()->valid() && AP::gripper()->released()) {
switch (state) {
case State::FlyToLocation:
case State::Descent_Start:
gcs().send_text(MAV_SEVERITY_INFO, "%s Manual release", prefix_str);
nav_payload_place.state = PayloadPlaceStateType_Done;
state = State::Done;
break;
case PayloadPlaceStateType_Descent:
case State::Descent:
gcs().send_text(MAV_SEVERITY_INFO, "%s Manual release", prefix_str);
nav_payload_place.state = PayloadPlaceStateType_Release;
state = State::Release;
break;
case PayloadPlaceStateType_Release:
case PayloadPlaceStateType_Releasing:
case PayloadPlaceStateType_Delay:
case PayloadPlaceStateType_Ascent_Start:
case PayloadPlaceStateType_Ascent:
case PayloadPlaceStateType_Done:
case State::Release:
case State::Releasing:
case State::Delay:
case State::Ascent_Start:
case State::Ascent:
case State::Done:
break;
}
}
#endif
switch (nav_payload_place.state) {
case PayloadPlaceStateType_FlyToLocation:
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()) {
payload_place_start();
start_descent();
}
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();
case State::Descent_Start:
descent_established_time_ms = now_ms;
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;
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());
descent_thrust_level = 1.0;
state = State::Descent;
FALLTHROUGH;
case PayloadPlaceStateType_Descent:
case State::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;
if (!is_zero(descent_max_cm) &&
descent_start_altitude_cm - inertial_nav.get_position_z_up_cm() > descent_max_cm) {
state = State::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) {
if (pos_control->get_vel_desired_cms().z > -0.95 * descent_speed_cms) {
// decent rate has not reached descent_speed_cms
nav_payload_place.descent_established_time_ms = now_ms;
descent_established_time_ms = now_ms;
break;
} else if (now_ms - nav_payload_place.descent_established_time_ms < descent_thrust_cal_duration_ms) {
} else if (now_ms - 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;
descent_thrust_level = MIN(descent_thrust_level, thrust_level);
place_start_time_ms = now_ms;
break;
} else if (thrust_level > g2.pldp_thrust_placed_fraction * nav_payload_place.descent_thrust_level) {
} else if (thrust_level > g2.pldp_thrust_placed_fraction * descent_thrust_level) {
// thrust is above minimum threshold
nav_payload_place.place_start_time_ms = now_ms;
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;
state = State::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;
place_start_time_ms = now_ms;
break;
}
}
@ -1287,57 +1294,56 @@ void ModeAuto::payload_place_run()
// 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));
if (now_ms - place_start_time_ms > placed_check_duration_ms) {
state = State::Release;
gcs().send_text(MAV_SEVERITY_INFO, "%s payload release thrust threshold: %f", prefix_str, static_cast<double>(g2.pldp_thrust_placed_fraction * descent_thrust_level));
}
break;
case PayloadPlaceStateType_Release:
case State::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;
state = State::Releasing;
} else {
nav_payload_place.state = PayloadPlaceStateType_Delay;
state = State::Delay;
}
#else
nav_payload_place.state = PayloadPlaceStateType_Delay;
state = State::Delay;
#endif
break;
case PayloadPlaceStateType_Releasing:
case State::Releasing:
#if AP_GRIPPER_ENABLED == ENABLED
if (g2.gripper.valid() && !g2.gripper.released()) {
break;
}
#endif
nav_payload_place.state = PayloadPlaceStateType_Delay;
state = State::Delay;
FALLTHROUGH;
case PayloadPlaceStateType_Delay:
case State::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) {
if (now_ms - 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;
case State::Ascent_Start:
copter.flightmode->auto_takeoff.start(descent_start_altitude_cm, false);
state = State::Ascent;
break;
case State::Ascent:
if (copter.flightmode->auto_takeoff.complete) {
state = State::Done;
}
break;
case PayloadPlaceStateType_Ascent:
if (auto_takeoff.complete) {
nav_payload_place.state = PayloadPlaceStateType_Done;
}
break;
case PayloadPlaceStateType_Done:
case State::Done:
break;
default:
// this should never happen
@ -1345,28 +1351,28 @@ void ModeAuto::payload_place_run()
break;
}
switch (nav_payload_place.state) {
case PayloadPlaceStateType_FlyToLocation:
switch (state) {
case State::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_Ascent:
case PayloadPlaceStateType_Done:
return takeoff_run();
return copter.mode_auto.wp_run();
case State::Descent_Start:
case State::Descent:
return run_descent();
case State::Release:
case State::Releasing:
case State::Delay:
case State::Ascent_Start:
return run_hover();
case State::Ascent:
case State::Done:
return copter.mode_auto.takeoff_run();
}
}
bool ModeAuto::payload_place_run_should_run()
bool PayloadPlace::run_should_run()
{
// must be armed
if (!motors->armed()) {
if (!copter.motors->armed()) {
return false;
}
// must be auto-armed
@ -1374,30 +1380,35 @@ 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_Descent_Start)) {
if (copter.ap.land_complete &&
(state == State::FlyToLocation || state == State::Descent_Start)) {
return false;
}
// interlock must be enabled (i.e. unsafe)
if (!motors->get_interlock()) {
if (!copter.motors->get_interlock()) {
return false;
}
return true;
}
void ModeAuto::payload_place_run_hover()
void PayloadPlace::run_hover()
{
land_run_horizontal_control();
const auto &pos_control = copter.pos_control;
copter.flightmode->land_run_horizontal_control();
// 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_descent()
void PayloadPlace::run_descent()
{
land_run_horizontal_control();
const auto &pos_control = copter.pos_control;
copter.flightmode->land_run_horizontal_control();
// update altitude target and call position controller
pos_control->land_at_climb_rate_cm(-nav_payload_place.descent_speed_cms, true);
pos_control->land_at_climb_rate_cm(-descent_speed_cms, true);
pos_control->update_z_controller();
}
@ -1933,7 +1944,7 @@ void ModeAuto::do_payload_place(const AP_Mission::Mission_Command& cmd)
// if location provided we fly to that location at current altitude
if (cmd.content.location.lat != 0 || cmd.content.location.lng != 0) {
// set state to fly to location
nav_payload_place.state = PayloadPlaceStateType_FlyToLocation;
payload_place.state = PayloadPlace::State::FlyToLocation;
// convert cmd to location class
Location target_loc(cmd.content.location);
@ -1949,13 +1960,14 @@ void ModeAuto::do_payload_place(const AP_Mission::Mission_Command& cmd)
copter.failsafe_terrain_on_event();
return;
}
// set submode
set_submode(SubMode::NAV_PAYLOAD_PLACE);
} else {
// initialise placing controller
payload_place_start();
payload_place.start_descent();
}
nav_payload_place.descent_max_cm = cmd.p1;
payload_place.descent_max_cm = cmd.p1;
// set submode
set_submode(SubMode::NAV_PAYLOAD_PLACE);
}
// do_RTL - start Return-to-Launch
@ -2027,19 +2039,19 @@ bool ModeAuto::verify_land()
}
// verify_payload_place - returns true if placing has been completed
bool ModeAuto::verify_payload_place()
bool PayloadPlace::verify()
{
switch (nav_payload_place.state) {
case PayloadPlaceStateType_FlyToLocation:
case PayloadPlaceStateType_Descent_Start:
case PayloadPlaceStateType_Descent:
case PayloadPlaceStateType_Release:
case PayloadPlaceStateType_Releasing:
case PayloadPlaceStateType_Delay:
case PayloadPlaceStateType_Ascent_Start:
case PayloadPlaceStateType_Ascent:
switch (state) {
case State::FlyToLocation:
case State::Descent_Start:
case State::Descent:
case State::Release:
case State::Releasing:
case State::Delay:
case State::Ascent_Start:
case State::Ascent:
return false;
case PayloadPlaceStateType_Done:
case State::Done:
return true;
}
// should never get here