I think this should fix it.

lthall · Oct 1, 2024 · 8b24fa6 · 8b24fa6
1 parent 11958e4
commit 8b24fa6
Showing 1 changed file with 4 additions and 3 deletions.
diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp
@@ -466,8 +466,8 @@ void AC_PosControl::init_xy_controller_stopping_point()
 {
     init_xy_controller();
 
-    get_stopping_point_xy_cm(_pos_target.xy());
-    _pos_desired.xy() = _pos_target.xy() - _pos_offset.xy();
+    get_stopping_point_xy_cm(_pos_desired.xy());
+    _pos_target.xy() = _pos_desired.xy() + _pos_offset.xy();
     _vel_desired.xy().zero();
     _accel_desired.xy().zero();
 }
@@ -796,7 +796,8 @@ void AC_PosControl::init_z_controller_stopping_point()
     // Initialise the position controller to the current throttle, position, velocity and acceleration.
     init_z_controller();
 
-    get_stopping_point_z_cm(_pos_target.z);
+    get_stopping_point_z_cm(_pos_desired.z);
+    _pos_target.z = _pos_desired.z + _pos_offset.z;
     _vel_desired.z = 0.0f;
     _accel_desired.z = 0.0f;
 }