mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-03-03 04:03:59 -04:00
Copter: make SmartRTLState an enum class
This commit is contained in:
parent
c4cc3659fc
commit
0fe10c6c57
@ -106,12 +106,12 @@ enum class AirMode {
|
|||||||
};
|
};
|
||||||
|
|
||||||
// Safe RTL states
|
// Safe RTL states
|
||||||
enum SmartRTLState {
|
enum class SmartRTLState : uint8_t {
|
||||||
SmartRTL_WaitForPathCleanup,
|
WAIT_FOR_PATH_CLEANUP,
|
||||||
SmartRTL_PathFollow,
|
PATH_FOLLOW,
|
||||||
SmartRTL_PreLandPosition,
|
PRELAND_POSITION,
|
||||||
SmartRTL_Descend,
|
DESCEND,
|
||||||
SmartRTL_Land
|
LAND
|
||||||
};
|
};
|
||||||
|
|
||||||
enum PayloadPlaceStateType {
|
enum PayloadPlaceStateType {
|
||||||
|
@ -1239,7 +1239,7 @@ private:
|
|||||||
void path_follow_run();
|
void path_follow_run();
|
||||||
void pre_land_position_run();
|
void pre_land_position_run();
|
||||||
void land();
|
void land();
|
||||||
SmartRTLState smart_rtl_state = SmartRTL_PathFollow;
|
SmartRTLState smart_rtl_state = SmartRTLState::PATH_FOLLOW;
|
||||||
|
|
||||||
// keep track of how long we have failed to get another return
|
// keep track of how long we have failed to get another return
|
||||||
// point while following our path home. If we take too long we
|
// point while following our path home. If we take too long we
|
||||||
|
@ -25,7 +25,7 @@ bool ModeSmartRTL::init(bool ignore_checks)
|
|||||||
auto_yaw.set_mode_to_default(true);
|
auto_yaw.set_mode_to_default(true);
|
||||||
|
|
||||||
// wait for cleanup of return path
|
// wait for cleanup of return path
|
||||||
smart_rtl_state = SmartRTL_WaitForPathCleanup;
|
smart_rtl_state = SmartRTLState::WAIT_FOR_PATH_CLEANUP;
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -41,19 +41,19 @@ void ModeSmartRTL::exit()
|
|||||||
void ModeSmartRTL::run()
|
void ModeSmartRTL::run()
|
||||||
{
|
{
|
||||||
switch (smart_rtl_state) {
|
switch (smart_rtl_state) {
|
||||||
case SmartRTL_WaitForPathCleanup:
|
case SmartRTLState::WAIT_FOR_PATH_CLEANUP:
|
||||||
wait_cleanup_run();
|
wait_cleanup_run();
|
||||||
break;
|
break;
|
||||||
case SmartRTL_PathFollow:
|
case SmartRTLState::PATH_FOLLOW:
|
||||||
path_follow_run();
|
path_follow_run();
|
||||||
break;
|
break;
|
||||||
case SmartRTL_PreLandPosition:
|
case SmartRTLState::PRELAND_POSITION:
|
||||||
pre_land_position_run();
|
pre_land_position_run();
|
||||||
break;
|
break;
|
||||||
case SmartRTL_Descend:
|
case SmartRTLState::DESCEND:
|
||||||
descent_run(); // Re-using the descend method from normal rtl mode.
|
descent_run(); // Re-using the descend method from normal rtl mode.
|
||||||
break;
|
break;
|
||||||
case SmartRTL_Land:
|
case SmartRTLState::LAND:
|
||||||
land_run(true); // Re-using the land method from normal rtl mode.
|
land_run(true); // Re-using the land method from normal rtl mode.
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
@ -61,7 +61,7 @@ void ModeSmartRTL::run()
|
|||||||
|
|
||||||
bool ModeSmartRTL::is_landing() const
|
bool ModeSmartRTL::is_landing() const
|
||||||
{
|
{
|
||||||
return smart_rtl_state == SmartRTL_Land;
|
return smart_rtl_state == SmartRTLState::LAND;
|
||||||
}
|
}
|
||||||
|
|
||||||
void ModeSmartRTL::wait_cleanup_run()
|
void ModeSmartRTL::wait_cleanup_run()
|
||||||
@ -75,7 +75,7 @@ void ModeSmartRTL::wait_cleanup_run()
|
|||||||
// check if return path is computed and if yes, begin journey home
|
// check if return path is computed and if yes, begin journey home
|
||||||
if (g2.smart_rtl.request_thorough_cleanup()) {
|
if (g2.smart_rtl.request_thorough_cleanup()) {
|
||||||
path_follow_last_pop_fail_ms = 0;
|
path_follow_last_pop_fail_ms = 0;
|
||||||
smart_rtl_state = SmartRTL_PathFollow;
|
smart_rtl_state = SmartRTLState::PATH_FOLLOW;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -100,7 +100,7 @@ void ModeSmartRTL::path_follow_run()
|
|||||||
if (g2.smart_rtl.get_num_points() == 0) {
|
if (g2.smart_rtl.get_num_points() == 0) {
|
||||||
// this is the very last point, add 2m to the target alt and move to pre-land state
|
// this is the very last point, add 2m to the target alt and move to pre-land state
|
||||||
dest_NED.z -= 2.0f;
|
dest_NED.z -= 2.0f;
|
||||||
smart_rtl_state = SmartRTL_PreLandPosition;
|
smart_rtl_state = SmartRTLState::PRELAND_POSITION;
|
||||||
wp_nav->set_wp_destination_NED(dest_NED);
|
wp_nav->set_wp_destination_NED(dest_NED);
|
||||||
} else {
|
} else {
|
||||||
// peek at the next point. this can fail if the IO task currently has the path semaphore
|
// peek at the next point. this can fail if the IO task currently has the path semaphore
|
||||||
@ -123,7 +123,7 @@ void ModeSmartRTL::path_follow_run()
|
|||||||
// We should never get here; should always have at least
|
// We should never get here; should always have at least
|
||||||
// two points and the "zero points left" is handled above.
|
// two points and the "zero points left" is handled above.
|
||||||
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
|
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
|
||||||
smart_rtl_state = SmartRTL_PreLandPosition;
|
smart_rtl_state = SmartRTLState::PRELAND_POSITION;
|
||||||
} else if (path_follow_last_pop_fail_ms == 0) {
|
} else if (path_follow_last_pop_fail_ms == 0) {
|
||||||
// first time we've failed to pop off (ever, or after a success)
|
// first time we've failed to pop off (ever, or after a success)
|
||||||
path_follow_last_pop_fail_ms = AP_HAL::millis();
|
path_follow_last_pop_fail_ms = AP_HAL::millis();
|
||||||
@ -131,7 +131,7 @@ void ModeSmartRTL::path_follow_run()
|
|||||||
// we failed to pop a point off for 10 seconds. This is
|
// we failed to pop a point off for 10 seconds. This is
|
||||||
// almost certainly a bug.
|
// almost certainly a bug.
|
||||||
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
|
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
|
||||||
smart_rtl_state = SmartRTL_PreLandPosition;
|
smart_rtl_state = SmartRTLState::PRELAND_POSITION;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -157,11 +157,11 @@ void ModeSmartRTL::pre_land_position_run()
|
|||||||
// choose descend and hold, or land based on user parameter rtl_alt_final
|
// choose descend and hold, or land based on user parameter rtl_alt_final
|
||||||
if (g.rtl_alt_final <= 0 || copter.failsafe.radio) {
|
if (g.rtl_alt_final <= 0 || copter.failsafe.radio) {
|
||||||
land_start();
|
land_start();
|
||||||
smart_rtl_state = SmartRTL_Land;
|
smart_rtl_state = SmartRTLState::LAND;
|
||||||
} else {
|
} else {
|
||||||
set_descent_target_alt(copter.g.rtl_alt_final);
|
set_descent_target_alt(copter.g.rtl_alt_final);
|
||||||
descent_start();
|
descent_start();
|
||||||
smart_rtl_state = SmartRTL_Descend;
|
smart_rtl_state = SmartRTLState::DESCEND;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -184,12 +184,12 @@ bool ModeSmartRTL::get_wp(Location& destination)
|
|||||||
{
|
{
|
||||||
// provide target in states which use wp_nav
|
// provide target in states which use wp_nav
|
||||||
switch (smart_rtl_state) {
|
switch (smart_rtl_state) {
|
||||||
case SmartRTL_WaitForPathCleanup:
|
case SmartRTLState::WAIT_FOR_PATH_CLEANUP:
|
||||||
case SmartRTL_PathFollow:
|
case SmartRTLState::PATH_FOLLOW:
|
||||||
case SmartRTL_PreLandPosition:
|
case SmartRTLState::PRELAND_POSITION:
|
||||||
case SmartRTL_Descend:
|
case SmartRTLState::DESCEND:
|
||||||
return wp_nav->get_wp_destination_loc(destination);
|
return wp_nav->get_wp_destination_loc(destination);
|
||||||
case SmartRTL_Land:
|
case SmartRTLState::LAND:
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
Loading…
Reference in New Issue
Block a user