Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_SmartRTL: complete rename to SmartRTL

Browse Source
This commit is contained in:
Randy Mackay 2017-09-14 22:19:28 +09:00
parent d475b91564
commit 6d15a72f3a
3 changed files with 58 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

60
libraries/AP_SmartRTL/AP_SmartRTL.cpp
View File

@ -17,20 +17,20 @@ extern const AP_HAL::HAL& hal;
const AP_Param::GroupInfo AP_SmartRTL::var_info[] = { const AP_Param::GroupInfo AP_SmartRTL::var_info[] = {
// @Param: ACCURACY // @Param: ACCURACY
// @DisplayName: SafeRTL accuracy // @DisplayName: SmartRTL accuracy
// @Description: SafeRTL accuracy. The minimum distance between points. // @Description: SmartRTL accuracy. The minimum distance between points.
// @Units: m // @Units: m
// @Range: 0 10 // @Range: 0 10
// @User: Advanced // @User: Advanced
AP_GROUPINFO("ACCURACY", 0, AP_SmartRTL, _accuracy, SAFERTL_ACCURACY_DEFAULT), AP_GROUPINFO("ACCURACY", 0, AP_SmartRTL, _accuracy, SMARTRTL_ACCURACY_DEFAULT),
// @Param: POINTS // @Param: POINTS
// @DisplayName: SafeRTL maximum number of points on path // @DisplayName: SmartRTL 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. // @Description: SmartRTL maximum number of points on path. Set to 0 to disable SmartRTL. 100 points consumes about 3k of memory.
// @Range: 0 500 // @Range: 0 500
// @User: Advanced // @User: Advanced
// @RebootRequired: True // @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 AP_GROUPEND
}; };
@ -57,8 +57,8 @@ const AP_Param::GroupInfo AP_SmartRTL::var_info[] = {
* for a more complete description. * for a more complete description.
* *
* The simplification and pruning algorithms run in the background and do not * The simplification and pruning algorithms run in the background and do not
* alter the path in memory. Two definitions, SAFERTL_SIMPLIFY_TIME_US and * alter the path in memory. Two definitions, SMARTRTL_SIMPLIFY_TIME_US and
* SAFERTL_PRUNING_LOOP_TIME_US are used to limit how long each algorithm will * SMARTRTL_PRUNING_LOOP_TIME_US are used to limit how long each algorithm will
* be run before they save their state and return. * be run before they save their state and return.
* *
* Both algorithms are "anytime algorithms" meaning they can be interrupted * 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 * Once the algorithms have completed the simplify.complete and
* prune.complete flags are set to true. The "thorough cleanup" procedure, * 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 * these flags to become true. This can force the vehicle to pause for a few
* seconds before initiating the return journey. * 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. // 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)) { if (_points_max == 0 || !is_positive(_accuracy)) {
return; return;
} }
@ -105,16 +105,16 @@ void AP_SmartRTL::init()
// allocate arrays // allocate arrays
_path = (Vector3f*)calloc(_points_max, sizeof(Vector3f)); _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)); _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)); _simplify.stack = (simplify_start_finish_t*)calloc(_simplify.stack_max, sizeof(simplify_start_finish_t));
// check if memory allocation failed // check if memory allocation failed
if (_path == nullptr || _prune.loops == nullptr || _simplify.stack == nullptr) { if (_path == nullptr || _prune.loops == nullptr || _simplify.stack == nullptr) {
log_action(SRTL_DEACTIVATED_INIT_FAILED); 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(_path);
free(_prune.loops); free(_prune.loops);
free(_simplify.stack); free(_simplify.stack);
@ -229,13 +229,13 @@ void AP_SmartRTL::update(bool position_ok, const Vector3f& current_pos)
// add the point // add the point
if (add_point(current_pos)) { if (add_point(current_pos)) {
_last_position_save_ms = now; _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 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(SRTL_DEACTIVATED_PATH_FULL_TIMEOUT, "buffer full");
} }
} else { } else {
// check for timeout due to bad position // 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"); deactivate(SRTL_DEACTIVATED_BAD_POSITION_TIMEOUT, "bad position");
return; return;
} }
@ -327,7 +327,7 @@ void AP_SmartRTL::run_background_cleanup()
// local copy of _path_points_count and _path_points_completed_limit // 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_count = _path_points_count;
const uint16_t path_points_completed_limit = _path_points_completed_limit; 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(); _path_sem->give();
// check if thorough cleanup is required // 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 // routine cleanup is called regularly from run_background_cleanup
// simplifies the path after SAFERTL_CLEANUP_POINT_TRIGGER points (50 points) have been added OR // simplifies the path after SMARTRTL_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 // 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 SAFERTL_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) 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 // 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 // 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 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 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); restart_simplification(path_points_count);
return; 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 // we are low on space, prune
if (low_on_space) { if (low_on_space) {
// remove at least 10 points // 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; return false;
} }
// remove pruning points // remove pruning points
if (!remove_points_by_loops(SAFERTL_POINTS_MAX)) { if (!remove_points_by_loops(SMARTRTL_POINTS_MAX)) {
return false; return false;
} }
} }
@ -468,7 +468,7 @@ void AP_SmartRTL::detect_simplifications()
while (_simplify.stack_count > 0) { // while there is something to do while (_simplify.stack_count > 0) { // while there is something to do
// if this method has run for long enough, exit // 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; 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 // 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 // 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 the to-do list is full, give up on simplifying. This should never happen.
if (_simplify.stack_count >= _simplify.stack_max) { if (_simplify.stack_count >= _simplify.stack_max) {
_simplify.complete = true; _simplify.complete = true;
@ -532,7 +532,7 @@ void AP_SmartRTL::detect_loops()
const uint32_t start_time_us = AP_HAL::micros(); const uint32_t start_time_us = AP_HAL::micros();
// run for defined amount of time // 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 // advance inner loop
_prune.j++; _prune.j++;
@ -551,7 +551,7 @@ void AP_SmartRTL::detect_loops()
// find the closest distance between two line segments and the mid-point // 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]); 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 there is a loop here, add to loop array
if (!add_loop(_prune.j, _prune.i-1, dp.midpoint)) { if (!add_loop(_prune.j, _prune.i-1, dp.midpoint)) {
// if the buffer is full, stop trying to prune // 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}; 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) void AP_SmartRTL::deactivate(SRTL_Actions action, const char *reason)
{ {
_active = false; _active = false;
log_action(action); log_action(action);
gcs().send_text(MAV_SEVERITY_WARNING, "SafeRTL deactivated: %s", reason); gcs().send_text(MAV_SEVERITY_WARNING, "SmartRTL deactivated: %s", reason);
} }
// logging // logging

46
libraries/AP_SmartRTL/AP_SmartRTL.h
View File

@ -9,21 +9,21 @@
#include <GCS_MAVLink/GCS.h> #include <GCS_MAVLink/GCS.h>
// definitions and macros // 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 SMARTRTL_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 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 SAFERTL_POINTS_MAX 500 // the absolute maximum number of points this library can support. #define SMARTRTL_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 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 SAFERTL_CLEANUP_POINT_TRIGGER 50 // simplification will trigger when this many points are added to the path #define SMARTRTL_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 SMARTRTL_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 SMARTRTL_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 SMARTRTL_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. #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, SAFERTL_POINTS_MAX-s)), where s = pow(2, floor(log(SAFERTL_POINTS_MAX)/log(2))) // 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(SAFERTL_POINTS_MAX*2.0f/3.0f) // To avoid this annoying math, a good-enough overestimate is ceil(SMARTRTL_POINTS_MAX*2.0f/3.0f)
#define SAFERTL_SIMPLIFY_TIME_US 200 // maximum time (in microseconds) the simplification algorithm will run before returning #define SMARTRTL_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 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 SAFERTL_PRUNING_LOOP_BUFFER_LEN_MULT 0.25f // pruning loop buffer size as compared to maximum number of points #define SMARTRTL_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_PRUNING_LOOP_TIME_US 200 // maximum time (in microseconds) that the loop finding algorithm will run before returning
class AP_SmartRTL { class AP_SmartRTL {
@ -48,7 +48,7 @@ public:
bool pop_point(Vector3f& point); 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 // 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 // example sketches use the method that allows providing vehicle position directly
void reset_path(bool position_ok); void reset_path(bool position_ok);
void reset_path(bool position_ok, const Vector3f& current_pos); void reset_path(bool position_ok, const Vector3f& current_pos);
@ -101,7 +101,7 @@ private:
// add point to end of path // add point to end of path
bool add_point(const Vector3f& point); 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); 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. // 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 // 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); 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); void deactivate(SRTL_Actions action, const char *reason);
// logging // logging
@ -172,10 +172,10 @@ private:
AP_Float _accuracy; AP_Float _accuracy;
AP_Int16 _points_max; AP_Int16 _points_max;
// SafeRTL State Variables // SmartRTL 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) 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 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 _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_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 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 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 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_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; simplify_start_finish_t* stack;
uint16_t stack_max; // maximum number of elements in the _simplify_stack array uint16_t stack_max; // maximum number of elements in the _simplify_stack array
uint16_t stack_count; // number of elements in _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; } _simplify;
// Pruning // Pruning

10
libraries/AP_SmartRTL/examples/SmartRTL_test/SmartRTL_test.cpp
View File

@ -39,7 +39,7 @@ void check_path(const std::vector<Vector3f> &correct_path, const char* test_name
void setup() void setup()
{ {
hal.console->printf("SafeRTL test\n"); hal.console->printf("SmartRTL test\n");
AP_BoardConfig{}.init(); AP_BoardConfig{}.init();
smart_rtl.init(); smart_rtl.init();
} }
@ -118,15 +118,15 @@ void check_path(const std::vector<Vector3f>& correct_path, const char* test_name
// display the first failed point and all subsequent points // display the first failed point and all subsequent points
if (!points_match) { if (!points_match) {
for (uint16_t j = failure_index; j < points_to_compare; j++) { 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", hal.console->printf(" expected point %d to be %4.2f,%4.2f,%4.2f, got %4.2f,%4.2f,%4.2f\n",
(int)j, (int)j,
(double)correct_path[j].x, (double)correct_path[j].x,
(double)correct_path[j].y, (double)correct_path[j].y,
(double)correct_path[j].z, (double)correct_path[j].z,
(double)safertl_point.x, (double)smartrtl_point.x,
(double)safertl_point.y, (double)smartrtl_point.y,
(double)safertl_point.z (double)smartrtl_point.z
); );
} }
} }