AR_WPNav: send next destination to OA

rmackay9 · Dec 18, 2023 · 6213e30 · 6213e30
1 parent ec0bfdb
commit 6213e30
Show file tree

Hide file tree

Showing 3 changed files with 17 additions and 3 deletions.
diff --git a/libraries/AR_WPNav/AR_WPNav.h b/libraries/AR_WPNav/AR_WPNav.h
@@ -168,6 +168,7 @@ class AR_WPNav {
     uint32_t _last_update_ms;       // system time of last call to update
     Location _origin;               // origin Location (vehicle will travel from the origin to the destination)
     Location _destination;          // destination Location when in Guided_WP
+    Location _next_destination;     // next destination Location when in Guided_WP
     bool _orig_and_dest_valid;      // true if the origin and destination have been set
     bool _reversed;                 // execute the mission by backing up
     enum class NavControllerType {

diff --git a/libraries/AR_WPNav/AR_WPNav_OA.cpp b/libraries/AR_WPNav/AR_WPNav_OA.cpp
@@ -39,17 +39,28 @@ void AR_WPNav_OA::update(float dt)
     // run path planning around obstacles
     bool stop_vehicle = false;
 
-    // backup _origin and _destination when not doing oa
+    // backup _origin, _destination and _next_destination when not doing oa
     if (!_oa_active) {
         _origin_oabak = _origin;
         _destination_oabak = _destination;
+        _next_destination_oabak = _next_destination;
     }
 
     AP_OAPathPlanner *oa = AP_OAPathPlanner::get_singleton();
     if (oa != nullptr) {
-        Location oa_origin_new, oa_destination_new;
+        Location oa_origin_new, oa_destination_new, oa_next_destination_new;
         AP_OAPathPlanner::OAPathPlannerUsed path_planner_used;
-        const AP_OAPathPlanner::OA_RetState oa_retstate = oa->mission_avoidance(current_loc, _origin_oabak, _destination_oabak, oa_origin_new, oa_destination_new, path_planner_used);
+        bool dest_to_next_dest_clear;
+        const AP_OAPathPlanner::OA_RetState oa_retstate = oa->mission_avoidance(current_loc,
+                                                                                _origin_oabak,
+                                                                                _destination_oabak,
+                                                                                _next_destination_oabak,
+                                                                                oa_origin_new,
+                                                                                oa_destination_new,
+                                                                                oa_next_destination_new,
+                                                                                dest_to_next_dest_clear,
+                                                                                path_planner_used);
+
         switch (oa_retstate) {
 
         case AP_OAPathPlanner::OA_NOT_REQUIRED:

diff --git a/libraries/AR_WPNav/AR_WPNav_OA.h b/libraries/AR_WPNav/AR_WPNav_OA.h
@@ -36,8 +36,10 @@ class AR_WPNav_OA : public AR_WPNav {
     bool _oa_active;                // true if we should use alternative destination to avoid obstacles
     Location _origin_oabak;         // backup of _origin so it can be restored when oa completes
     Location _destination_oabak;    // backup of _desitnation so it can be restored when oa completes
+    Location _next_destination_oabak; // backup of _next_destination so it can be restored when oa completes
     Location _oa_origin;            // intermediate origin during avoidance
     Location _oa_destination;       // intermediate destination during avoidance
+    Location _oa_next_destination;  // intermediate next destination during avoidance
     float _oa_distance_to_destination; // OA (object avoidance) distance from vehicle to _oa_destination in meters
     float _oa_wp_bearing_cd;        // OA adjusted heading to _oa_destination in centi-degrees
 };