AP_SmartRTL: renamed from SafeRTL

libraries/AP_SmartRTL/AP_SmartRTL.cpp

@@ -11,18 +11,18 @@
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
 
-#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) {

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

libraries/AP_SmartRTL/examples/SmartRTL_test/SmartRTL_test.cpp

@@ -1,4 +1,4 @@
-#include "SafeRTL_test.h"
+#include "SmartRTL_test.h"
 #include <AP_AHRS/AP_AHRS.h>
 #include <AP_Baro/AP_Baro.h>
 #include <AP_Compass/AP_Compass.h>
@@ -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)) {
+    while (!smart_rtl.request_thorough_cleanup(AP_SmartRTL::THOROUGH_CLEAN_SIMPLIFY_ONLY)) {
-        safe_rtl.run_background_cleanup();
+        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)) {
+    while (!smart_rtl.request_thorough_cleanup(AP_SmartRTL::THOROUGH_CLEAN_ALL)) {
-        safe_rtl.run_background_cleanup();
+        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<Vector3f>& 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();
+    bool num_points_match = correct_path.size() == smart_rtl.get_num_points();
-    uint16_t points_to_compare = MIN(correct_path.size(), safe_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<Vector3f>& 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,

libraries/AP_SmartRTL/examples/SmartRTL_test/SmartRTL_test.h

@@ -3,7 +3,7 @@
 #include <AP_BoardConfig/AP_BoardConfig.h>
 #include <AP_HAL/AP_HAL.h>
 #include <AP_Math/AP_Math.h>
-#include <AP_SafeRTL/AP_SafeRTL.h>
+#include <AP_SmartRTL/AP_SmartRTL.h>
 #include "../../../../ArduCopter/GCS_Copter.h"
 
 // vectors defined below: