Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_AttControl: constrain earth frame yaw angle error

Browse Source 
This temporarily resolves the wobble we see in Auto when the target
heading is suddenly changed leading to a large yaw angle error
This commit is contained in:
Randy Mackay 2014-03-29 20:41:22 +09:00
parent 197683d539
commit 7bff8e9312
1 changed files with 5 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -207,16 +207,17 @@ void AC_AttitudeControl::angle_ef_roll_pitch_yaw(float roll_angle_ef, float pitc
angle_ef_error.y = wrap_180_cd_float(_angle_ef_target.y - _ahrs.pitch_sensor);
angle_ef_error.z = wrap_180_cd_float(_angle_ef_target.z - _ahrs.yaw_sensor);
// constrain the yaw angle error
if (slew_yaw) {
angle_ef_error.z = constrain_float(angle_ef_error.z,-_slew_yaw,_slew_yaw);
}
// convert earth-frame angle errors to body-frame angle errors
frame_conversion_ef_to_bf(angle_ef_error, _angle_bf_error);
// convert body-frame angle errors to body-frame rate targets
update_rate_bf_targets();
if (slew_yaw) {
_rate_bf_target.z = constrain_float(_rate_bf_target.z,-_slew_yaw,_slew_yaw);
}
// body-frame to motor outputs should be called separately
}