From 90dc9411a57ce6847d8e42c09f782bf1718a0bd8 Mon Sep 17 00:00:00 2001 From: lthall Date: Tue, 5 Aug 2014 21:29:25 +0930 Subject: [PATCH] Copter: ACRO Error calculation fix --- .../AC_AttitudeControl/AC_AttitudeControl.cpp | 56 +++++++++---------- 1 file changed, 28 insertions(+), 28 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index 33256088a7..78dcf8021c 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -329,6 +329,34 @@ void AC_AttitudeControl::rate_bf_roll_pitch_yaw(float roll_rate_bf, float pitch_ { Vector3f angle_ef_error; + float rate_change, rate_change_limit; + + // update the rate feed forward with angular acceleration limits + if (_accel_rp_max > 0.0f) { + rate_change_limit = _accel_rp_max * _dt; + + rate_change = roll_rate_bf - _rate_bf_desired.x; + rate_change = constrain_float(rate_change, -rate_change_limit, rate_change_limit); + _rate_bf_desired.x += rate_change; + + rate_change = pitch_rate_bf - _rate_bf_desired.y; + rate_change = constrain_float(rate_change, -rate_change_limit, rate_change_limit); + _rate_bf_desired.y += rate_change; + } else { + _rate_bf_desired.x = roll_rate_bf; + _rate_bf_desired.y = pitch_rate_bf; + } + + if (_accel_y_max > 0.0f) { + rate_change_limit = _accel_y_max * _dt; + + rate_change = yaw_rate_bf - _rate_bf_desired.z; + rate_change = constrain_float(rate_change, -rate_change_limit, rate_change_limit); + _rate_bf_desired.z += rate_change; + } else { + _rate_bf_desired.z = yaw_rate_bf; + } + // Update angle error if (labs(_ahrs.pitch_sensor)<_acro_angle_switch) { _acro_angle_switch = 6000; @@ -364,34 +392,6 @@ void AC_AttitudeControl::rate_bf_roll_pitch_yaw(float roll_rate_bf, float pitch_ // convert body-frame angle errors to body-frame rate targets update_rate_bf_targets(); - float rate_change, rate_change_limit; - - // update the rate feed forward with angular acceleration limits - if (_accel_rp_max > 0.0f) { - rate_change_limit = _accel_rp_max * _dt; - - rate_change = roll_rate_bf - _rate_bf_desired.x; - rate_change = constrain_float(rate_change, -rate_change_limit, rate_change_limit); - _rate_bf_desired.x += rate_change; - - rate_change = pitch_rate_bf - _rate_bf_desired.y; - rate_change = constrain_float(rate_change, -rate_change_limit, rate_change_limit); - _rate_bf_desired.y += rate_change; - } else { - _rate_bf_desired.x = roll_rate_bf; - _rate_bf_desired.y = pitch_rate_bf; - } - - if (_accel_y_max > 0.0f) { - rate_change_limit = _accel_y_max * _dt; - - rate_change = yaw_rate_bf - _rate_bf_desired.z; - rate_change = constrain_float(rate_change, -rate_change_limit, rate_change_limit); - _rate_bf_desired.z += rate_change; - } else { - _rate_bf_desired.z = yaw_rate_bf; - } - // body-frame rate commands added _rate_bf_target += _rate_bf_desired; }