aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Rover: make smartrtk substate enum class

Browse Source
This commit is contained in:
Pierre Kancir 2023-09-21 15:58:41 +02:00 committed by Peter Barker
parent c7f3d0046d
commit 499dd5ea5b
2 changed files with 19 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
Rover/mode.h
View File

@ -719,7 +719,7 @@ public:
// return distance (in meters) to destination
float get_distance_to_destination() const override { return _distance_to_destination; }
bool reached_destination() const override { return smart_rtl_state == SmartRTL_StopAtHome; }
bool reached_destination() const override { return smart_rtl_state == SmartRTLState::StopAtHome; }
// set desired speed in m/s
bool set_desired_speed(float speed) override;
@ -730,11 +730,11 @@ public:
protected:
// Safe RTL states
enum SmartRTLState {
SmartRTL_WaitForPathCleanup,
SmartRTL_PathFollow,
SmartRTL_StopAtHome,
SmartRTL_Failure
enum class SmartRTLState: uint8_t {
WaitForPathCleanup,
PathFollow,
StopAtHome,
Failure
} smart_rtl_state;
bool _enter() override;

26
Rover/mode_smart_rtl.cpp
View File

@ -22,7 +22,7 @@ bool ModeSmartRTL::_enter()
}
// init state
smart_rtl_state = SmartRTL_WaitForPathCleanup;
smart_rtl_state = SmartRTLState::WaitForPathCleanup;
_loitering = false;
return true;
@ -31,24 +31,24 @@ bool ModeSmartRTL::_enter()
void ModeSmartRTL::update()
{
switch (smart_rtl_state) {
case SmartRTL_WaitForPathCleanup:
case SmartRTLState::WaitForPathCleanup:
// check if return path is computed and if yes, begin journey home
if (g2.smart_rtl.request_thorough_cleanup()) {
smart_rtl_state = SmartRTL_PathFollow;
smart_rtl_state = SmartRTLState::PathFollow;
_load_point = true;
}
// Note: this may lead to an unnecessary 20ms slow down of the vehicle (but it is unlikely)
stop_vehicle();
break;
case SmartRTL_PathFollow:
case SmartRTLState::PathFollow:
// load point if required
if (_load_point) {
Vector3f dest_NED;
if (!g2.smart_rtl.pop_point(dest_NED)) {
// if not more points, we have reached home
gcs().send_text(MAV_SEVERITY_INFO, "Reached destination");
smart_rtl_state = SmartRTL_StopAtHome;
smart_rtl_state = SmartRTLState::StopAtHome;
break;
} else {
// peek at the next point. this can fail if the IO task currently has the path semaphore
@ -57,7 +57,7 @@ void ModeSmartRTL::update()
if (!g2.wp_nav.set_desired_location_NED(dest_NED, next_dest_NED)) {
// this should never happen because the EKF origin should already be set
gcs().send_text(MAV_SEVERITY_INFO, "SmartRTL: failed to set destination");
smart_rtl_state = SmartRTL_Failure;
smart_rtl_state = SmartRTLState::Failure;
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
}
} else {
@ -65,7 +65,7 @@ void ModeSmartRTL::update()
if (!g2.wp_nav.set_desired_location_NED(dest_NED)) {
// this should never happen because the EKF origin should already be set
gcs().send_text(MAV_SEVERITY_INFO, "SmartRTL: failed to set destination");
smart_rtl_state = SmartRTL_Failure;
smart_rtl_state = SmartRTLState::Failure;
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
}
}
@ -81,8 +81,8 @@ void ModeSmartRTL::update()
}
break;
case SmartRTL_StopAtHome:
case SmartRTL_Failure:
case SmartRTLState::StopAtHome:
case SmartRTLState::Failure:
_reached_destination = true;
// we have reached the destination
// boats loiters, rovers stop
@ -107,16 +107,16 @@ void ModeSmartRTL::update()
bool ModeSmartRTL::get_desired_location(Location& destination) const
{
switch (smart_rtl_state) {
case SmartRTL_WaitForPathCleanup:
case SmartRTLState::WaitForPathCleanup:
return false;
case SmartRTL_PathFollow:
case SmartRTLState::PathFollow:
if (g2.wp_nav.is_destination_valid()) {
destination = g2.wp_nav.get_destination();
return true;
}
return false;
case SmartRTL_StopAtHome:
case SmartRTL_Failure:
case SmartRTLState::StopAtHome:
case SmartRTLState::Failure:
return false;
}
// should never reach here but just in case