From 7e62e82c6d91cf3fa328acb955e32afcef4805f3 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Thu, 19 Sep 2024 10:58:49 +0930 Subject: [PATCH] AC_PosControl: fix terrain following --- libraries/AC_WPNav/AC_WPNav.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 9403b5f8e79b7..9d477bf471335 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -472,7 +472,7 @@ bool AC_WPNav::advance_wp_target_along_track(float dt) const Vector3p& psc_pos_offset_target = _pos_control.get_pos_offset_target_cm(); // get current position and adjust altitude to origin and destination's frame (i.e. _frame) - const Vector3f &curr_pos = _inav.get_position_neu_cm() - psc_pos_offset_target.tofloat(); + const Vector3f &curr_pos = _inav.get_position_neu_cm() - Vector3f{float(psc_pos_offset_target.x), float(psc_pos_offset_target.y), terr_offset}; Vector3f curr_target_vel = _pos_control.get_vel_desired_cms(); curr_target_vel.z -= _pos_control.get_vel_offset_z_cms();