AC_WPNav: Navigation based on type of BendyRuler

libraries/AC_WPNav/AC_WPNav_OA.cpp

@@ -112,10 +112,12 @@ bool AC_WPNav_OA::update_wpnav()
                 // convert Location to offset from EKF origin
                 Vector3f dest_NEU;
                 if (_oa_destination.get_vector_from_origin_NEU(dest_NEU)) {
-                    // calculate target altitude by calculating OA adjusted destination's distance along the original track
+                    if (oa_ptr -> get_bendy_type() == AP_OABendyRuler::OABendyType::OA_BENDY_HORIZONTAL || oa_ptr -> get_bendy_type() == AP_OABendyRuler::OABendyType::OA_BENDY_DISABLED) {
-                    // and then linear interpolate using the original track's origin and destination altitude
+                        // calculate target altitude by calculating OA adjusted destination's distance along the original track
-                    const float dist_along_path = constrain_float(oa_destination_new.line_path_proportion(origin_loc, destination_loc), 0.0f, 1.0f);
+                        // and then linear interpolate using the original track's origin and destination altitude
-                    dest_NEU.z = linear_interpolate(_origin_oabak.z, _destination_oabak.z, dist_along_path, 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);
+                    }       
                     if (set_wp_destination(dest_NEU, _terrain_alt)) {
                         _oa_state = oa_retstate;
                     }

libraries/AC_WPNav/AC_WPNav_OA.h

@@ -2,6 +2,7 @@
 
 #include <AC_WPNav/AC_WPNav.h>
 #include <AC_Avoidance/AP_OAPathPlanner.h>
+#include <AC_Avoidance/AP_OABendyRuler.h>
 
 class AC_WPNav_OA : public AC_WPNav
 {