Simplify init z controller

lthall · Oct 1, 2024 · 11958e4 · 11958e4
1 parent f94d04f
commit 11958e4
Showing 1 changed file with 5 additions and 3 deletions.
diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp
@@ -818,15 +818,17 @@ 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.
+    // initialise terrain targets and offsets to zero
     init_terrain();
+
+    // initialise offsets to target offsets and ensure offset targets are zero if they have not been updated.
     init_offsets_z();
 
     _pos_target.z = _inav.get_position_z_up_cm();
-    _pos_desired.z = _pos_target.z - (_pos_offset.z + _pos_terrain);
+    _pos_desired.z = _pos_target.z - _pos_offset.z;
 
     _vel_target.z = _inav.get_velocity_z_up_cms();
-    _vel_desired.z = _vel_target.z - (_vel_offset.z + _vel_terrain);
+    _vel_desired.z = _vel_target.z - _vel_offset.z;
 
     // Reset I term of velocity PID
     _pid_vel_z.reset_filter();