From 6fed0dbc7aa03419abfe84082299ddb7a788d12a Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 1 Dec 2023 17:14:04 +0900 Subject: [PATCH] AC_WPNav: OA supports fast waypoints with dijkstras --- libraries/AC_WPNav/AC_WPNav_OA.cpp | 51 ++++++++++++++++++++++++++++-- libraries/AC_WPNav/AC_WPNav_OA.h | 2 ++ 2 files changed, 50 insertions(+), 3 deletions(-) diff --git a/libraries/AC_WPNav/AC_WPNav_OA.cpp b/libraries/AC_WPNav/AC_WPNav_OA.cpp index d46e374501..b0ab5502b3 100644 --- a/libraries/AC_WPNav/AC_WPNav_OA.cpp +++ b/libraries/AC_WPNav/AC_WPNav_OA.cpp @@ -77,14 +77,25 @@ bool AC_WPNav_OA::update_wpnav() _origin_oabak = _origin; _destination_oabak = _destination; _terrain_alt_oabak = _terrain_alt; + _next_destination_oabak = _next_destination; } - // convert origin and destination to Locations and pass into oa + // convert origin, destination and next_destination to Locations and pass into oa const Location origin_loc(_origin_oabak, _terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN); const Location destination_loc(_destination_oabak, _terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN); - Location oa_origin_new, oa_destination_new; + const Location next_destination_loc(_next_destination_oabak, _terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN); + Location oa_origin_new, oa_destination_new, oa_next_destination_new; + bool dest_to_next_dest_clear = true; AP_OAPathPlanner::OAPathPlannerUsed path_planner_used = AP_OAPathPlanner::OAPathPlannerUsed::None; - const AP_OAPathPlanner::OA_RetState oa_retstate = oa_ptr->mission_avoidance(current_loc, origin_loc, destination_loc, oa_origin_new, oa_destination_new, path_planner_used); + const AP_OAPathPlanner::OA_RetState oa_retstate = oa_ptr->mission_avoidance(current_loc, + origin_loc, + destination_loc, + next_destination_loc, + oa_origin_new, + oa_destination_new, + oa_next_destination_new, + dest_to_next_dest_clear, + path_planner_used); switch (oa_retstate) { @@ -95,11 +106,31 @@ bool AC_WPNav_OA::update_wpnav() // trigger terrain failsafe return false; } + + // if path from destination to next_destination is clear + if (dest_to_next_dest_clear && (oa_ptr->get_options() & AP_OAPathPlanner::OA_OPTION_FAST_WAYPOINTS)) { + // set next destination if non-zero + if (!_next_destination_oabak.is_zero()) { + set_wp_destination_next(_next_destination_oabak); + } + } _oa_state = oa_retstate; } + + // ensure we stop at next waypoint + // Note that this check is run on every iteration even if the path planner is not active + if (!dest_to_next_dest_clear) { + force_stop_at_next_wp(); + } break; case AP_OAPathPlanner::OA_PROCESSING: + if (oa_ptr->get_options() & AP_OAPathPlanner::OA_OPTION_FAST_WAYPOINTS) { + // if fast waypoint option is set, proceed during processing + break; + } + FALLTHROUGH; + case AP_OAPathPlanner::OA_ERROR: // during processing or in case of error stop the vehicle // by setting the oa_destination to a stopping point @@ -108,6 +139,7 @@ bool AC_WPNav_OA::update_wpnav() Vector3f stopping_point; get_wp_stopping_point(stopping_point); _oa_destination = Location(stopping_point, Location::AltFrame::ABOVE_ORIGIN); + _oa_next_destination.zero(); if (set_wp_destination(stopping_point, false)) { _oa_state = oa_retstate; } @@ -130,6 +162,8 @@ bool AC_WPNav_OA::update_wpnav() Location origin_oabak_loc(_origin_oabak, _terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN); Location destination_oabak_loc(_destination_oabak, _terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN); oa_destination_new.linearly_interpolate_alt(origin_oabak_loc, destination_oabak_loc); + + // set new OA adjusted destination if (!set_wp_destination_loc(oa_destination_new)) { // trigger terrain failsafe return false; @@ -137,6 +171,17 @@ bool AC_WPNav_OA::update_wpnav() // if new target set successfully, update oa state and destination _oa_state = oa_retstate; _oa_destination = oa_destination_new; + + // if a next destination was provided then use it + if ((oa_ptr->get_options() & AP_OAPathPlanner::OA_OPTION_FAST_WAYPOINTS) && !oa_next_destination_new.is_zero()) { + // calculate oa_next_destination_new's altitude using linear interpolation between original origin and destination + // this "next destination" is still an intermediate point between the origin and destination + Location next_destination_oabak_loc(_next_destination_oabak, _terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN); + oa_next_destination_new.linearly_interpolate_alt(origin_oabak_loc, destination_oabak_loc); + if (set_wp_destination_next_loc(oa_next_destination_new)) { + _oa_next_destination = oa_next_destination_new; + } + } } break; diff --git a/libraries/AC_WPNav/AC_WPNav_OA.h b/libraries/AC_WPNav/AC_WPNav_OA.h index cf532de016..2687c92552 100644 --- a/libraries/AC_WPNav/AC_WPNav_OA.h +++ b/libraries/AC_WPNav/AC_WPNav_OA.h @@ -40,6 +40,8 @@ protected: AP_OAPathPlanner::OA_RetState _oa_state; // state of object avoidance, if OA_SUCCESS we use _oa_destination to avoid obstacles Vector3f _origin_oabak; // backup of _origin so it can be restored when oa completes Vector3f _destination_oabak; // backup of _destination so it can be restored when oa completes + Vector3f _next_destination_oabak;// backup of _next_destination so it can be restored when oa completes bool _terrain_alt_oabak; // true if backup origin and destination z-axis are terrain altitudes Location _oa_destination; // intermediate destination during avoidance + Location _oa_next_destination; // intermediate next destination during avoidance };