Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-18 06:38:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: rename AutoMode enum to SubMode

Browse Source
This commit is contained in:
Pierre Kancir 2021-04-13 17:06:43 +02:00 committed by Andrew Tridgell
parent 3fb73d7e93
commit f43e42f0b6
2 changed files with 43 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
ArduCopter/mode.h
View File

@ -358,17 +358,16 @@ public:
bool has_manual_throttle() const override { return false; } bool has_manual_throttle() const override { return false; }
bool allows_arming(AP_Arming::Method method) const override; bool allows_arming(AP_Arming::Method method) const override;
bool is_autopilot() const override { return true; } bool is_autopilot() const override { return true; }
bool in_guided_mode() const override { return mode() == AutoMode::NAVGUIDED; } bool in_guided_mode() const override { return mode() == SubMode::NAVGUIDED; }
// Auto modes // Auto modes
enum class AutoMode : uint8_t { enum class SubMode : uint8_t {
TAKEOFF, TAKEOFF,
WP, WP,
LAND, LAND,
RTL, RTL,
CIRCLE_MOVE_TO_EDGE, CIRCLE_MOVE_TO_EDGE,
CIRCLE, CIRCLE,
SPLINE,
NAVGUIDED, NAVGUIDED,
LOITER, LOITER,
LOITER_TO_ALT, LOITER_TO_ALT,
@ -376,7 +375,7 @@ public:
}; };
// Auto // Auto
AutoMode mode() const { return _mode; } SubMode mode() const { return _mode; }
bool loiter_start(); bool loiter_start();
void rtl_start(); void rtl_start();
@ -452,7 +451,7 @@ private:
void payload_place_run_descend(); void payload_place_run_descend();
void payload_place_run_release(); void payload_place_run_release();
AutoMode _mode = AutoMode::TAKEOFF; // controls which auto controller is run SubMode _mode = SubMode::TAKEOFF; // controls which auto controller is run
Location terrain_adjusted_location(const AP_Mission::Mission_Command& cmd) const; Location terrain_adjusted_location(const AP_Mission::Mission_Command& cmd) const;

78
ArduCopter/mode_auto.cpp
View File

@ -23,7 +23,7 @@
bool ModeAuto::init(bool ignore_checks) bool ModeAuto::init(bool ignore_checks)
{ {
if (mission.num_commands() > 1 || ignore_checks) { if (mission.num_commands() > 1 || ignore_checks) {
_mode = AutoMode::LOITER; _mode = SubMode::LOITER;
// reject switching to auto mode if landed with motors armed but first command is not a takeoff (reduce chance of flips) // reject switching to auto mode if landed with motors armed but first command is not a takeoff (reduce chance of flips)
if (motors->armed() && copter.ap.land_complete && !mission.starts_with_takeoff_cmd()) { if (motors->armed() && copter.ap.land_complete && !mission.starts_with_takeoff_cmd()) {
@ -86,7 +86,7 @@ void ModeAuto::run()
// check for mission changes // check for mission changes
if (check_for_mission_change()) { if (check_for_mission_change()) {
// if mission is running restart the current command if it is a waypoint or spline command // if mission is running restart the current command if it is a waypoint or spline command
if ((copter.mode_auto.mission.state() == AP_Mission::MISSION_RUNNING) && (_mode == AutoMode::WP)) { if ((copter.mode_auto.mission.state() == AP_Mission::MISSION_RUNNING) && (_mode == SubMode::WP)) {
if (mission.restart_current_nav_cmd()) { if (mission.restart_current_nav_cmd()) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "Auto mission changed, restarted command"); gcs().send_text(MAV_SEVERITY_CRITICAL, "Auto mission changed, restarted command");
} else { } else {
@ -102,42 +102,42 @@ void ModeAuto::run()
// call the correct auto controller // call the correct auto controller
switch (_mode) { switch (_mode) {
case AutoMode::TAKEOFF: case SubMode::TAKEOFF:
takeoff_run(); takeoff_run();
break; break;
case AutoMode::WP: case SubMode::WP:
case AutoMode::CIRCLE_MOVE_TO_EDGE: case SubMode::CIRCLE_MOVE_TO_EDGE:
wp_run(); wp_run();
break; break;
case AutoMode::LAND: case SubMode::LAND:
land_run(); land_run();
break; break;
case AutoMode::RTL: case SubMode::RTL:
rtl_run(); rtl_run();
break; break;
case AutoMode::CIRCLE: case SubMode::CIRCLE:
circle_run(); circle_run();
break; break;
case AutoMode::NAVGUIDED: case SubMode::NAVGUIDED:
#if NAV_GUIDED == ENABLED #if NAV_GUIDED == ENABLED
nav_guided_run(); nav_guided_run();
#endif #endif
break; break;
case AutoMode::LOITER: case SubMode::LOITER:
loiter_run(); loiter_run();
break; break;
case AutoMode::LOITER_TO_ALT: case SubMode::LOITER_TO_ALT:
loiter_to_alt_run(); loiter_to_alt_run();
break; break;
case AutoMode::NAV_PAYLOAD_PLACE: case SubMode::NAV_PAYLOAD_PLACE:
payload_place_run(); payload_place_run();
break; break;
} }
@ -156,7 +156,7 @@ bool ModeAuto::loiter_start()
if (!copter.position_ok()) { if (!copter.position_ok()) {
return false; return false;
} }
_mode = AutoMode::LOITER; _mode = SubMode::LOITER;
// calculate stopping point // calculate stopping point
Vector3f stopping_point; Vector3f stopping_point;
@ -174,7 +174,7 @@ bool ModeAuto::loiter_start()
// auto_rtl_start - initialises RTL in AUTO flight mode // auto_rtl_start - initialises RTL in AUTO flight mode
void ModeAuto::rtl_start() void ModeAuto::rtl_start()
{ {
_mode = AutoMode::RTL; _mode = SubMode::RTL;
// call regular rtl flight mode initialisation and ask it to ignore checks // call regular rtl flight mode initialisation and ask it to ignore checks
copter.mode_rtl.init(true); copter.mode_rtl.init(true);
@ -183,7 +183,7 @@ void ModeAuto::rtl_start()
// auto_takeoff_start - initialises waypoint controller to implement take-off // auto_takeoff_start - initialises waypoint controller to implement take-off
void ModeAuto::takeoff_start(const Location& dest_loc) void ModeAuto::takeoff_start(const Location& dest_loc)
{ {
_mode = AutoMode::TAKEOFF; _mode = SubMode::TAKEOFF;
Location dest(dest_loc); Location dest(dest_loc);
@ -242,7 +242,7 @@ void ModeAuto::wp_start(const Location& dest_loc)
return; return;
} }
_mode = AutoMode::WP; _mode = SubMode::WP;
// initialise yaw // initialise yaw
// To-Do: reset the yaw only when the previous navigation command is not a WP. this would allow removing the special check for ROI // To-Do: reset the yaw only when the previous navigation command is not a WP. this would allow removing the special check for ROI
@ -265,7 +265,7 @@ void ModeAuto::land_start()
// auto_land_start - initialises controller to implement a landing // auto_land_start - initialises controller to implement a landing
void ModeAuto::land_start(const Vector3f& destination) void ModeAuto::land_start(const Vector3f& destination)
{ {
_mode = AutoMode::LAND; _mode = SubMode::LAND;
// initialise loiter target destination // initialise loiter target destination
loiter_nav->init_target(destination); loiter_nav->init_target(destination);
@ -310,7 +310,7 @@ void ModeAuto::circle_movetoedge_start(const Location &circle_center, float radi
// if more than 3m then fly to edge // if more than 3m then fly to edge
if (dist_to_edge > 300.0f) { if (dist_to_edge > 300.0f) {
// set the state to move to the edge of the circle // set the state to move to the edge of the circle
_mode = AutoMode::CIRCLE_MOVE_TO_EDGE; _mode = SubMode::CIRCLE_MOVE_TO_EDGE;
// convert circle_edge_neu to Location // convert circle_edge_neu to Location
Location circle_edge(circle_edge_neu, Location::AltFrame::ABOVE_ORIGIN); Location circle_edge(circle_edge_neu, Location::AltFrame::ABOVE_ORIGIN);
@ -347,7 +347,7 @@ void ModeAuto::circle_movetoedge_start(const Location &circle_center, float radi
// assumes that circle_nav object has already been initialised with circle center and radius // assumes that circle_nav object has already been initialised with circle center and radius
void ModeAuto::circle_start() void ModeAuto::circle_start()
{ {
_mode = AutoMode::CIRCLE; _mode = SubMode::CIRCLE;
// initialise circle controller // initialise circle controller
copter.circle_nav->init(copter.circle_nav->get_center(), copter.circle_nav->center_is_terrain_alt()); copter.circle_nav->init(copter.circle_nav->get_center(), copter.circle_nav->center_is_terrain_alt());
@ -361,7 +361,7 @@ void ModeAuto::circle_start()
// auto_nav_guided_start - hand over control to external navigation controller in AUTO mode // auto_nav_guided_start - hand over control to external navigation controller in AUTO mode
void ModeAuto::nav_guided_start() void ModeAuto::nav_guided_start()
{ {
_mode = AutoMode::NAVGUIDED; _mode = SubMode::NAVGUIDED;
// call regular guided flight mode initialisation // call regular guided flight mode initialisation
copter.mode_guided.init(true); copter.mode_guided.init(true);
@ -374,9 +374,9 @@ void ModeAuto::nav_guided_start()
bool ModeAuto::is_landing() const bool ModeAuto::is_landing() const
{ {
switch(_mode) { switch(_mode) {
case AutoMode::LAND: case SubMode::LAND:
return true; return true;
case AutoMode::RTL: case SubMode::RTL:
return copter.mode_rtl.is_landing(); return copter.mode_rtl.is_landing();
default: default:
return false; return false;
@ -386,7 +386,7 @@ bool ModeAuto::is_landing() const
bool ModeAuto::is_taking_off() const bool ModeAuto::is_taking_off() const
{ {
return ((_mode == AutoMode::TAKEOFF) && !wp_nav->reached_wp_destination()); return ((_mode == SubMode::TAKEOFF) && !wp_nav->reached_wp_destination());
} }
// auto_payload_place_start - initialises controller to implement a placing // auto_payload_place_start - initialises controller to implement a placing
@ -608,7 +608,7 @@ bool ModeAuto::check_for_mission_change()
bool ModeAuto::do_guided(const AP_Mission::Mission_Command& cmd) bool ModeAuto::do_guided(const AP_Mission::Mission_Command& cmd)
{ {
// only process guided waypoint if we are in guided mode // only process guided waypoint if we are in guided mode
if (copter.flightmode->mode_number() != Mode::Number::GUIDED && !(copter.flightmode->mode_number() == Mode::Number::AUTO && mode() == AutoMode::NAVGUIDED)) { if (copter.flightmode->mode_number() != Mode::Number::GUIDED && !(copter.flightmode->mode_number() == Mode::Number::AUTO && mode() == SubMode::NAVGUIDED)) {
return false; return false;
} }
@ -637,10 +637,10 @@ bool ModeAuto::do_guided(const AP_Mission::Mission_Command& cmd)
uint32_t ModeAuto::wp_distance() const uint32_t ModeAuto::wp_distance() const
{ {
switch (_mode) { switch (_mode) {
case AutoMode::CIRCLE: case SubMode::CIRCLE:
return copter.circle_nav->get_distance_to_target(); return copter.circle_nav->get_distance_to_target();
case AutoMode::WP: case SubMode::WP:
case AutoMode::CIRCLE_MOVE_TO_EDGE: case SubMode::CIRCLE_MOVE_TO_EDGE:
default: default:
return wp_nav->get_wp_distance_to_destination(); return wp_nav->get_wp_distance_to_destination();
} }
@ -649,10 +649,10 @@ uint32_t ModeAuto::wp_distance() const
int32_t ModeAuto::wp_bearing() const int32_t ModeAuto::wp_bearing() const
{ {
switch (_mode) { switch (_mode) {
case AutoMode::CIRCLE: case SubMode::CIRCLE:
return copter.circle_nav->get_bearing_to_target(); return copter.circle_nav->get_bearing_to_target();
case AutoMode::WP: case SubMode::WP:
case AutoMode::CIRCLE_MOVE_TO_EDGE: case SubMode::CIRCLE_MOVE_TO_EDGE:
default: default:
return wp_nav->get_wp_bearing_to_destination(); return wp_nav->get_wp_bearing_to_destination();
} }
@ -661,11 +661,11 @@ int32_t ModeAuto::wp_bearing() const
bool ModeAuto::get_wp(Location& destination) bool ModeAuto::get_wp(Location& destination)
{ {
switch (_mode) { switch (_mode) {
case AutoMode::NAVGUIDED: case SubMode::NAVGUIDED:
return copter.mode_guided.get_wp(destination); return copter.mode_guided.get_wp(destination);
case AutoMode::WP: case SubMode::WP:
return wp_nav->get_oa_wp_destination(destination); return wp_nav->get_oa_wp_destination(destination);
case AutoMode::RTL: case SubMode::RTL:
return copter.mode_rtl.get_wp(destination); return copter.mode_rtl.get_wp(destination);
default: default:
return false; return false;
@ -955,7 +955,7 @@ void ModeAuto::loiter_to_alt_run()
if (!loiter_to_alt.loiter_start_done) { if (!loiter_to_alt.loiter_start_done) {
loiter_nav->clear_pilot_desired_acceleration(); loiter_nav->clear_pilot_desired_acceleration();
loiter_nav->init_target(); loiter_nav->init_target();
_mode = AutoMode::LOITER_TO_ALT; _mode = SubMode::LOITER_TO_ALT;
loiter_to_alt.loiter_start_done = true; loiter_to_alt.loiter_start_done = true;
} }
const float alt_error_cm = copter.current_loc.alt - loiter_to_alt.alt; const float alt_error_cm = copter.current_loc.alt - loiter_to_alt.alt;
@ -989,7 +989,7 @@ void ModeAuto::loiter_to_alt_run()
// auto_payload_place_start - initialises controller to implement placement of a load // auto_payload_place_start - initialises controller to implement placement of a load
void ModeAuto::payload_place_start(const Vector3f& destination) void ModeAuto::payload_place_start(const Vector3f& destination)
{ {
_mode = AutoMode::NAV_PAYLOAD_PLACE; _mode = SubMode::NAV_PAYLOAD_PLACE;
nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover_Start; nav_payload_place.state = PayloadPlaceStateType_Calibrating_Hover_Start;
// initialise loiter target destination // initialise loiter target destination
@ -1153,7 +1153,7 @@ void ModeAuto::do_nav_wp(const AP_Mission::Mission_Command& cmd)
return; return;
} }
_mode = AutoMode::WP; _mode = SubMode::WP;
// this will be used to remember the time in millis after we reach or pass the WP. // this will be used to remember the time in millis after we reach or pass the WP.
loiter_time = 0; loiter_time = 0;
@ -1309,7 +1309,7 @@ void ModeAuto::do_loiter_to_alt(const AP_Mission::Mission_Command& cmd)
{ {
// re-use loiter unlimited // re-use loiter unlimited
do_loiter_unlimited(cmd); do_loiter_unlimited(cmd);
_mode = AutoMode::LOITER_TO_ALT; _mode = SubMode::LOITER_TO_ALT;
// if we aren't navigating to a location then we have to adjust // if we aren't navigating to a location then we have to adjust
// altitude for current location // altitude for current location
@ -1357,7 +1357,7 @@ void ModeAuto::do_spline_wp(const AP_Mission::Mission_Command& cmd)
return; return;
} }
_mode = AutoMode::WP; _mode = SubMode::WP;
// this will be used to remember the time in millis after we reach or pass the WP. // this will be used to remember the time in millis after we reach or pass the WP.
loiter_time = 0; loiter_time = 0;
@ -1903,7 +1903,7 @@ bool ModeAuto::verify_nav_wp(const AP_Mission::Mission_Command& cmd)
bool ModeAuto::verify_circle(const AP_Mission::Mission_Command& cmd) bool ModeAuto::verify_circle(const AP_Mission::Mission_Command& cmd)
{ {
// check if we've reached the edge // check if we've reached the edge
if (mode() == AutoMode::CIRCLE_MOVE_TO_EDGE) { if (mode() == SubMode::CIRCLE_MOVE_TO_EDGE) {
if (copter.wp_nav->reached_wp_destination()) { if (copter.wp_nav->reached_wp_destination()) {
// start circling // start circling
circle_start(); circle_start();