diff --git a/libraries/AP_SmartRTL/AP_SmartRTL.cpp b/libraries/AP_SmartRTL/AP_SmartRTL.cpp index 173d12c8f4..ed25eee43b 100644 --- a/libraries/AP_SmartRTL/AP_SmartRTL.cpp +++ b/libraries/AP_SmartRTL/AP_SmartRTL.cpp @@ -117,7 +117,7 @@ void AP_SmartRTL::init() // check if memory allocation failed if (_path == nullptr || _prune.loops == nullptr || _simplify.stack == nullptr) { - log_action(SRTL_DEACTIVATED_INIT_FAILED); + log_action(Action::DEACTIVATED_INIT_FAILED); GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "SmartRTL deactivated: init failed"); free(_path); free(_prune.loops); @@ -150,7 +150,7 @@ bool AP_SmartRTL::pop_point(Vector3f& point) // get semaphore if (!_path_sem.take_nonblocking()) { - log_action(SRTL_POP_FAILED_NO_SEMAPHORE); + log_action(Action::POP_FAILED_NO_SEMAPHORE); return false; } @@ -180,7 +180,7 @@ bool AP_SmartRTL::peek_point(Vector3f& point) // get semaphore if (!_path_sem.take_nonblocking()) { - log_action(SRTL_PEEK_FAILED_NO_SEMAPHORE); + log_action(Action::PEEK_FAILED_NO_SEMAPHORE); return false; } @@ -268,12 +268,12 @@ void AP_SmartRTL::update(bool position_ok, const Vector3f& current_pos) _last_position_save_ms = now; } else if (AP_HAL::millis() - _last_position_save_ms > SMARTRTL_TIMEOUT) { // deactivate after timeout due to failure to save points to path (most likely due to buffer filling up) - deactivate(SRTL_DEACTIVATED_PATH_FULL_TIMEOUT, "buffer full"); + deactivate(Action::DEACTIVATED_PATH_FULL_TIMEOUT, "buffer full"); } } else { // check for timeout due to bad position if (AP_HAL::millis() - _last_good_position_ms > SMARTRTL_TIMEOUT) { - deactivate(SRTL_DEACTIVATED_BAD_POSITION_TIMEOUT, "bad position"); + deactivate(Action::DEACTIVATED_BAD_POSITION_TIMEOUT, "bad position"); return; } } @@ -322,7 +322,7 @@ bool AP_SmartRTL::add_point(const Vector3f& point) { // get semaphore if (!_path_sem.take_nonblocking()) { - log_action(SRTL_ADD_FAILED_NO_SEMAPHORE, point); + log_action(Action::ADD_FAILED_NO_SEMAPHORE, point); return false; } @@ -338,13 +338,13 @@ bool AP_SmartRTL::add_point(const Vector3f& point) // check we have space in the path if (_path_points_count >= _path_points_max) { _path_sem.give(); - log_action(SRTL_ADD_FAILED_PATH_FULL, point); + log_action(Action::ADD_FAILED_PATH_FULL, point); return false; } // add point to path _path[_path_points_count++] = point; - log_action(SRTL_POINT_ADD, point); + log_action(Action::POINT_ADD, point); _path_sem.give(); return true; @@ -672,7 +672,7 @@ void AP_SmartRTL::remove_points_by_simplify_bitmask() uint16_t removed = 0; for (uint16_t src = 1; src < _path_points_count; src++) { if (!_simplify.bitmask.get(src)) { - log_action(SRTL_POINT_SIMPLIFY, _path[src]); + log_action(Action::POINT_SIMPLIFY, _path[src]); removed++; } else { _path[dest] = _path[src]; @@ -687,7 +687,7 @@ void AP_SmartRTL::remove_points_by_simplify_bitmask() _simplify.path_points_completed = _simplify.path_points_count; } else { // this is an error that should never happen so deactivate - deactivate(SRTL_DEACTIVATED_PROGRAM_ERROR, "program error"); + deactivate(Action::DEACTIVATED_PROGRAM_ERROR, "program error"); } _path_sem.give(); @@ -724,7 +724,7 @@ bool AP_SmartRTL::remove_points_by_loops(uint16_t num_points_to_remove) // shift points after the end of the loop down by the number of points in the loop uint16_t loop_num_points_to_remove = loop.end_index - loop.start_index; for (uint16_t dest = loop.start_index + 1; dest < _path_points_count - loop_num_points_to_remove; dest++) { - log_action(SRTL_POINT_PRUNE, _path[dest]); + log_action(Action::POINT_PRUNE, _path[dest]); _path[dest] = _path[dest + loop_num_points_to_remove]; } @@ -733,7 +733,7 @@ bool AP_SmartRTL::remove_points_by_loops(uint16_t num_points_to_remove) removed_points += loop_num_points_to_remove; } else { // this is an error that should never happen so deactivate - deactivate(SRTL_DEACTIVATED_PROGRAM_ERROR, "program error"); + deactivate(Action::DEACTIVATED_PROGRAM_ERROR, "program error"); _path_sem.give(); // we return true so thorough_cleanup does not get stuck return true; @@ -862,7 +862,7 @@ AP_SmartRTL::dist_point AP_SmartRTL::segment_segment_dist(const Vector3f &p1, co } // de-activate SmartRTL, send warning to GCS and logger -void AP_SmartRTL::deactivate(SRTL_Actions action, const char *reason) +void AP_SmartRTL::deactivate(Action action, const char *reason) { _active = false; log_action(action); @@ -871,7 +871,7 @@ void AP_SmartRTL::deactivate(SRTL_Actions action, const char *reason) #if HAL_LOGGING_ENABLED // logging -void AP_SmartRTL::log_action(SRTL_Actions action, const Vector3f &point) const +void AP_SmartRTL::log_action(Action action, const Vector3f &point) const { if (!_example_mode) { AP::logger().Write_SRTL(_active, _path_points_count, _path_points_max, action, point); diff --git a/libraries/AP_SmartRTL/AP_SmartRTL.h b/libraries/AP_SmartRTL/AP_SmartRTL.h index 144762e512..0d83372cf3 100644 --- a/libraries/AP_SmartRTL/AP_SmartRTL.h +++ b/libraries/AP_SmartRTL/AP_SmartRTL.h @@ -88,19 +88,19 @@ public: private: // enums for logging latest actions - enum SRTL_Actions { - SRTL_POINT_ADD, - SRTL_POINT_PRUNE, - SRTL_POINT_SIMPLIFY, - SRTL_ADD_FAILED_NO_SEMAPHORE, - SRTL_ADD_FAILED_PATH_FULL, - SRTL_POP_FAILED_NO_SEMAPHORE, - SRTL_PEEK_FAILED_NO_SEMAPHORE, - SRTL_DEACTIVATED_INIT_FAILED, - SRTL_DEACTIVATED_BAD_POSITION, - SRTL_DEACTIVATED_BAD_POSITION_TIMEOUT, - SRTL_DEACTIVATED_PATH_FULL_TIMEOUT, - SRTL_DEACTIVATED_PROGRAM_ERROR, + enum Action : uint8_t { + POINT_ADD = 0, + POINT_PRUNE = 1, + POINT_SIMPLIFY = 2, + ADD_FAILED_NO_SEMAPHORE = 3, + ADD_FAILED_PATH_FULL = 4, + POP_FAILED_NO_SEMAPHORE = 5, + PEEK_FAILED_NO_SEMAPHORE = 6, + DEACTIVATED_INIT_FAILED = 7, + // DEACTIVATED_BAD_POSITION = 8, unused, but historical + DEACTIVATED_BAD_POSITION_TIMEOUT = 9, + DEACTIVATED_PATH_FULL_TIMEOUT = 10, + DEACTIVATED_PROGRAM_ERROR = 11, }; // enum for SRTL_OPTIONS parameter @@ -171,13 +171,13 @@ private: static dist_point segment_segment_dist(const Vector3f& p1, const Vector3f& p2, const Vector3f& p3, const Vector3f& p4); // de-activate SmartRTL, send warning to GCS and logger - void deactivate(SRTL_Actions action, const char *reason); + void deactivate(Action action, const char *reason); #if HAL_LOGGING_ENABLED // logging - void log_action(SRTL_Actions action, const Vector3f &point = Vector3f()) const; + void log_action(Action action, const Vector3f &point = Vector3f()) const; #else - void log_action(SRTL_Actions action, const Vector3f &point = Vector3f()) const {} + void log_action(Action action, const Vector3f &point = Vector3f()) const {} #endif // parameters