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

AC_WPNav: OA stores and uses original terrain alt

This commit is contained in:
Randy Mackay 2021-03-15 17:38:06 +09:00
parent da580bd1ea
commit 0d730e11cc
2 changed files with 4 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
libraries/AC_WPNav/AC_WPNav_OA.cpp
View File

@ -76,6 +76,7 @@ bool AC_WPNav_OA::update_wpnav()
if (_oa_state == AP_OAPathPlanner::OA_NOT_REQUIRED) {
_origin_oabak = _origin;
_destination_oabak = _destination;
_terrain_alt_oabak = _terrain_alt;
}
// convert origin and destination to Locations and pass into oa
@ -87,7 +88,7 @@ bool AC_WPNav_OA::update_wpnav()
case AP_OAPathPlanner::OA_NOT_REQUIRED:
if (_oa_state != oa_retstate) {
// object avoidance has become inactive so reset target to original destination
set_wp_destination(_destination_oabak, _terrain_alt);
set_wp_destination(_destination_oabak, _terrain_alt_oabak);
_oa_state = oa_retstate;
}
break;
@ -119,7 +120,7 @@ bool AC_WPNav_OA::update_wpnav()
const float dist_along_path = constrain_float(oa_destination_new.line_path_proportion(origin_loc, destination_loc), 0.0f, 1.0f);
dest_NEU.z = linear_interpolate(_origin_oabak.z, _destination_oabak.z, dist_along_path, 0.0f, 1.0f);
}
if (set_wp_destination(dest_NEU, _terrain_alt)) {
if (set_wp_destination(dest_NEU, _terrain_alt_oabak)) {
_oa_state = oa_retstate;
}
}

1
libraries/AC_WPNav/AC_WPNav_OA.h
View File

@ -40,5 +40,6 @@ protected:
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 _destination_oabak; // backup of _destination so it can be restored when oa completes
bool _terrain_alt_oabak; // true if backup origin and destination z-axis are terrain altitudes
Location _oa_destination; // intermediate destination during avoidance
};