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

AR_WPNav: stop vehicle if OA processing fails

This commit is contained in:
Randy Mackay 2019-06-26 16:16:00 +09:00
parent 951ded1f51
commit 030eab0f86
1 changed files with 25 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
libraries/AR_WPNav/AR_WPNav.cpp
View File

@ -102,9 +102,24 @@ void AR_WPNav::update(float dt)
_last_update_ms = AP_HAL::millis();
// run path planning around obstacles
bool stop_vehicle = false;
AP_OAPathPlanner *oa = AP_OAPathPlanner::get_singleton();
if (oa != nullptr) {
_oa_active = 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);
switch (oa_retstate) {
case AP_OAPathPlanner::OA_NOT_REQUIRED:
_oa_active = false;
break;
case AP_OAPathPlanner::OA_PROCESSING:
case AP_OAPathPlanner::OA_ERROR:
// during processing or in case of error, slow vehicle to a stop
stop_vehicle = true;
_oa_active = false;
break;
case AP_OAPathPlanner::OA_SUCCESS:
_oa_active = true;
break;
}
}
if (!_oa_active) {
_oa_origin = _origin;
@ -120,6 +135,15 @@ void AR_WPNav::update(float dt)
_reached_destination = true;
}
// handle stopping vehicle if avoidance has failed
if (stop_vehicle) {
// decelerate to speed to zero and set turn rate to zero
_desired_speed_limited = _atc.get_desired_speed_accel_limited(0.0f, dt);
_desired_lat_accel = 0.0f;
_desired_turn_rate_rads = 0.0f;
return;
}
// calculate the required turn of the wheels
update_steering(current_loc, speed);