From 8d66d4b824908d328a60ffd55df21a4202a5c08b Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Sat, 28 Sep 2024 12:55:53 +0900
Subject: [PATCH] AC_PosControl: fix ekf-z-reset

---
 libraries/AC_AttitudeControl/AC_PosControl.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp
index 676f03cee8c96b..7d38fe4c76063d 100644
--- a/libraries/AC_AttitudeControl/AC_PosControl.cpp
+++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp
@@ -1529,7 +1529,7 @@ void AC_PosControl::handle_ekf_z_reset()
 
         // To zero real position shift during relative position modes like Loiter, PosHold, Guided velocity and accleration control.
         _pos_target.z = _inav.get_position_z_up_cm() + _p_pos_z.get_error();
-        _pos_desired.z = _pos_target.z - (_pos_offset.z - _pos_terrain);
+        _pos_desired.z = _pos_target.z - (_pos_offset.z + _pos_terrain);
         _vel_target.z = _inav.get_velocity_z_up_cms() + _pid_vel_z.get_error();
         _vel_desired.z = _vel_target.z - (_vel_offset.z + _vel_terrain);