My Fixes

libraries/AC_AttitudeControl/AC_PosControl.cpp

@@ -504,6 +504,9 @@ void AC_PosControl::soften_for_landing_xy()
 ///     This function is the default initialisation for any position control that provides position, velocity and acceleration.
 void AC_PosControl::init_xy_controller()
 {
+    // initialise offsets to target offsets and ensure offset targets are zero if they have not been updated.
+    init_offsets_xy();
+
     // set roll, pitch lean angle targets to current attitude
     const Vector3f &att_target_euler_cd = _attitude_control.get_att_target_euler_cd();
     _roll_target = att_target_euler_cd.x;
@@ -539,9 +542,6 @@ void AC_PosControl::init_xy_controller()
     // initialise ekf xy reset handler
     init_ekf_xy_reset();
 
-    // initialise offsets to target offsets and ensure offset targets are zero if they have not been updated.
-    init_offsets_xy();
-
     // initialise z_controller time out
     _last_update_xy_ticks = AP::scheduler().ticks32();
 }
@@ -797,7 +797,6 @@ void AC_PosControl::init_z_controller_stopping_point()
     init_z_controller();
 
     get_stopping_point_z_cm(_pos_target.z);
-    _pos_desired.z = _pos_target.z - (_pos_offset.z + _pos_terrain);
     _vel_desired.z = 0.0f;
     _accel_desired.z = 0.0f;
 }
@@ -819,11 +818,15 @@ void AC_PosControl::relax_z_controller(float throttle_setting)
 ///     This function is private and contains all the shared z axis initialisation functions
 void AC_PosControl::init_z_controller()
 {
+    // initialise offsets to target offsets and ensure offset targets are zero if they have not been updated.
+    init_terrain();
+    init_offsets_z();
+
     _pos_target.z = _inav.get_position_z_up_cm();
-    _pos_desired.z = _pos_target.z - _pos_offset.z;     // _pos_terrain is not included because it is initialised to zero below 
+    _pos_desired.z = _pos_target.z - (_pos_offset.z + _pos_terrain);
 
     _vel_target.z = _inav.get_velocity_z_up_cms();
-    _vel_desired.z = _vel_target.z - _vel_offset.z;
+    _vel_desired.z = _vel_target.z - (_vel_offset.z + _vel_terrain);
 
     // Reset I term of velocity PID
     _pid_vel_z.reset_filter();
@@ -833,10 +836,6 @@ void AC_PosControl::init_z_controller()
     _accel_desired.z = _accel_target.z - (_accel_offset.z + _accel_terrain);
     _pid_accel_z.reset_filter();
 
-    // initialise offsets to target offsets and ensure offset targets are zero if they have not been updated.
-    init_terrain();
-    init_offsets_z();
-
     // Set accel PID I term based on the current throttle
     // Remove the expected P term due to _accel_desired.z being constrained to _accel_max_z_cmss
     // Remove the expected FF term due to non-zero _accel_target.z
@@ -1282,7 +1281,8 @@ void AC_PosControl::get_stopping_point_xy_cm(Vector2p &stopping_point) const
 /// get_stopping_point_z_cm - calculates stopping point in NEU cm based on current position, velocity, vehicle acceleration
 void AC_PosControl::get_stopping_point_z_cm(postype_t &stopping_point) const
 {
-    const float curr_pos_z = _inav.get_position_z_up_cm();
+    float curr_pos_z = _inav.get_position_z_up_cm();
+    curr_pos_z -= (_pos_offset.z + _pos_terrain);
 
     // avoid divide by zero by using current position if kP is very low or acceleration is zero
     if (!is_positive(_p_pos_z.kP()) || !is_positive(_accel_max_z_cmss)) {