From 734769234095f6ec96735788cdfde513d1947a83 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Wed, 25 Sep 2024 08:34:25 +0930 Subject: [PATCH] PosControlFixes --- .../AC_AttitudeControl/AC_PosControl.cpp | 30 ++++++++++++++----- libraries/AC_AttitudeControl/AC_PosControl.h | 13 +++++--- 2 files changed, 32 insertions(+), 11 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 1792f2416f143e..2097b22daccabd 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -417,7 +417,7 @@ float AC_PosControl::pos_offset_z_scaler(float pos_offset_z, float pos_offset_z_ if (is_zero(pos_offset_z_buffer)) { return 1.0; } - float pos_offset_error_z = _inav.get_position_z_up_cm() - (_pos_target.z - _pos_offset.z + pos_offset_z); + float pos_offset_error_z = _inav.get_position_z_up_cm() - (_pos_target.z + (pos_offset_z - _pos_terrain)); return constrain_float((1.0 - (fabsf(pos_offset_error_z) - 0.5 * pos_offset_z_buffer) / (0.5 * pos_offset_z_buffer)), 0.01, 1.0); } @@ -540,7 +540,7 @@ void AC_PosControl::init_xy_controller() init_ekf_xy_reset(); // initialise offsets to target offsets and ensure offset targets are zero if they have not been updated. - init_offsets_xy_cm(); + init_offsets_xy(); // initialise z_controller time out _last_update_xy_ticks = AP::scheduler().ticks32(); @@ -820,10 +820,10 @@ void AC_PosControl::relax_z_controller(float throttle_setting) void AC_PosControl::init_z_controller() { _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(); @@ -834,7 +834,8 @@ void AC_PosControl::init_z_controller() _pid_accel_z.reset_filter(); // initialise offsets to target offsets and ensure offset targets are zero if they have not been updated. - init_offsets_z_cm(); + 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 @@ -1113,6 +1114,21 @@ Vector3f AC_PosControl::lean_angles_to_accel(const Vector3f& att_target_euler) c /// Terrain +/// set the terrain position, velocity and acceleration in cm, cms and cm/s/s from EKF origin in NE frame +/// this is used to initiate the offsets when initialise the position controller or do an offset reset +void AC_PosControl::init_terrain() +{ + // set terrain position and target to zero + _pos_terrain_target = 0.0; + _pos_terrain = 0.0; + + // set velocity offset to zero + _vel_terrain = 0.0; + + // set acceleration offset to zero + _accel_terrain = 0.0; +} + // remove offsets from the position velocity and acceleration targets void AC_PosControl::init_pos_terrain_cm(float pos_terrain_cm) { @@ -1126,7 +1142,7 @@ void AC_PosControl::init_pos_terrain_cm(float pos_terrain_cm) /// set the horizontal position, velocity and acceleration offsets in cm, cms and cm/s/s from EKF origin in NE frame /// this is used to initiate the offsets when initialise the position controller or do an offset reset -void AC_PosControl::init_offsets_xy_cm() +void AC_PosControl::init_offsets_xy() { // check for offset target timeout uint32_t now_ms = AP_HAL::millis(); @@ -1148,7 +1164,7 @@ void AC_PosControl::init_offsets_xy_cm() /// set the horizontal position, velocity and acceleration offsets in cm, cms and cm/s/s from EKF origin in NE frame /// this is used to initiate the offsets when initialise the position controller or do an offset reset -void AC_PosControl::init_offsets_z_cm() +void AC_PosControl::init_offsets_z() { // check for offset target timeout uint32_t now_ms = AP_HAL::millis(); diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index 1c80338967afbb..5c66e98b901906 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -474,6 +474,11 @@ class AC_PosControl /// Terrain Following + /// set the position, velocity and acceleration offsets in cm, cms and cm/s/s from EKF origin in NE frame + /// this is used to initiate the offsets when initialise the position controller or do an offset reset + /// note that this sets the actual offsets, not the offset targets + void init_terrain(); + /// update_terrain - updates the terrain position, velocity and acceleration estimation /// this moves the estimated terrain position _pos_terrain towards the target _pos_terrain_target void update_terrain(); @@ -481,13 +486,13 @@ class AC_PosControl /// Offsets - /// set the position, velocity and acceleration offsets in cm, cms and cm/s/s from EKF origin in NE frame + /// init_offsets - set the position, velocity and acceleration offsets in cm, cms and cm/s/s from EKF origin in NE frame /// this is used to initiate the offsets when initialise the position controller or do an offset reset /// note that this sets the actual offsets, not the offset targets - void init_offsets_xy_cm(); - void init_offsets_z_cm(); + void init_offsets_xy(); + void init_offsets_z(); - /// update the position and velocity offsets + /// update_offsets - update the position and velocity offsets /// this moves the offsets (e.g _pos_offset, _vel_offset, _accel_offset) towards the targets (e.g. _pos_offset_target or _vel_offset_target) void update_offsets_xy(); void update_offsets_z();