diff --git a/libraries/AP_SafeRTL/AP_SafeRTL.cpp b/libraries/AP_SmartRTL/AP_SmartRTL.cpp similarity index 93% rename from libraries/AP_SafeRTL/AP_SafeRTL.cpp rename to libraries/AP_SmartRTL/AP_SmartRTL.cpp index 0dfb8e4d7b..854623ddc6 100644 --- a/libraries/AP_SafeRTL/AP_SafeRTL.cpp +++ b/libraries/AP_SmartRTL/AP_SmartRTL.cpp @@ -11,18 +11,18 @@ along with this program. If not, see . */ -#include "AP_SafeRTL.h" +#include "AP_SmartRTL.h" extern const AP_HAL::HAL& hal; -const AP_Param::GroupInfo AP_SafeRTL::var_info[] = { +const AP_Param::GroupInfo AP_SmartRTL::var_info[] = { // @Param: ACCURACY // @DisplayName: SafeRTL accuracy // @Description: SafeRTL accuracy. The minimum distance between points. // @Units: m // @Range: 0 10 // @User: Advanced - AP_GROUPINFO("ACCURACY", 0, AP_SafeRTL, _accuracy, SAFERTL_ACCURACY_DEFAULT), + AP_GROUPINFO("ACCURACY", 0, AP_SmartRTL, _accuracy, SAFERTL_ACCURACY_DEFAULT), // @Param: POINTS // @DisplayName: SafeRTL maximum number of points on path @@ -30,7 +30,7 @@ const AP_Param::GroupInfo AP_SafeRTL::var_info[] = { // @Range: 0 500 // @User: Advanced // @RebootRequired: True - AP_GROUPINFO("POINTS", 1, AP_SafeRTL, _points_max, SAFERTL_POINTS_DEFAULT), + AP_GROUPINFO("POINTS", 1, AP_SmartRTL, _points_max, SAFERTL_POINTS_DEFAULT), AP_GROUPEND }; @@ -72,7 +72,7 @@ const AP_Param::GroupInfo AP_SafeRTL::var_info[] = { * seconds before initiating the return journey. */ -AP_SafeRTL::AP_SafeRTL(const AP_AHRS& ahrs, bool example_mode) : +AP_SmartRTL::AP_SmartRTL(const AP_AHRS& ahrs, bool example_mode) : _ahrs(ahrs), _example_mode(example_mode) { @@ -81,7 +81,7 @@ AP_SafeRTL::AP_SafeRTL(const AP_AHRS& ahrs, bool example_mode) : } // initialise safe rtl including setting up background processes -void AP_SafeRTL::init() +void AP_SmartRTL::init() { // protect against repeated call to init if (_path != nullptr) { @@ -126,18 +126,18 @@ void AP_SafeRTL::init() // when running the example sketch, we want the cleanup tasks to run when we tell them to, not in the background (so that they can be timed.) if (!_example_mode){ // register background cleanup to run in IO thread - hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&AP_SafeRTL::run_background_cleanup, void)); + hal.scheduler->register_io_process(FUNCTOR_BIND_MEMBER(&AP_SmartRTL::run_background_cleanup, void)); } } // returns number of points on the path -uint16_t AP_SafeRTL::get_num_points() const +uint16_t AP_SmartRTL::get_num_points() const { return _path_points_count; } // get next point on the path to home, returns true on success -bool AP_SafeRTL::pop_point(Vector3f& point) +bool AP_SmartRTL::pop_point(Vector3f& point) { // check we are active if (!_active) { @@ -167,14 +167,14 @@ bool AP_SafeRTL::pop_point(Vector3f& point) } // clear return path and set home location. This should be called as part of the arming procedure -void AP_SafeRTL::reset_path(bool position_ok) +void AP_SmartRTL::reset_path(bool position_ok) { Vector3f current_pos; position_ok &= _ahrs.get_relative_position_NED_origin(current_pos); reset_path(position_ok, current_pos); } -void AP_SafeRTL::reset_path(bool position_ok, const Vector3f& current_pos) +void AP_SmartRTL::reset_path(bool position_ok, const Vector3f& current_pos) { if (_path == nullptr) { return; @@ -206,7 +206,7 @@ void AP_SafeRTL::reset_path(bool position_ok, const Vector3f& current_pos) } // call this at 3hz (or higher) regardless of what mode the vehicle is in -void AP_SafeRTL::update(bool position_ok, bool save_position) +void AP_SmartRTL::update(bool position_ok, bool save_position) { if (!_active || !save_position) { return; @@ -217,7 +217,7 @@ void AP_SafeRTL::update(bool position_ok, bool save_position) update(position_ok, current_pos); } -void AP_SafeRTL::update(bool position_ok, const Vector3f& current_pos) +void AP_SmartRTL::update(bool position_ok, const Vector3f& current_pos) { if (!_active) { return; @@ -245,7 +245,7 @@ void AP_SafeRTL::update(bool position_ok, const Vector3f& current_pos) // request thorough cleanup including simplification, pruning and removal of all unnecessary points // returns true if the thorough cleanup was completed, false if it has not yet completed // this method should be called repeatedly until it returns true before initiating the return journey -bool AP_SafeRTL::request_thorough_cleanup(ThoroughCleanupType clean_type) +bool AP_SmartRTL::request_thorough_cleanup(ThoroughCleanupType clean_type) { // this should never happen but just in case if (!_active) { @@ -271,7 +271,7 @@ bool AP_SafeRTL::request_thorough_cleanup(ThoroughCleanupType clean_type) } // cancel request for thorough cleanup -void AP_SafeRTL::cancel_request_for_thorough_cleanup() +void AP_SmartRTL::cancel_request_for_thorough_cleanup() { _thorough_clean_request_ms = 0; } @@ -281,7 +281,7 @@ void AP_SafeRTL::cancel_request_for_thorough_cleanup() // // add point to end of path (if necessary), returns true on success -bool AP_SafeRTL::add_point(const Vector3f& point) +bool AP_SmartRTL::add_point(const Vector3f& point) { // get semaphore if (!_path_sem->take_nonblocking()) { @@ -314,7 +314,7 @@ bool AP_SafeRTL::add_point(const Vector3f& point) } // run background cleanup - should be run regularly from the IO thread -void AP_SafeRTL::run_background_cleanup() +void AP_SmartRTL::run_background_cleanup() { if (!_active) { return; @@ -355,7 +355,7 @@ void AP_SafeRTL::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 -void AP_SafeRTL::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.complete) { @@ -406,7 +406,7 @@ void AP_SafeRTL::routine_cleanup(uint16_t path_points_count, uint16_t path_point // thorough cleanup simplifies and prunes all loops. returns true if the cleanup was completed. // path_points_count is _path_points_count but passed in to avoid having to take the semaphore -bool AP_SafeRTL::thorough_cleanup(uint16_t path_points_count, ThoroughCleanupType clean_type) +bool AP_SmartRTL::thorough_cleanup(uint16_t path_points_count, ThoroughCleanupType clean_type) { if (clean_type != THOROUGH_CLEAN_PRUNE_ONLY) { // restart simplify if new points have appeared on path @@ -446,7 +446,7 @@ bool AP_SafeRTL::thorough_cleanup(uint16_t path_points_count, ThoroughCleanupTyp // Simplifies a 3D path, according to the Ramer-Douglas-Peucker algorithm. // _simplify.complete is set to true when all simplifications on the path have been identified -void AP_SafeRTL::detect_simplifications() +void AP_SmartRTL::detect_simplifications() { // complete immediately if only one segment if (_simplify.path_points_count < 3) { @@ -520,7 +520,7 @@ void AP_SafeRTL::detect_simplifications() * * reset_pruning should have been called at least once before this function is called to setup the indexes (_prune.i, etc) */ -void AP_SafeRTL::detect_loops() +void AP_SmartRTL::detect_loops() { // if there are less than 4 points (3 segments), mark complete if (_prune.path_points_count < 4) { @@ -565,7 +565,7 @@ void AP_SafeRTL::detect_loops() // restart simplify if new points have been added to path // path_points_count is _path_points_count but passed in to avoid having to take the semaphore -void AP_SafeRTL::restart_simplify_if_new_points(uint16_t path_points_count) +void AP_SmartRTL::restart_simplify_if_new_points(uint16_t path_points_count) { // any difference in the number of points is because of new points being added to path if (_simplify.path_points_count != path_points_count) { @@ -574,7 +574,7 @@ void AP_SafeRTL::restart_simplify_if_new_points(uint16_t path_points_count) } // reset pruning if new points have been simplified -void AP_SafeRTL::restart_pruning_if_new_points() +void AP_SmartRTL::restart_pruning_if_new_points() { // any difference in the number of points is because of new points being added to path if (_prune.path_points_count != _simplify.path_points_completed) { @@ -583,7 +583,7 @@ void AP_SafeRTL::restart_pruning_if_new_points() } // restart simplification algorithm so that it will check new points in the path -void AP_SafeRTL::restart_simplification(uint16_t path_points_count) +void AP_SmartRTL::restart_simplification(uint16_t path_points_count) { _simplify.complete = false; _simplify.removal_required = false; @@ -593,14 +593,14 @@ void AP_SafeRTL::restart_simplification(uint16_t path_points_count) } // reset simplification algorithm so that it will re-check all points in the path -void AP_SafeRTL::reset_simplification() +void AP_SmartRTL::reset_simplification() { restart_simplification(0); _simplify.path_points_completed = 0; } // restart pruning algorithm to check new points that have arrived -void AP_SafeRTL::restart_pruning(uint16_t path_points_count) +void AP_SmartRTL::restart_pruning(uint16_t path_points_count) { _prune.complete = false; _prune.i = (path_points_count > 0) ? path_points_count - 1 : 0; @@ -609,7 +609,7 @@ void AP_SafeRTL::restart_pruning(uint16_t path_points_count) } // reset pruning algorithm so that it will re-check all points in the path -void AP_SafeRTL::reset_pruning() +void AP_SmartRTL::reset_pruning() { restart_pruning(0); _prune.loops_count = 0; // clear the loops that we've recorded @@ -617,7 +617,7 @@ void AP_SafeRTL::reset_pruning() } // remove all simplify-able points from the path -void AP_SafeRTL::remove_points_by_simplify_bitmask() +void AP_SmartRTL::remove_points_by_simplify_bitmask() { // get semaphore before modifying path if (!_path_sem->take_nonblocking()) { @@ -655,7 +655,7 @@ void AP_SafeRTL::remove_points_by_simplify_bitmask() // remove loops until at least num_point_to_delete have been removed from path // does not necessarily prune all loops // returns false if it failed to remove points (because it could not take semaphore) -bool AP_SafeRTL::remove_points_by_loops(uint16_t num_points_to_remove) +bool AP_SmartRTL::remove_points_by_loops(uint16_t num_points_to_remove) { // exit immediately if no loops to prune if (_prune.loops_count == 0) { @@ -716,7 +716,7 @@ bool AP_SafeRTL::remove_points_by_loops(uint16_t num_points_to_remove) // add loop to loops array // returns true if loop added successfully, false if loop array is full // checks if loop overlaps with an existing loop, keeps only the longer loop -bool AP_SafeRTL::add_loop(uint16_t start_index, uint16_t end_index, const Vector3f& midpoint) +bool AP_SmartRTL::add_loop(uint16_t start_index, uint16_t end_index, const Vector3f& midpoint) { // if the buffer is full, return failure if (_prune.loops_count >= _prune.loops_max) { @@ -779,7 +779,7 @@ bool AP_SafeRTL::add_loop(uint16_t start_index, uint16_t end_index, const Vector * This does not matter for the path cleanup algorithm because the pruning will still occur fine between the first * parallel segment and a segment which is directly before or after the second segment. */ -AP_SafeRTL::dist_point AP_SafeRTL::segment_segment_dist(const Vector3f &p1, const Vector3f &p2, const Vector3f &p3, const Vector3f &p4) +AP_SmartRTL::dist_point AP_SmartRTL::segment_segment_dist(const Vector3f &p1, const Vector3f &p2, const Vector3f &p3, const Vector3f &p4) { const Vector3f line1 = p2-p1; const Vector3f line2 = p4-p3; @@ -817,7 +817,7 @@ AP_SafeRTL::dist_point AP_SafeRTL::segment_segment_dist(const Vector3f &p1, cons } // de-activate SafeRTL, send warning to GCS and log to dataflash -void AP_SafeRTL::deactivate(SRTL_Actions action, const char *reason) +void AP_SmartRTL::deactivate(SRTL_Actions action, const char *reason) { _active = false; log_action(action); @@ -825,7 +825,7 @@ void AP_SafeRTL::deactivate(SRTL_Actions action, const char *reason) } // logging -void AP_SafeRTL::log_action(SRTL_Actions action, const Vector3f &point) +void AP_SmartRTL::log_action(SRTL_Actions action, const Vector3f &point) { if (!_example_mode) { DataFlash_Class::instance()->Log_Write_SRTL(_active, _path_points_count, _path_points_max, action, point); @@ -833,7 +833,7 @@ void AP_SafeRTL::log_action(SRTL_Actions action, const Vector3f &point) } // returns true if the two loops overlap (used within add_loop to determine which loops to keep or throw away) -bool AP_SafeRTL::loops_overlap(const prune_loop_t &loop1, const prune_loop_t &loop2) const +bool AP_SmartRTL::loops_overlap(const prune_loop_t &loop1, const prune_loop_t &loop2) const { // check if loop1 within loop2 if (loop1.start_index >= loop2.start_index && loop1.end_index <= loop2.end_index) { diff --git a/libraries/AP_SafeRTL/AP_SafeRTL.h b/libraries/AP_SmartRTL/AP_SmartRTL.h similarity index 98% rename from libraries/AP_SafeRTL/AP_SafeRTL.h rename to libraries/AP_SmartRTL/AP_SmartRTL.h index cc60bb6215..df675ac8be 100644 --- a/libraries/AP_SafeRTL/AP_SafeRTL.h +++ b/libraries/AP_SmartRTL/AP_SmartRTL.h @@ -25,17 +25,17 @@ #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 -class AP_SafeRTL { +class AP_SmartRTL { public: // constructor, destructor - AP_SafeRTL(const AP_AHRS& ahrs, bool example_mode = false); + AP_SmartRTL(const AP_AHRS& ahrs, bool example_mode = false); // initialise safe rtl including setting up background processes void init(); - // return true if safe_rtl is usable (it may become unusable if the user took off without GPS lock or the path became too long) + // return true if smart_rtl is usable (it may become unusable if the user took off without GPS lock or the path became too long) bool is_active() const { return _active; } // returns number of points on the path diff --git a/libraries/AP_SafeRTL/examples/SafeRTL_test/SafeRTL_test.cpp b/libraries/AP_SmartRTL/examples/SmartRTL_test/SmartRTL_test.cpp similarity index 80% rename from libraries/AP_SafeRTL/examples/SafeRTL_test/SafeRTL_test.cpp rename to libraries/AP_SmartRTL/examples/SmartRTL_test/SmartRTL_test.cpp index b34fbf9313..f2cd74389d 100644 --- a/libraries/AP_SafeRTL/examples/SafeRTL_test/SafeRTL_test.cpp +++ b/libraries/AP_SmartRTL/examples/SmartRTL_test/SmartRTL_test.cpp @@ -1,4 +1,4 @@ -#include "SafeRTL_test.h" +#include "SmartRTL_test.h" #include #include #include @@ -30,7 +30,7 @@ public: static DummyVehicle vehicle; AP_AHRS_NavEKF ahrs(vehicle.ahrs); -AP_SafeRTL safe_rtl {ahrs, true}; +AP_SmartRTL smart_rtl {ahrs, true}; void setup(); void loop(); @@ -41,7 +41,7 @@ void setup() { hal.console->printf("SafeRTL test\n"); AP_BoardConfig{}.init(); - safe_rtl.init(); + smart_rtl.init(); } void loop() @@ -53,7 +53,7 @@ void loop() hal.console->printf("--------------------\n"); - // reset path and upload "test_path_before" to safe_rtl + // reset path and upload "test_path_before" to smart_rtl reference_time = AP_HAL::micros(); reset(); run_time = AP_HAL::micros() - reference_time; @@ -63,8 +63,8 @@ void loop() // test simplifications reference_time = AP_HAL::micros(); - while (!safe_rtl.request_thorough_cleanup(AP_SafeRTL::THOROUGH_CLEAN_SIMPLIFY_ONLY)) { - safe_rtl.run_background_cleanup(); + while (!smart_rtl.request_thorough_cleanup(AP_SmartRTL::THOROUGH_CLEAN_SIMPLIFY_ONLY)) { + smart_rtl.run_background_cleanup(); } run_time = AP_HAL::micros() - reference_time; check_path(test_path_after_simplifying, "simplify", run_time); @@ -73,8 +73,8 @@ void loop() hal.scheduler->delay(5); // delay 5 milliseconds because request_through_cleanup uses millisecond timestamps reset(); reference_time = AP_HAL::micros(); - while (!safe_rtl.request_thorough_cleanup(AP_SafeRTL::THOROUGH_CLEAN_ALL)) { - safe_rtl.run_background_cleanup(); + while (!smart_rtl.request_thorough_cleanup(AP_SmartRTL::THOROUGH_CLEAN_ALL)) { + smart_rtl.run_background_cleanup(); } run_time = AP_HAL::micros() - reference_time; check_path(test_path_complete, "simplify and pruning", run_time); @@ -83,27 +83,27 @@ void loop() hal.scheduler->delay(5e3); // 5 seconds } -// reset path (i.e. clear path and add home) and upload "test_path_before" to safe_rtl +// reset path (i.e. clear path and add home) and upload "test_path_before" to smart_rtl void reset() { - safe_rtl.reset_path(true, Vector3f{0.0f, 0.0f, 0.0f}); + smart_rtl.reset_path(true, Vector3f{0.0f, 0.0f, 0.0f}); for (Vector3f v : test_path_before) { - safe_rtl.update(true, v); + smart_rtl.update(true, v); } } -// compare the vector array passed in with the path held in the safe_rtl object +// compare the vector array passed in with the path held in the smart_rtl object void check_path(const std::vector& correct_path, const char* test_name, uint32_t time_us) { // check number of points - bool num_points_match = correct_path.size() == safe_rtl.get_num_points(); - uint16_t points_to_compare = MIN(correct_path.size(), safe_rtl.get_num_points()); + bool num_points_match = correct_path.size() == smart_rtl.get_num_points(); + uint16_t points_to_compare = MIN(correct_path.size(), smart_rtl.get_num_points()); // check all points match bool points_match = true; uint16_t failure_index = 0; for (uint16_t i = 0; i < points_to_compare; i++) { - if (safe_rtl.get_point(i) != correct_path[i]) { + if (smart_rtl.get_point(i) != correct_path[i]) { failure_index = i; points_match = false; } @@ -113,12 +113,12 @@ void check_path(const std::vector& correct_path, const char* test_name hal.console->printf("%s: %s time:%u us\n", test_name, (num_points_match && points_match) ? "success" : "fail", time_us); // display number of points - hal.console->printf(" expected %u points, got %u\n", (unsigned)correct_path.size(), (unsigned)safe_rtl.get_num_points()); + hal.console->printf(" expected %u points, got %u\n", (unsigned)correct_path.size(), (unsigned)smart_rtl.get_num_points()); // 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 = safe_rtl.get_point(j); + const Vector3f& safertl_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, diff --git a/libraries/AP_SafeRTL/examples/SafeRTL_test/SafeRTL_test.h b/libraries/AP_SmartRTL/examples/SmartRTL_test/SmartRTL_test.h similarity index 99% rename from libraries/AP_SafeRTL/examples/SafeRTL_test/SafeRTL_test.h rename to libraries/AP_SmartRTL/examples/SmartRTL_test/SmartRTL_test.h index 6275edcb78..c6a1c67322 100644 --- a/libraries/AP_SafeRTL/examples/SafeRTL_test/SafeRTL_test.h +++ b/libraries/AP_SmartRTL/examples/SmartRTL_test/SmartRTL_test.h @@ -3,7 +3,7 @@ #include #include #include -#include +#include #include "../../../../ArduCopter/GCS_Copter.h" // vectors defined below: diff --git a/libraries/AP_SafeRTL/examples/SafeRTL_test/wscript b/libraries/AP_SmartRTL/examples/SmartRTL_test/wscript similarity index 100% rename from libraries/AP_SafeRTL/examples/SafeRTL_test/wscript rename to libraries/AP_SmartRTL/examples/SmartRTL_test/wscript