AP_SmartRTL: attempts to set home in update

update will continuously attempt to set SmartRTL home as long as
position is ok and home has not be set. Updated examples to use set_home.

libraries/AP_SmartRTL/AP_SmartRTL.cpp

@@ -167,14 +167,14 @@ bool AP_SmartRTL::pop_point(Vector3f& point)
 }
 
 // clear return path and set home location.  This should be called as part of the arming procedure
-void AP_SmartRTL::reset_path(bool position_ok)
+void AP_SmartRTL::set_home(bool position_ok)
 {
     Vector3f current_pos;
     position_ok &= _ahrs.get_relative_position_NED_origin(current_pos);
-    reset_path(position_ok, current_pos);
+    set_home(position_ok, current_pos);
 }
 
-void AP_SmartRTL::reset_path(bool position_ok, const Vector3f& current_pos)
+void AP_SmartRTL::set_home(bool position_ok, const Vector3f& current_pos)
 {
     if (_path == nullptr) {
         return;
@@ -188,26 +188,30 @@ void AP_SmartRTL::reset_path(bool position_ok, const Vector3f& current_pos)
     reset_simplification();
     reset_pruning();
 
-    // de-activate if no position at take-off
+    // don't continue if no position at take-off
     if (!position_ok) {
-        deactivate(SRTL_DEACTIVATED_BAD_POSITION, "bad position");
         return;
     }
 
     // save current position as first point in path
     if (!add_point(current_pos)) {
-        deactivate(SRTL_ADD_FAILED_NO_SEMAPHORE, "failed to save initial point");
         return;
     }
 
     // successfully added point and reset path
     _last_good_position_ms = AP_HAL::millis();
     _active = true;
+    _home_saved = true;
 }
 
 // call this at 3hz (or higher) regardless of what mode the vehicle is in
 void AP_SmartRTL::update(bool position_ok, bool save_position)
 {
+    // try to save home if not already saved
+    if (position_ok && !_home_saved) {
+        set_home(true);
+    }
+
     if (!_active || !save_position) {
         return;
     }

libraries/AP_SmartRTL/AP_SmartRTL.h

@@ -50,8 +50,8 @@ public:
     // 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, 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);
+    void set_home(bool position_ok);
+    void set_home(bool position_ok, const Vector3f& current_pos);
 
     // call this at 3hz (or higher) regardless of what mode the vehicle is in
     // example sketches use method that allows providing vehicle position directly
@@ -175,6 +175,7 @@ private:
     // 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
+    bool _home_saved;   // true once home has been saved successfully by the set_home or update methods
     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)

libraries/AP_SmartRTL/examples/SmartRTL_test/SmartRTL_test.cpp

@@ -86,7 +86,7 @@ void loop()
 // reset path (i.e. clear path and add home) and upload "test_path_before" to smart_rtl
 void reset()
 {
-    smart_rtl.reset_path(true, Vector3f{0.0f, 0.0f, 0.0f});
+    smart_rtl.set_home(true, Vector3f{0.0f, 0.0f, 0.0f});
     for (Vector3f v : test_path_before) {
         smart_rtl.update(true, v);
     }