From 162dd6d2bce821001cbeb9bef26b0c4fb7674b22 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Tue, 1 Feb 2022 01:15:17 +1030 Subject: [PATCH] AC_AttitudeControl: AC_PosControl: Remove velocity override --- libraries/AC_AttitudeControl/AC_PosControl.cpp | 11 +++-------- libraries/AC_AttitudeControl/AC_PosControl.h | 10 +--------- 2 files changed, 4 insertions(+), 17 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index d8ab060c4e..0f8b6e7c48 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -609,14 +609,9 @@ void AC_PosControl::update_xy_controller() // Velocity Controller - // check if vehicle velocity is being overridden - // todo: remove this and use input shaping - if (_flags.vehicle_horiz_vel_override) { - _flags.vehicle_horiz_vel_override = false; - } else { - _vehicle_horiz_vel = _inav.get_velocity_xy_cms(); - } - Vector2f accel_target = _pid_vel_xy.update_all(_vel_target.xy(), _vehicle_horiz_vel, _limit_vector.xy()); + const Vector2f &curr_vel = _inav.get_velocity_xy_cms(); + Vector2f accel_target = _pid_vel_xy.update_all(_vel_target.xy(), curr_vel, _limit_vector.xy()); + // acceleration to correct for velocity error and scale PID output to compensate for optical flow measurement induced EKF noise accel_target *= ahrsControlScaleXY; diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index 0af6371548..d19de89961 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -354,9 +354,6 @@ public: /// this prevents integrator buildup void set_externally_limited_xy() { _limit_vector.x = _accel_target.x; _limit_vector.y = _accel_target.y; } - // overrides the velocity process variable for one timestep - void override_vehicle_velocity_xy(const Vector2f& vel_xy) { _vehicle_horiz_vel = vel_xy; _flags.vehicle_horiz_vel_override = true; } - // lean_angles_to_accel - convert roll, pitch lean angles to lat/lon frame accelerations in cm/s/s Vector3f lean_angles_to_accel(const Vector3f& att_target_euler) const; @@ -387,11 +384,6 @@ public: protected: - // general purpose flags - struct poscontrol_flags { - uint16_t vehicle_horiz_vel_override : 1; // 1 if we should use _vehicle_horiz_vel as our velocity process variable for one timestep - } _flags; - // get throttle using vibration-resistant calculation (uses feed forward with manually calculated gain) float get_throttle_with_vibration_override(); @@ -458,7 +450,7 @@ protected: Vector3f _accel_desired; // desired acceleration in NEU cm/s/s (feed forward) Vector3f _accel_target; // acceleration target in NEU cm/s/s Vector3f _limit_vector; // the direction that the position controller is limited, zero when not limited - Vector2f _vehicle_horiz_vel; // velocity to use if _flags.vehicle_horiz_vel_override is set + float _pos_offset_target_z; // vertical position offset target, frame NEU in cm relative to the EKF origin float _pos_offset_z; // vertical position offset, frame NEU in cm relative to the EKF origin float _vel_offset_z; // vertical velocity offset in NEU cm/s calculated by pos_to_rate step