From 43877751ada3f33a1a8aaa85a441de29c3e77528 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Mon, 30 Sep 2024 22:02:41 +0930 Subject: [PATCH] AC_PosControl: lthall fixes --- .../AC_AttitudeControl/AC_PosControl.cpp | 19 +++++++++---------- 1 file changed, 9 insertions(+), 10 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 8fb3bd42c5af2b..04e9ed4a9bbc7d 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/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