From 1362bdc338d88ae25a0e8ba643c2e790ec157bc4 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 4 Aug 2014 21:03:35 +0900 Subject: [PATCH] AC_PosControl: smooth take-off with accel PID's I term This avoids an instantaneous jump in throttle during take-off by loading the accel PID's I term with the expected change in throttle level --- libraries/AC_AttitudeControl/AC_PosControl.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_PosControl.cpp b/libraries/AC_AttitudeControl/AC_PosControl.cpp index bae032df4d..34b066cb7e 100644 --- a/libraries/AC_AttitudeControl/AC_PosControl.cpp +++ b/libraries/AC_AttitudeControl/AC_PosControl.cpp @@ -195,10 +195,11 @@ void AC_PosControl::init_takeoff() _pos_target.z = curr_pos.z + POSCONTROL_TAKEOFF_JUMP_CM; - // clear i term from acceleration controller - if (_pid_alt_accel.get_integrator() < 0) { - _pid_alt_accel.reset_I(); - } + // freeze feedforward to avoid jump + freeze_ff_z(); + + // shift difference between last motor out and hover throttle into accelerometer I + _pid_alt_accel.set_integrator(_motors.get_throttle_out()-_throttle_hover); } // is_active_z - returns true if the z-axis position controller has been run very recently