From 5acfc77a3ca5ffc12b83857680e785fa4fbdb2c8 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Sat, 28 Sep 2019 23:08:23 +0930 Subject: [PATCH] AC_AttitudeControl: Support for Standby functions --- .../AC_AttitudeControl/AC_PosControl.cpp | 19 +++++++++++++++++++ libraries/AC_AttitudeControl/AC_PosControl.h | 6 ++++++ 2 files changed, 25 insertions(+) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index 875b70f569..add9e64f0b 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -763,6 +763,25 @@ void AC_PosControl::init_xy_controller() init_ekf_xy_reset(); } +/// standby_xyz_reset - resets I terms and removes position error +/// This function will let Loiter and Alt Hold continue to operate +/// in the event that the flight controller is in control of the +/// aircraft when in standby. +void AC_PosControl::standby_xyz_reset() +{ + // Set _pid_accel_z integrator to zero. + _pid_accel_z.set_integrator(0.0f); + + // Set the target position to the current pos. + _pos_target = _inav.get_position(); + + // Set _pid_vel_xy integrators and derivative to zero. + _pid_vel_xy.reset_filter(); + + // initialise ekf xy reset handler + init_ekf_xy_reset(); +} + /// update_xy_controller - run the horizontal position controller - should be called at 100hz or higher void AC_PosControl::update_xy_controller() { diff --git a/libraries/AC_AttitudeControl/AC_PosControl.h b/libraries/AC_AttitudeControl/AC_PosControl.h index 86db19321f..62f28fc216 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.h +++ b/libraries/AC_AttitudeControl/AC_PosControl.h @@ -164,6 +164,12 @@ public: /// this does not update the xy target void init_xy_controller(); + /// standby_xyz_reset - resets I terms and removes position error + /// This function will let Loiter and Alt Hold continue to operate + /// in the event that the flight controller is in control of the + /// aircraft when in standby. + void standby_xyz_reset(); + /// set_max_accel_xy - set the maximum horizontal acceleration in cm/s/s /// leash length will be recalculated void set_max_accel_xy(float accel_cmss);