diff --git a/libraries/AP_SmartRTL/AP_SmartRTL.cpp b/libraries/AP_SmartRTL/AP_SmartRTL.cpp index 729eb886e4..952309f66e 100644 --- a/libraries/AP_SmartRTL/AP_SmartRTL.cpp +++ b/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; } diff --git a/libraries/AP_SmartRTL/AP_SmartRTL.h b/libraries/AP_SmartRTL/AP_SmartRTL.h index 2e084b47bf..89d2c3a216 100644 --- a/libraries/AP_SmartRTL/AP_SmartRTL.h +++ b/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) diff --git a/libraries/AP_SmartRTL/examples/SmartRTL_test/SmartRTL_test.cpp b/libraries/AP_SmartRTL/examples/SmartRTL_test/SmartRTL_test.cpp index 4899bef71a..60cb57c670 100644 --- a/libraries/AP_SmartRTL/examples/SmartRTL_test/SmartRTL_test.cpp +++ b/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); }