From 6d15a72f3a29a46f676b4e0ef606a15c994d3854 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 14 Sep 2017 22:19:28 +0900 Subject: [PATCH] AP_SmartRTL: complete rename to SmartRTL --- libraries/AP_SmartRTL/AP_SmartRTL.cpp | 60 +++++++++---------- libraries/AP_SmartRTL/AP_SmartRTL.h | 46 +++++++------- .../examples/SmartRTL_test/SmartRTL_test.cpp | 10 ++-- 3 files changed, 58 insertions(+), 58 deletions(-) diff --git a/libraries/AP_SmartRTL/AP_SmartRTL.cpp b/libraries/AP_SmartRTL/AP_SmartRTL.cpp index 854623ddc6..729eb886e4 100644 --- a/libraries/AP_SmartRTL/AP_SmartRTL.cpp +++ b/libraries/AP_SmartRTL/AP_SmartRTL.cpp @@ -17,20 +17,20 @@ extern const AP_HAL::HAL& hal; const AP_Param::GroupInfo AP_SmartRTL::var_info[] = { // @Param: ACCURACY - // @DisplayName: SafeRTL accuracy - // @Description: SafeRTL accuracy. The minimum distance between points. + // @DisplayName: SmartRTL accuracy + // @Description: SmartRTL accuracy. The minimum distance between points. // @Units: m // @Range: 0 10 // @User: Advanced - AP_GROUPINFO("ACCURACY", 0, AP_SmartRTL, _accuracy, SAFERTL_ACCURACY_DEFAULT), + AP_GROUPINFO("ACCURACY", 0, AP_SmartRTL, _accuracy, SMARTRTL_ACCURACY_DEFAULT), // @Param: POINTS - // @DisplayName: SafeRTL maximum number of points on path - // @Description: SafeRTL maximum number of points on path. Set to 0 to disable SafeRTL. 100 points consumes about 3k of memory. + // @DisplayName: SmartRTL maximum number of points on path + // @Description: SmartRTL maximum number of points on path. Set to 0 to disable SmartRTL. 100 points consumes about 3k of memory. // @Range: 0 500 // @User: Advanced // @RebootRequired: True - AP_GROUPINFO("POINTS", 1, AP_SmartRTL, _points_max, SAFERTL_POINTS_DEFAULT), + AP_GROUPINFO("POINTS", 1, AP_SmartRTL, _points_max, SMARTRTL_POINTS_DEFAULT), AP_GROUPEND }; @@ -57,8 +57,8 @@ const AP_Param::GroupInfo AP_SmartRTL::var_info[] = { * for a more complete description. * * The simplification and pruning algorithms run in the background and do not -* alter the path in memory. Two definitions, SAFERTL_SIMPLIFY_TIME_US and -* SAFERTL_PRUNING_LOOP_TIME_US are used to limit how long each algorithm will +* alter the path in memory. Two definitions, SMARTRTL_SIMPLIFY_TIME_US and +* SMARTRTL_PRUNING_LOOP_TIME_US are used to limit how long each algorithm will * be run before they save their state and return. * * Both algorithms are "anytime algorithms" meaning they can be interrupted @@ -67,7 +67,7 @@ const AP_Param::GroupInfo AP_SmartRTL::var_info[] = { * * Once the algorithms have completed the simplify.complete and * prune.complete flags are set to true. The "thorough cleanup" procedure, -* which is run as the vehicle initiates the SafeRTL flight mode, waits for +* which is run as the vehicle initiates the SmartRTL flight mode, waits for * these flags to become true. This can force the vehicle to pause for a few * seconds before initiating the return journey. */ @@ -89,9 +89,9 @@ void AP_SmartRTL::init() } // constrain the path length, in case the user decided to make the path unreasonably long. - _points_max = constrain_int16(_points_max, 0, SAFERTL_POINTS_MAX); + _points_max = constrain_int16(_points_max, 0, SMARTRTL_POINTS_MAX); - // check if user has disabled SafeRTL + // check if user has disabled SmartRTL if (_points_max == 0 || !is_positive(_accuracy)) { return; } @@ -105,16 +105,16 @@ void AP_SmartRTL::init() // allocate arrays _path = (Vector3f*)calloc(_points_max, sizeof(Vector3f)); - _prune.loops_max = _points_max * SAFERTL_PRUNING_LOOP_BUFFER_LEN_MULT; + _prune.loops_max = _points_max * SMARTRTL_PRUNING_LOOP_BUFFER_LEN_MULT; _prune.loops = (prune_loop_t*)calloc(_prune.loops_max, sizeof(prune_loop_t)); - _simplify.stack_max = _points_max * SAFERTL_SIMPLIFY_STACK_LEN_MULT; + _simplify.stack_max = _points_max * SMARTRTL_SIMPLIFY_STACK_LEN_MULT; _simplify.stack = (simplify_start_finish_t*)calloc(_simplify.stack_max, sizeof(simplify_start_finish_t)); // check if memory allocation failed if (_path == nullptr || _prune.loops == nullptr || _simplify.stack == nullptr) { log_action(SRTL_DEACTIVATED_INIT_FAILED); - gcs().send_text(MAV_SEVERITY_WARNING, "SafeRTL deactivated: init failed"); + gcs().send_text(MAV_SEVERITY_WARNING, "SmartRTL deactivated: init failed"); free(_path); free(_prune.loops); free(_simplify.stack); @@ -229,13 +229,13 @@ void AP_SmartRTL::update(bool position_ok, const Vector3f& current_pos) // add the point if (add_point(current_pos)) { _last_position_save_ms = now; - } else if (AP_HAL::millis() - _last_position_save_ms > SAFERTL_TIMEOUT) { + } 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"); } } else { // check for timeout due to bad position - if (AP_HAL::millis() - _last_good_position_ms > SAFERTL_TIMEOUT) { + if (AP_HAL::millis() - _last_good_position_ms > SMARTRTL_TIMEOUT) { deactivate(SRTL_DEACTIVATED_BAD_POSITION_TIMEOUT, "bad position"); return; } @@ -327,7 +327,7 @@ void AP_SmartRTL::run_background_cleanup() // local copy of _path_points_count and _path_points_completed_limit const uint16_t path_points_count = _path_points_count; const uint16_t path_points_completed_limit = _path_points_completed_limit; - _path_points_completed_limit = SAFERTL_POINTS_MAX; + _path_points_completed_limit = SMARTRTL_POINTS_MAX; _path_sem->give(); // check if thorough cleanup is required @@ -352,9 +352,9 @@ void AP_SmartRTL::run_background_cleanup() } // routine cleanup is called regularly from run_background_cleanup -// simplifies the path after SAFERTL_CLEANUP_POINT_TRIGGER points (50 points) have been added OR -// SAFERTL_CLEANUP_POINT_MIN (10 points) have been added and the path has less than SAFERTL_CLEANUP_START_MARGIN spaces (10 spaces) remaining -// prunes the path if the path has less than SAFERTL_CLEANUP_START_MARGIN spaces (10 spaces) remaining +// simplifies the path after SMARTRTL_CLEANUP_POINT_TRIGGER points (50 points) have been added OR +// SMARTRTL_CLEANUP_POINT_MIN (10 points) have been added and the path has less than SMARTRTL_CLEANUP_START_MARGIN spaces (10 spaces) remaining +// prunes the path if the path has less than SMARTRTL_CLEANUP_START_MARGIN spaces (10 spaces) remaining void AP_SmartRTL::routine_cleanup(uint16_t path_points_count, uint16_t path_points_completed_limit) { // if simplify is running, let it run to completion @@ -389,10 +389,10 @@ void AP_SmartRTL::routine_cleanup(uint16_t path_points_count, uint16_t path_poin // calculate the number of points we could simplify const uint16_t points_to_simplify = (path_points_count > _simplify.path_points_completed) ? (path_points_count - _simplify.path_points_completed) : 0 ; - const bool low_on_space = (_path_points_max - path_points_count) <= SAFERTL_CLEANUP_START_MARGIN; + const bool low_on_space = (_path_points_max - path_points_count) <= SMARTRTL_CLEANUP_START_MARGIN; // if 50 points can be simplified or we are low on space and at least 10 points can be simplified - if ((points_to_simplify >= SAFERTL_CLEANUP_POINT_TRIGGER) || (low_on_space && (points_to_simplify >= SAFERTL_CLEANUP_POINT_MIN))) { + if ((points_to_simplify >= SMARTRTL_CLEANUP_POINT_TRIGGER) || (low_on_space && (points_to_simplify >= SMARTRTL_CLEANUP_POINT_MIN))) { restart_simplification(path_points_count); return; } @@ -400,7 +400,7 @@ void AP_SmartRTL::routine_cleanup(uint16_t path_points_count, uint16_t path_poin // we are low on space, prune if (low_on_space) { // remove at least 10 points - remove_points_by_loops(SAFERTL_CLEANUP_POINT_MIN); + remove_points_by_loops(SMARTRTL_CLEANUP_POINT_MIN); } } @@ -436,7 +436,7 @@ bool AP_SmartRTL::thorough_cleanup(uint16_t path_points_count, ThoroughCleanupTy return false; } // remove pruning points - if (!remove_points_by_loops(SAFERTL_POINTS_MAX)) { + if (!remove_points_by_loops(SMARTRTL_POINTS_MAX)) { return false; } } @@ -468,7 +468,7 @@ void AP_SmartRTL::detect_simplifications() while (_simplify.stack_count > 0) { // while there is something to do // if this method has run for long enough, exit - if (AP_HAL::micros() - start_time_us > SAFERTL_SIMPLIFY_TIME_US) { + if (AP_HAL::micros() - start_time_us > SMARTRTL_SIMPLIFY_TIME_US) { return; } @@ -493,7 +493,7 @@ void AP_SmartRTL::detect_simplifications() // if the farthest point is more than ACCURACY * 0.5 add two new elements to the _simplification_stack // so that on the next iteration we will check between start-to-farthestpoint and farthestpoint-to-end - if (max_dist > SAFERTL_SIMPLIFY_EPSILON) { + if (max_dist > SMARTRTL_SIMPLIFY_EPSILON) { // if the to-do list is full, give up on simplifying. This should never happen. if (_simplify.stack_count >= _simplify.stack_max) { _simplify.complete = true; @@ -532,7 +532,7 @@ void AP_SmartRTL::detect_loops() const uint32_t start_time_us = AP_HAL::micros(); // run for defined amount of time - while (AP_HAL::micros() - start_time_us < SAFERTL_PRUNING_LOOP_TIME_US) { + while (AP_HAL::micros() - start_time_us < SMARTRTL_PRUNING_LOOP_TIME_US) { // advance inner loop _prune.j++; @@ -551,7 +551,7 @@ void AP_SmartRTL::detect_loops() // find the closest distance between two line segments and the mid-point dist_point dp = segment_segment_dist(_path[_prune.i], _path[_prune.i-1], _path[_prune.j-1], _path[_prune.j]); - if (dp.distance < SAFERTL_PRUNING_DELTA) { + if (dp.distance < SMARTRTL_PRUNING_DELTA) { // if there is a loop here, add to loop array if (!add_loop(_prune.j, _prune.i-1, dp.midpoint)) { // if the buffer is full, stop trying to prune @@ -816,12 +816,12 @@ AP_SmartRTL::dist_point AP_SmartRTL::segment_segment_dist(const Vector3f &p1, co return {dP.length(), midpoint}; } -// de-activate SafeRTL, send warning to GCS and log to dataflash +// de-activate SmartRTL, send warning to GCS and log to dataflash void AP_SmartRTL::deactivate(SRTL_Actions action, const char *reason) { _active = false; log_action(action); - gcs().send_text(MAV_SEVERITY_WARNING, "SafeRTL deactivated: %s", reason); + gcs().send_text(MAV_SEVERITY_WARNING, "SmartRTL deactivated: %s", reason); } // logging diff --git a/libraries/AP_SmartRTL/AP_SmartRTL.h b/libraries/AP_SmartRTL/AP_SmartRTL.h index df675ac8be..2e084b47bf 100644 --- a/libraries/AP_SmartRTL/AP_SmartRTL.h +++ b/libraries/AP_SmartRTL/AP_SmartRTL.h @@ -9,21 +9,21 @@ #include // definitions and macros -#define SAFERTL_ACCURACY_DEFAULT 2.0f // default _ACCURACY parameter value. Points will be no closer than this distance (in meters) together. -#define SAFERTL_POINTS_DEFAULT 150 // default _POINTS parameter value. High numbers improve path pruning but use more memory and CPU for cleanup. Memory used will be 20bytes * this number. -#define SAFERTL_POINTS_MAX 500 // the absolute maximum number of points this library can support. -#define SAFERTL_TIMEOUT 15000 // the time in milliseconds with no points saved to the path (for whatever reason), before SafeRTL is disabled for the flight -#define SAFERTL_CLEANUP_POINT_TRIGGER 50 // simplification will trigger when this many points are added to the path -#define SAFERTL_CLEANUP_START_MARGIN 10 // routine cleanup algorithms begin when the path array has only this many empty slots remaining -#define SAFERTL_CLEANUP_POINT_MIN 10 // cleanup algorithms will remove points if they remove at least this many points -#define SAFERTL_SIMPLIFY_EPSILON (_accuracy * 0.5f) -#define SAFERTL_SIMPLIFY_STACK_LEN_MULT (2.0f/3.0f)+1 // simplify buffer size as compared to maximum number of points. - // The minimum is int((s/2-1)+min(s/2, SAFERTL_POINTS_MAX-s)), where s = pow(2, floor(log(SAFERTL_POINTS_MAX)/log(2))) - // To avoid this annoying math, a good-enough overestimate is ceil(SAFERTL_POINTS_MAX*2.0f/3.0f) -#define SAFERTL_SIMPLIFY_TIME_US 200 // maximum time (in microseconds) the simplification algorithm will run before returning -#define SAFERTL_PRUNING_DELTA (_accuracy * 0.99) // How many meters apart must two points be, such that we can assume that there is no obstacle between them. must be smaller than _ACCURACY parameter -#define SAFERTL_PRUNING_LOOP_BUFFER_LEN_MULT 0.25f // pruning loop buffer size as compared to maximum number of points -#define SAFERTL_PRUNING_LOOP_TIME_US 200 // maximum time (in microseconds) that the loop finding algorithm will run before returning +#define SMARTRTL_ACCURACY_DEFAULT 2.0f // default _ACCURACY parameter value. Points will be no closer than this distance (in meters) together. +#define SMARTRTL_POINTS_DEFAULT 150 // default _POINTS parameter value. High numbers improve path pruning but use more memory and CPU for cleanup. Memory used will be 20bytes * this number. +#define SMARTRTL_POINTS_MAX 500 // the absolute maximum number of points this library can support. +#define SMARTRTL_TIMEOUT 15000 // the time in milliseconds with no points saved to the path (for whatever reason), before SmartRTL is disabled for the flight +#define SMARTRTL_CLEANUP_POINT_TRIGGER 50 // simplification will trigger when this many points are added to the path +#define SMARTRTL_CLEANUP_START_MARGIN 10 // routine cleanup algorithms begin when the path array has only this many empty slots remaining +#define SMARTRTL_CLEANUP_POINT_MIN 10 // cleanup algorithms will remove points if they remove at least this many points +#define SMARTRTL_SIMPLIFY_EPSILON (_accuracy * 0.5f) +#define SMARTRTL_SIMPLIFY_STACK_LEN_MULT (2.0f/3.0f)+1 // simplify buffer size as compared to maximum number of points. + // The minimum is int((s/2-1)+min(s/2, SMARTRTL_POINTS_MAX-s)), where s = pow(2, floor(log(SMARTRTL_POINTS_MAX)/log(2))) + // To avoid this annoying math, a good-enough overestimate is ceil(SMARTRTL_POINTS_MAX*2.0f/3.0f) +#define SMARTRTL_SIMPLIFY_TIME_US 200 // maximum time (in microseconds) the simplification algorithm will run before returning +#define SMARTRTL_PRUNING_DELTA (_accuracy * 0.99) // How many meters apart must two points be, such that we can assume that there is no obstacle between them. must be smaller than _ACCURACY parameter +#define SMARTRTL_PRUNING_LOOP_BUFFER_LEN_MULT 0.25f // pruning loop buffer size as compared to maximum number of points +#define SMARTRTL_PRUNING_LOOP_TIME_US 200 // maximum time (in microseconds) that the loop finding algorithm will run before returning class AP_SmartRTL { @@ -48,7 +48,7 @@ public: bool pop_point(Vector3f& point); // clear return path and set return location if position_ok is true. This should be called as part of the arming procedure - // if position_ok is false, SafeRTL will not be available. + // if position_ok is false, SmartRTL will not be available. // example sketches use the method that allows providing vehicle position directly void reset_path(bool position_ok); void reset_path(bool position_ok, const Vector3f& current_pos); @@ -101,7 +101,7 @@ private: // add point to end of path bool add_point(const Vector3f& point); - // routine cleanup attempts to remove 10 points (see SAFERTL_CLEANUP_POINT_MIN definition) by simplification or loop pruning + // routine cleanup attempts to remove 10 points (see SMARTRTL_CLEANUP_POINT_MIN definition) by simplification or loop pruning void routine_cleanup(uint16_t path_points_count, uint16_t path_points_complete_limit); // thorough cleanup simplifies and prunes all loops. returns true if the cleanup was completed. @@ -159,7 +159,7 @@ private: // get the closest distance between 2 line segments and the point midway between the closest points static dist_point segment_segment_dist(const Vector3f& p1, const Vector3f& p2, const Vector3f& p3, const Vector3f& p4); - // de-activate SafeRTL, send warning to GCS and log to dataflash + // de-activate SmartRTL, send warning to GCS and log to dataflash void deactivate(SRTL_Actions action, const char *reason); // logging @@ -172,10 +172,10 @@ private: AP_Float _accuracy; AP_Int16 _points_max; - // SafeRTL State Variables - bool _active; // true if safeRTL is usable. may become unusable if the path becomes too long to keep in memory, and too convoluted to be cleaned up, SafeRTL will be permanently deactivated (for the remainder of the flight) + // SmartRTL State Variables + bool _active; // true if SmartRTL is usable. may become unusable if the path becomes too long to keep in memory, and too convoluted to be cleaned up, SmartRTL will be permanently deactivated (for the remainder of the flight) bool _example_mode; // true when being called from the example sketch, logging and background tasks are disabled - uint32_t _last_good_position_ms; // the last system time a last good position was reported. If no position is available for a while, SafeRTL will be disabled. + uint32_t _last_good_position_ms; // the last system time a last good position was reported. If no position is available for a while, SmartRTL will be disabled. uint32_t _last_position_save_ms; // the system time a position was saved to the path (used for timeout) uint32_t _thorough_clean_request_ms;// the last system time the thorough cleanup was requested (set by thorough_cleanup method, used by background cleanup) uint32_t _thorough_clean_complete_ms; // set to _thorough_clean_request_ms when the background thread completes the thorough cleanup @@ -198,11 +198,11 @@ private: bool complete; // true after simplify_detection has completed bool removal_required; // true if some simplify-able points have been found on the path, set true by detect_simplifications, set false by remove_points_by_simplify_bitmask uint16_t path_points_count; // copy of _path_points_count taken when the simply algorithm started - uint16_t path_points_completed = SAFERTL_POINTS_MAX; // number of points in that path that have already been simplified and should be ignored + uint16_t path_points_completed = SMARTRTL_POINTS_MAX; // number of points in that path that have already been simplified and should be ignored simplify_start_finish_t* stack; uint16_t stack_max; // maximum number of elements in the _simplify_stack array uint16_t stack_count; // number of elements in _simplify_stack array - Bitmask bitmask = Bitmask(SAFERTL_POINTS_MAX); // simplify algorithm clears bits for each point that can be removed + Bitmask bitmask = Bitmask(SMARTRTL_POINTS_MAX); // simplify algorithm clears bits for each point that can be removed } _simplify; // Pruning diff --git a/libraries/AP_SmartRTL/examples/SmartRTL_test/SmartRTL_test.cpp b/libraries/AP_SmartRTL/examples/SmartRTL_test/SmartRTL_test.cpp index f2cd74389d..b708dcb98a 100644 --- a/libraries/AP_SmartRTL/examples/SmartRTL_test/SmartRTL_test.cpp +++ b/libraries/AP_SmartRTL/examples/SmartRTL_test/SmartRTL_test.cpp @@ -39,7 +39,7 @@ void check_path(const std::vector &correct_path, const char* test_name void setup() { - hal.console->printf("SafeRTL test\n"); + hal.console->printf("SmartRTL test\n"); AP_BoardConfig{}.init(); smart_rtl.init(); } @@ -118,15 +118,15 @@ void check_path(const std::vector& correct_path, const char* test_name // display the first failed point and all subsequent points if (!points_match) { for (uint16_t j = failure_index; j < points_to_compare; j++) { - const Vector3f& safertl_point = smart_rtl.get_point(j); + const Vector3f& smartrtl_point = smart_rtl.get_point(j); hal.console->printf(" expected point %d to be %4.2f,%4.2f,%4.2f, got %4.2f,%4.2f,%4.2f\n", (int)j, (double)correct_path[j].x, (double)correct_path[j].y, (double)correct_path[j].z, - (double)safertl_point.x, - (double)safertl_point.y, - (double)safertl_point.z + (double)smartrtl_point.x, + (double)smartrtl_point.y, + (double)smartrtl_point.z ); } }