From 1bdde31f6b535d265bb716280a58041afbca13b4 Mon Sep 17 00:00:00 2001 From: lthall Date: Wed, 21 May 2014 23:55:05 +0930 Subject: [PATCH] Copter: Fix zero throttle flip issue Stabilize The problem is that the init_targets is intended to ensure the controller doesn't build up the I term when the throttle is low. Because it is poorly named (or used) it have been written to do something else. Here I change it to do what the code is trying to use it to do. --- libraries/AC_AttitudeControl/AC_AttitudeControl.cpp | 13 ++----------- 1 file changed, 2 insertions(+), 11 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index f742cc082b..07cc85071c 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -72,18 +72,9 @@ void AC_AttitudeControl::set_dt(float delta_sec) // init_targets - resets target angles to current angles void AC_AttitudeControl::init_targets() { - // set earth frame angle targets to current lean angles - _angle_ef_target.x = _ahrs.roll_sensor; - _angle_ef_target.y = _ahrs.pitch_sensor; - _angle_ef_target.z = _ahrs.yaw_sensor; - - // clear body frame angle errors - _angle_bf_error.zero(); - - // clear earth-frame and body-frame feed forward rates + // ensure zero error in body frame rate controllers const Vector3f& gyro = _ins.get_gyro(); - _rate_bf_desired = gyro * AC_ATTITUDE_CONTROL_DEGX100; - frame_conversion_bf_to_ef(_rate_bf_desired,_rate_ef_desired); + _rate_bf_target = gyro * AC_ATTITUDE_CONTROL_DEGX100; } //