From 607fa40431e835078803f267499ef462b9f293c3 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Thu, 14 Dec 2023 15:40:28 +0900 Subject: [PATCH] AR_WPNav: send next destination to OA --- libraries/AR_WPNav/AR_WPNav.h | 1 + libraries/AR_WPNav/AR_WPNav_OA.cpp | 17 ++++++++++++++--- libraries/AR_WPNav/AR_WPNav_OA.h | 2 ++ 3 files changed, 17 insertions(+), 3 deletions(-) diff --git a/libraries/AR_WPNav/AR_WPNav.h b/libraries/AR_WPNav/AR_WPNav.h index 1bfb2864aa..64bbeb9a25 100644 --- a/libraries/AR_WPNav/AR_WPNav.h +++ b/libraries/AR_WPNav/AR_WPNav.h @@ -168,6 +168,7 @@ protected: 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 index 0ecdc22c95..f1fd9886ff 100644 --- 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 index 195ed3028f..2130579eeb 100644 --- a/libraries/AR_WPNav/AR_WPNav_OA.h +++ b/libraries/AR_WPNav/AR_WPNav_OA.h @@ -36,8 +36,10 @@ private: 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 };