Skip to content

Commit

Permalink
AC_WPNAV: more oa messing about
Browse files Browse the repository at this point in the history
  • Loading branch information
@rmackay9
rmackay9 committed Dec 15, 2023
1 parent 19080f7 commit 545aec5
Show file tree
Hide file tree
Showing 2 changed files with 61 additions and 6 deletions.
59 changes: 53 additions & 6 deletions libraries/AC_WPNav/AC_WPNav_OA.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -85,6 +85,7 @@ bool AC_WPNav_OA::update_wpnav()
const Location destination_loc(_destination_oabak, _terrain_alt_oabak ? Location::AltFrame::ABOVE_TERRAIN : Location::AltFrame::ABOVE_ORIGIN);
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 = false;
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,
Expand All @@ -93,8 +94,13 @@ bool AC_WPNav_OA::update_wpnav()
oa_origin_new,
oa_destination_new,
oa_next_destination_new,
dest_to_next_dest_clear,
path_planner_used);

if (_oa_state != oa_retstate) {
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "OA: state was:%d now:%d", (int)_oa_state, (int)oa_retstate);
}

switch (oa_retstate) {

case AP_OAPathPlanner::OA_NOT_REQUIRED:
Expand All @@ -105,23 +111,50 @@ bool AC_WPNav_OA::update_wpnav()
return false;
}

// also set next destination if non-zero
if (!_next_destination_oabak.is_zero()) {
set_wp_destination_next(_next_destination_oabak);
// if path from destination to next_destination is clear
if (dest_to_next_dest_clear) {
// set next destination if non-zero
if (!_next_destination_oabak.is_zero()) {
set_wp_destination_next(_next_destination_oabak);
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "OA: Next destination set");
}

// backup the clear path for use as we pass through the next waypoint
clear_path.valid = true;
clear_path.origin = destination_loc;
clear_path.destination = next_destination_loc;
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "OA: ClearPath oLat:%ld oLon:%ld dLat:%ld dLon:%ld",
(long)clear_path.origin.lat, (long)clear_path.origin.lng,
(long)clear_path.destination.lat, (long)clear_path.destination.lng);
}

_oa_state = oa_retstate;
}
break;

case AP_OAPathPlanner::OA_PROCESSING:
// let the vehicle continue to do whatever it was doing
break;
// if no change in state exit immediately
if (_oa_state == oa_retstate) {
break;
}
// determine if it is safe to continue based on previous avoidance results
if (clear_path.valid) {
if (origin_loc.same_latlon_as(clear_path.origin) && destination_loc.same_latlon_as(clear_path.destination)) {
// we are on the clear path so continue
// update oa state so we don't come back and check again
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "OA: using clear path");
_oa_state = oa_retstate;
break;
}
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "OA: have clear path but didnt match");
}
// FALLTHRU
// stop the vehicle

case AP_OAPathPlanner::OA_ERROR:
// during processing or in case of error stop the vehicle
// by setting the oa_destination to a stopping point
if (_oa_state != AP_OAPathPlanner::OA_ERROR) {
if (_oa_state != AP_OAPathPlanner::OA_PROCESSING && _oa_state != AP_OAPathPlanner::OA_ERROR) {
// calculate stopping point
Vector3f stopping_point;
get_wp_stopping_point(stopping_point);
Expand All @@ -130,6 +163,12 @@ bool AC_WPNav_OA::update_wpnav()
if (set_wp_destination(stopping_point, false)) {
_oa_state = oa_retstate;
}
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "OA: stopping");
}
// on error invalidate clear path structure
if ((_oa_state == AP_OAPathPlanner::OA_ERROR) && clear_path.valid) {
clear_path.valid = false;
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "OA: invalidated clear path");
}
break;

Expand Down Expand Up @@ -171,6 +210,14 @@ bool AC_WPNav_OA::update_wpnav()
_oa_next_destination = oa_next_destination_new;
}
}

// backup the clear path for use as we pass through the next waypoint
clear_path.valid = true;
clear_path.origin = destination_loc;
clear_path.destination = next_destination_loc;
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "OA: ClearPath2 oLat:%ld oLon:%ld dLat:%ld dLon:%ld",
(long)clear_path.origin.lat, (long)clear_path.origin.lng,
(long)clear_path.destination.lat, (long)clear_path.destination.lng);
}
break;

Expand Down
8 changes: 8 additions & 0 deletions libraries/AC_WPNav/AC_WPNav_OA.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,4 +44,12 @@ class AC_WPNav_OA : public AC_WPNav
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

// clear path structure. Allows vehicle to fly through a waypoint without stopping if earlier avoidance calls confirmed the path appeared clear
// only Dijkstras path planner supports this
struct {
bool valid; // true if a clear path has been found
Location origin; // origin of clear path
Location destination; // destination of clear path
} clear_path;
};

0 comments on commit 545aec5

Please sign in to comment.