From 7402444ffe424afc9a958d9631ed3766354f5c9d Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Fri, 24 Dec 2021 00:54:54 +1030 Subject: [PATCH] AC_AttitudeControl: AC_PosControl: add soften for landing --- libraries/AC_AttitudeControl/AC_PosControl.cpp | 12 +++++++++++- libraries/AC_AttitudeControl/AC_PosControl.h | 3 +++ 2 files changed, 14 insertions(+), 1 deletion(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 0f8b6e7c48..b776f9a79f 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -463,9 +463,19 @@ void AC_PosControl::relax_velocity_controller_xy() _pid_vel_xy.set_integrator(_accel_target - _accel_desired); } +/// reduce response for landing +void AC_PosControl::soften_for_landing_xy() +{ + // set target position to current position + _pos_target.xy() = _inav.get_position_xy_cm().topostype(); + + // also prevent I term build up in xy velocity controller. Note + // that this flag is reset on each loop, in update_xy_controller() + set_externally_limited_xy(); +} + /// init_xy_controller - initialise the position controller to the current position, velocity, acceleration and attitude. /// This function is the default initialisation for any position control that provides position, velocity and acceleration. -/// This function is private and contains all the shared xy axis initialisation functions void AC_PosControl::init_xy_controller() { // set roll, pitch lean angle targets to current attitude diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index d19de89961..7ba799ff65 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -96,6 +96,9 @@ public: /// This function decays the output acceleration by 95% every half second to achieve a smooth transition to zero requested acceleration. void relax_velocity_controller_xy(); + /// reduce response for landing + void soften_for_landing_xy(); + // init_xy_controller - initialise the position controller to the current position, velocity, acceleration and attitude. /// This function is the default initialisation for any position control that provides position, velocity and acceleration. /// This function is private and contains all the shared xy axis initialisation functions