diff --git a/libraries/AC_WPNav/AC_WPNav_OA.cpp b/libraries/AC_WPNav/AC_WPNav_OA.cpp
index 2faa2c0666..910bd1328e 100644
--- a/libraries/AC_WPNav/AC_WPNav_OA.cpp
+++ b/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
-                    // and then linear interpolate using the original track's origin and destination altitude
-                    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 (oa_ptr -> get_bendy_type() == AP_OABendyRuler::OABendyType::OA_BENDY_HORIZONTAL || oa_ptr -> get_bendy_type() == AP_OABendyRuler::OABendyType::OA_BENDY_DISABLED) {
+                        // calculate target altitude by calculating OA adjusted destination's distance along the original track
+                        // and then linear interpolate using the original track's origin and destination altitude
+                        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;
                     }
diff --git a/libraries/AC_WPNav/AC_WPNav_OA.h b/libraries/AC_WPNav/AC_WPNav_OA.h
index 5836df5e97..6df6e8fe9a 100644
--- a/libraries/AC_WPNav/AC_WPNav_OA.h
+++ b/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
 {