Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-25 01:03:59 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_WPNav: OA stores and uses original terrain alt

Browse Source
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) { if (_oa_state == AP_OAPathPlanner::OA_NOT_REQUIRED) {
_origin_oabak = _origin; _origin_oabak = _origin;
_destination_oabak = _destination; _destination_oabak = _destination;
_terrain_alt_oabak = _terrain_alt;
} }
// convert origin and destination to Locations and pass into oa // 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: case AP_OAPathPlanner::OA_NOT_REQUIRED:
if (_oa_state != oa_retstate) { if (_oa_state != oa_retstate) {
// object avoidance has become inactive so reset target to original destination // 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; _oa_state = oa_retstate;
} }
break; 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); 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); 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; _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 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 _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 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 Location _oa_destination; // intermediate destination during avoidance
}; };