Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-23 08:13:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AR_WPNav: Reset WP origin if recovering from OA

Browse Source
This commit is contained in:
Rishabh 2020-05-21 14:31:40 +05:30 committed by Randy Mackay
parent 44f0aef5e9
commit e7d98648e6
1 changed files with 12 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
libraries/AR_WPNav/AR_WPNav.cpp
View File

@ -112,6 +112,10 @@ void AR_WPNav::update(float dt)
// run path planning around obstacles // run path planning around obstacles
bool stop_vehicle = false; bool stop_vehicle = false;
// true if OA has been recently active;
bool _oa_was_active = _oa_active;
AP_OAPathPlanner *oa = AP_OAPathPlanner::get_singleton(); AP_OAPathPlanner *oa = AP_OAPathPlanner::get_singleton();
if (oa != nullptr) { if (oa != nullptr) {
const AP_OAPathPlanner::OA_RetState oa_retstate = oa->mission_avoidance(current_loc, _origin, _destination, _oa_origin, _oa_destination); const AP_OAPathPlanner::OA_RetState oa_retstate = oa->mission_avoidance(current_loc, _origin, _destination, _oa_origin, _oa_destination);
@ -137,6 +141,14 @@ void AR_WPNav::update(float dt)
update_distance_and_bearing_to_destination(); update_distance_and_bearing_to_destination();
// check if vehicle is in recovering state after switching off OA
if (!_oa_active && _oa_was_active) {
if (oa->get_options() & AP_OAPathPlanner::OA_OPTION_WP_RESET) {
//reset wp origin to vehicles current location
_origin = current_loc;
}
}
// check if vehicle has reached the destination // check if vehicle has reached the destination
const bool near_wp = _distance_to_destination <= _radius; const bool near_wp = _distance_to_destination <= _radius;
const bool past_wp = !_oa_active && current_loc.past_interval_finish_line(_origin, _destination); const bool past_wp = !_oa_active && current_loc.past_interval_finish_line(_origin, _destination);