Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_SmartRTL: use enum class for Action, number entries 

we log these values, so number them explicitly
This commit is contained in:
Peter Barker 2024-06-26 23:15:05 +10:00 committed by Peter Barker
parent 2717470df1
commit 2c89622f09
2 changed files with 30 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
libraries/AP_SmartRTL/AP_SmartRTL.cpp
View File

@ -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);

32
libraries/AP_SmartRTL/AP_SmartRTL.h
View File

@ -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