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

26
Rover/mode_smart_rtl.cpp
View File

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