Browse Source

Copter: make RTLState an enum class

c415-sdk
Pierre Kancir 4 years ago committed by Andrew Tridgell
parent
commit
c4cc3659fc
  1. 16
      ArduCopter/mode.h
  2. 2
      ArduCopter/mode_auto.cpp
  3. 62
      ArduCopter/mode_rtl.cpp

16
ArduCopter/mode.h

@ -1116,13 +1116,13 @@ public:
bool get_wp(Location &loc) override; bool get_wp(Location &loc) override;
// RTL states // RTL states
enum RTLState { enum class RTLState : uint8_t {
RTL_Starting, STARTING,
RTL_InitialClimb, INITIAL_CLIMB,
RTL_ReturnHome, RETURN_HOME,
RTL_LoiterAtHome, LOITER_AT_HOME,
RTL_FinalDescent, FINAL_DESCENT,
RTL_Land LAND
}; };
RTLState state() { return _state; } RTLState state() { return _state; }
@ -1167,7 +1167,7 @@ private:
void build_path(); void build_path();
void compute_return_target(); void compute_return_target();
RTLState _state = RTL_InitialClimb; // records state of rtl (initial climb, returning home, etc) RTLState _state = RTLState::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

@ -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::RTL_FinalDescent || copter.mode_rtl.state() == ModeRTL::RTL_Land) && (copter.mode_rtl.state() == ModeRTL::RTLState::FINAL_DESCENT || copter.mode_rtl.state() == ModeRTL::RTLState::LAND) &&
(motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE)); (motors->get_spool_state() == AP_Motors::SpoolState::GROUND_IDLE));
} }

62
ArduCopter/mode_rtl.cpp

@ -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 = RTL_Starting; _state = RTLState::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 = RTL_Starting; _state = RTLState::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 RTL_Starting: case RTLState::STARTING:
build_path(); build_path();
climb_start(); climb_start();
break; break;
case RTL_InitialClimb: case RTLState::INITIAL_CLIMB:
return_start(); return_start();
break; break;
case RTL_ReturnHome: case RTLState::RETURN_HOME:
loiterathome_start(); loiterathome_start();
break; break;
case RTL_LoiterAtHome: case RTLState::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 RTL_FinalDescent: case RTLState::FINAL_DESCENT:
// do nothing // do nothing
break; break;
case RTL_Land: case RTLState::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 RTL_Starting: case RTLState::STARTING:
// should not be reached: // should not be reached:
_state = RTL_InitialClimb; _state = RTLState::INITIAL_CLIMB;
FALLTHROUGH; FALLTHROUGH;
case RTL_InitialClimb: case RTLState::INITIAL_CLIMB:
climb_return_run(); climb_return_run();
break; break;
case RTL_ReturnHome: case RTLState::RETURN_HOME:
climb_return_run(); climb_return_run();
break; break;
case RTL_LoiterAtHome: case RTLState::LOITER_AT_HOME:
loiterathome_run(); loiterathome_run();
break; break;
case RTL_FinalDescent: case RTLState::FINAL_DESCENT:
descent_run(); descent_run();
break; break;
case RTL_Land: case RTLState::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 = RTL_InitialClimb; _state = RTLState::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 = RTL_ReturnHome; _state = RTLState::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)) {
@ -187,10 +187,10 @@ void ModeRTL::climb_return_run()
_state_complete = wp_nav->reached_wp_destination(); _state_complete = wp_nav->reached_wp_destination();
} }
// rtl_loiterathome_start - initialise return to home // loiterathome_start - initialise return to home
void ModeRTL::loiterathome_start() void ModeRTL::loiterathome_start()
{ {
_state = RTL_LoiterAtHome; _state = RTLState::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 = RTL_FinalDescent; _state = RTLState::FINAL_DESCENT;
_state_complete = false; _state_complete = false;
// Set wp navigation target to above home // Set wp navigation target to above home
@ -346,10 +346,10 @@ void ModeRTL::descent_run()
_state_complete = labs(rtl_path.descent_target.alt - copter.current_loc.alt) < 20; _state_complete = labs(rtl_path.descent_target.alt - copter.current_loc.alt) < 20;
} }
// rtl_loiterathome_start - initialise controllers to loiter over home // land_start - initialise controllers to loiter over home
void ModeRTL::land_start() void ModeRTL::land_start()
{ {
_state = RTL_Land; _state = RTLState::LAND;
_state_complete = false; _state_complete = false;
// Set wp navigation target to above home // Set wp navigation target to above home
@ -377,11 +377,11 @@ void ModeRTL::land_start()
bool ModeRTL::is_landing() const bool ModeRTL::is_landing() const
{ {
return _state == RTL_Land; return _state == RTLState::LAND;
} }
// rtl_returnhome_run - return home // land_run - run the landing controllers to put the aircraft on the ground
// called by rtl_run at 100hz or more // called by rtl_run at 100hz or more
void ModeRTL::land_run(bool disarm_on_land) void ModeRTL::land_run(bool disarm_on_land)
{ {
// check if we've completed this stage of RTL // check if we've completed this stage of RTL
@ -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 RTL_Starting: case RTLState::STARTING:
case RTL_InitialClimb: case RTLState::INITIAL_CLIMB:
case RTL_ReturnHome: case RTLState::RETURN_HOME:
case RTL_LoiterAtHome: case RTLState::LOITER_AT_HOME:
case RTL_FinalDescent: case RTLState::FINAL_DESCENT:
return wp_nav->get_oa_wp_destination(destination); return wp_nav->get_oa_wp_destination(destination);
case RTL_Land: case RTLState::LAND:
return false; return false;
} }

Loading…
Cancel
Save