From a98b3bad2214c94474e286e4af7032036264c1cc Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 18 Sep 2024 20:11:29 +0900 Subject: [PATCH] AC_WPNav: fix terrain following --- libraries/AC_WPNav/AC_WPNav.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 3ce5d1e98b817a..9403b5f8e79b7c 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -531,6 +531,11 @@ bool AC_WPNav::advance_wp_target_along_track(float dt) target_accel *= sq(vel_scaler_dt); target_accel += accel_offset; + // convert final_target.z to altitude above the ekf origin + target_pos.z += _pos_control.get_pos_offset_z_cm(); + target_vel.z += _pos_control.get_vel_offset_z_cms(); + target_accel.z += _pos_control.get_accel_offset_z_cmss(); + // pass new target to the position controller _pos_control.set_pos_vel_accel(target_pos.topostype(), target_vel, target_accel);