Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AC_WPNav: OA supports fast waypoints with dijkstras

This commit is contained in:
Randy Mackay 2023-12-01 17:14:04 +09:00
parent 0c8f427d42
commit 6fed0dbc7a
2 changed files with 50 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

51
libraries/AC_WPNav/AC_WPNav_OA.cpp
View File

@ -77,14 +77,25 @@ bool AC_WPNav_OA::update_wpnav()
_origin_oabak = _origin; _origin_oabak = _origin;
_destination_oabak = _destination; _destination_oabak = _destination;
_terrain_alt_oabak = _terrain_alt; _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 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); 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; 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) { switch (oa_retstate) {
@ -95,11 +106,31 @@ bool AC_WPNav_OA::update_wpnav()
// trigger terrain failsafe // trigger terrain failsafe
return false; 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; _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; break;
case AP_OAPathPlanner::OA_PROCESSING: 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: case AP_OAPathPlanner::OA_ERROR:
// during processing or in case of error stop the vehicle // during processing or in case of error stop the vehicle
// by setting the oa_destination to a stopping point // by setting the oa_destination to a stopping point
@ -108,6 +139,7 @@ bool AC_WPNav_OA::update_wpnav()
Vector3f stopping_point; Vector3f stopping_point;
get_wp_stopping_point(stopping_point); get_wp_stopping_point(stopping_point);
_oa_destination = Location(stopping_point, Location::AltFrame::ABOVE_ORIGIN); _oa_destination = Location(stopping_point, Location::AltFrame::ABOVE_ORIGIN);
_oa_next_destination.zero();
if (set_wp_destination(stopping_point, false)) { if (set_wp_destination(stopping_point, false)) {
_oa_state = oa_retstate; _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 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); 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); 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)) { if (!set_wp_destination_loc(oa_destination_new)) {
// trigger terrain failsafe // trigger terrain failsafe
return false; return false;
@ -137,6 +171,17 @@ bool AC_WPNav_OA::update_wpnav()
// if new target set successfully, update oa state and destination // if new target set successfully, update oa state and destination
_oa_state = oa_retstate; _oa_state = oa_retstate;
_oa_destination = oa_destination_new; _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; break;

2
libraries/AC_WPNav/AC_WPNav_OA.h
View File

@ -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 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 _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 _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 bool _terrain_alt_oabak; // true if backup origin and destination z-axis are terrain altitudes
Location _oa_destination; // intermediate destination during avoidance Location _oa_destination; // intermediate destination during avoidance
Location _oa_next_destination; // intermediate next destination during avoidance
}; };