AC_WPNav: OA stores and uses original terrain alt

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;
                     }
                 }

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
 };