Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 08:38:36 -04:00
Code Issues Packages Projects Releases Wiki Activity

AR_WPNav: send next destination to OA

Browse Source
This commit is contained in:
Randy Mackay 2023-12-14 15:40:28 +09:00
parent a1e03cbbe1
commit 607fa40431
3 changed files with 17 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
libraries/AR_WPNav/AR_WPNav.h
View File

@ -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 {

17
libraries/AR_WPNav/AR_WPNav_OA.cpp
View File

@ -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:

2
libraries/AR_WPNav/AR_WPNav_OA.h
View File

@ -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
};