Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Copter: ACRO Error calculation fix

This commit is contained in:
lthall 2014-08-05 21:29:25 +09:30 committed by Randy Mackay
parent 3ba1c0c4aa
commit 90dc9411a5
1 changed files with 28 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

56
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -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;
}