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

Copter: rename RTLState to SubMode

This commit is contained in:
Pierre Kancir 2021-04-13 17:22:04 +02:00 committed by Andrew Tridgell
parent 84d385654a
commit 4368629fb6
3 changed files with 31 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ArduCopter/mode.h
View File

@ -1130,7 +1130,7 @@ public:
bool get_wp(Location &loc) override; bool get_wp(Location &loc) override;
// RTL states // RTL states
enum class RTLState : uint8_t { enum class SubMode : uint8_t {
STARTING, STARTING,
INITIAL_CLIMB, INITIAL_CLIMB,
RETURN_HOME, RETURN_HOME,
@ -1138,7 +1138,7 @@ public:
FINAL_DESCENT, FINAL_DESCENT,
LAND LAND
}; };
RTLState state() { return _state; } SubMode state() { return _state; }
// this should probably not be exposed // this should probably not be exposed
bool state_complete() const { return _state_complete; } bool state_complete() const { return _state_complete; }
@ -1181,7 +1181,7 @@ private:
void build_path(); void build_path();
void compute_return_target(); void compute_return_target();
RTLState _state = RTLState::INITIAL_CLIMB; // records state of rtl (initial climb, returning home, etc) SubMode _state = SubMode::INITIAL_CLIMB; // records state of rtl (initial climb, returning home, etc)
bool _state_complete = false; // set to true if the current state is completed bool _state_complete = false; // set to true if the current state is completed
struct { struct {

2
ArduCopter/mode_auto.cpp
View File

@ -1832,7 +1832,7 @@ bool ModeAuto::verify_loiter_to_alt() const
bool ModeAuto::verify_RTL() bool ModeAuto::verify_RTL()
{ {
return (copter.mode_rtl.state_complete() && return (copter.mode_rtl.state_complete() &&
(copter.mode_rtl.state() == ModeRTL::RTLState::FINAL_DESCENT || copter.mode_rtl.state() == ModeRTL::RTLState::LAND) && (copter.mode_rtl.state() == ModeRTL::SubMode::FINAL_DESCENT || copter.mode_rtl.state() == ModeRTL::SubMode::LAND) &&
(motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE)); (motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE));
} }

54
ArduCopter/mode_rtl.cpp
View File

@ -19,7 +19,7 @@ bool ModeRTL::init(bool ignore_checks)
} }
// initialise waypoint and spline controller // initialise waypoint and spline controller
wp_nav->wp_and_spline_init(g.rtl_speed_cms); wp_nav->wp_and_spline_init(g.rtl_speed_cms);
_state = RTLState::STARTING; _state = SubMode::STARTING;
_state_complete = true; // see run() method below _state_complete = true; // see run() method below
terrain_following_allowed = !copter.failsafe.terrain; terrain_following_allowed = !copter.failsafe.terrain;
return true; return true;
@ -30,7 +30,7 @@ void ModeRTL::restart_without_terrain()
{ {
AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::RESTARTED_RTL); AP::logger().Write_Error(LogErrorSubsystem::NAVIGATION, LogErrorCode::RESTARTED_RTL);
terrain_following_allowed = false; terrain_following_allowed = false;
_state = RTLState::STARTING; _state = SubMode::STARTING;
_state_complete = true; _state_complete = true;
gcs().send_text(MAV_SEVERITY_CRITICAL,"Restarting RTL - Terrain data missing"); gcs().send_text(MAV_SEVERITY_CRITICAL,"Restarting RTL - Terrain data missing");
} }
@ -55,27 +55,27 @@ void ModeRTL::run(bool disarm_on_land)
// check if we need to move to next state // check if we need to move to next state
if (_state_complete) { if (_state_complete) {
switch (_state) { switch (_state) {
case RTLState::STARTING: case SubMode::STARTING:
build_path(); build_path();
climb_start(); climb_start();
break; break;
case RTLState::INITIAL_CLIMB: case SubMode::INITIAL_CLIMB:
return_start(); return_start();
break; break;
case RTLState::RETURN_HOME: case SubMode::RETURN_HOME:
loiterathome_start(); loiterathome_start();
break; break;
case RTLState::LOITER_AT_HOME: case SubMode::LOITER_AT_HOME:
if (rtl_path.land || copter.failsafe.radio) { if (rtl_path.land || copter.failsafe.radio) {
land_start(); land_start();
}else{ }else{
descent_start(); descent_start();
} }
break; break;
case RTLState::FINAL_DESCENT: case SubMode::FINAL_DESCENT:
// do nothing // do nothing
break; break;
case RTLState::LAND: case SubMode::LAND:
// do nothing - rtl_land_run will take care of disarming motors // do nothing - rtl_land_run will take care of disarming motors
break; break;
} }
@ -84,28 +84,28 @@ void ModeRTL::run(bool disarm_on_land)
// call the correct run function // call the correct run function
switch (_state) { switch (_state) {
case RTLState::STARTING: case SubMode::STARTING:
// should not be reached: // should not be reached:
_state = RTLState::INITIAL_CLIMB; _state = SubMode::INITIAL_CLIMB;
FALLTHROUGH; FALLTHROUGH;
case RTLState::INITIAL_CLIMB: case SubMode::INITIAL_CLIMB:
climb_return_run(); climb_return_run();
break; break;
case RTLState::RETURN_HOME: case SubMode::RETURN_HOME:
climb_return_run(); climb_return_run();
break; break;
case RTLState::LOITER_AT_HOME: case SubMode::LOITER_AT_HOME:
loiterathome_run(); loiterathome_run();
break; break;
case RTLState::FINAL_DESCENT: case SubMode::FINAL_DESCENT:
descent_run(); descent_run();
break; break;
case RTLState::LAND: case SubMode::LAND:
land_run(disarm_on_land); land_run(disarm_on_land);
break; break;
} }
@ -114,7 +114,7 @@ void ModeRTL::run(bool disarm_on_land)
// rtl_climb_start - initialise climb to RTL altitude // rtl_climb_start - initialise climb to RTL altitude
void ModeRTL::climb_start() void ModeRTL::climb_start()
{ {
_state = RTLState::INITIAL_CLIMB; _state = SubMode::INITIAL_CLIMB;
_state_complete = false; _state_complete = false;
// set the destination // set the destination
@ -133,7 +133,7 @@ void ModeRTL::climb_start()
// rtl_return_start - initialise return to home // rtl_return_start - initialise return to home
void ModeRTL::return_start() void ModeRTL::return_start()
{ {
_state = RTLState::RETURN_HOME; _state = SubMode::RETURN_HOME;
_state_complete = false; _state_complete = false;
if (!wp_nav->set_wp_destination_loc(rtl_path.return_target)) { if (!wp_nav->set_wp_destination_loc(rtl_path.return_target)) {
@ -190,7 +190,7 @@ void ModeRTL::climb_return_run()
// loiterathome_start - initialise return to home // loiterathome_start - initialise return to home
void ModeRTL::loiterathome_start() void ModeRTL::loiterathome_start()
{ {
_state = RTLState::LOITER_AT_HOME; _state = SubMode::LOITER_AT_HOME;
_state_complete = false; _state_complete = false;
_loiter_start_time = millis(); _loiter_start_time = millis();
@ -257,7 +257,7 @@ void ModeRTL::loiterathome_run()
// rtl_descent_start - initialise descent to final alt // rtl_descent_start - initialise descent to final alt
void ModeRTL::descent_start() void ModeRTL::descent_start()
{ {
_state = RTLState::FINAL_DESCENT; _state = SubMode::FINAL_DESCENT;
_state_complete = false; _state_complete = false;
// Set wp navigation target to above home // Set wp navigation target to above home
@ -349,7 +349,7 @@ void ModeRTL::descent_run()
// land_start - initialise controllers to loiter over home // land_start - initialise controllers to loiter over home
void ModeRTL::land_start() void ModeRTL::land_start()
{ {
_state = RTLState::LAND; _state = SubMode::LAND;
_state_complete = false; _state_complete = false;
// Set wp navigation target to above home // Set wp navigation target to above home
@ -377,7 +377,7 @@ void ModeRTL::land_start()
bool ModeRTL::is_landing() const bool ModeRTL::is_landing() const
{ {
return _state == RTLState::LAND; return _state == SubMode::LAND;
} }
// land_run - run the landing controllers to put the aircraft on the ground // land_run - run the landing controllers to put the aircraft on the ground
@ -546,13 +546,13 @@ bool ModeRTL::get_wp(Location& destination)
{ {
// provide target in states which use wp_nav // provide target in states which use wp_nav
switch (_state) { switch (_state) {
case RTLState::STARTING: case SubMode::STARTING:
case RTLState::INITIAL_CLIMB: case SubMode::INITIAL_CLIMB:
case RTLState::RETURN_HOME: case SubMode::RETURN_HOME:
case RTLState::LOITER_AT_HOME: case SubMode::LOITER_AT_HOME:
case RTLState::FINAL_DESCENT: case SubMode::FINAL_DESCENT:
return wp_nav->get_oa_wp_destination(destination); return wp_nav->get_oa_wp_destination(destination);
case RTLState::LAND: case SubMode::LAND:
return false; return false;
} }