From 0d730e11cc07c15933a19810d73a11e0e2dbcfb1 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 15 Mar 2021 17:38:06 +0900 Subject: [PATCH] AC_WPNav: OA stores and uses original terrain alt --- libraries/AC_WPNav/AC_WPNav_OA.cpp | 5 +++-- libraries/AC_WPNav/AC_WPNav_OA.h | 1 + 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/libraries/AC_WPNav/AC_WPNav_OA.cpp b/libraries/AC_WPNav/AC_WPNav_OA.cpp index 733a5674d0..5827a91c9e 100644 --- a/libraries/AC_WPNav/AC_WPNav_OA.cpp +++ b/libraries/AC_WPNav/AC_WPNav_OA.cpp @@ -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; } } diff --git a/libraries/AC_WPNav/AC_WPNav_OA.h b/libraries/AC_WPNav/AC_WPNav_OA.h index 6df6e8fe9a..004e9b141a 100644 --- a/libraries/AC_WPNav/AC_WPNav_OA.h +++ b/libraries/AC_WPNav/AC_WPNav_OA.h @@ -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 };