Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-26 01:33:56 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_AttutudeControl: Yaw shift fix

Browse Source
This commit is contained in:
Leonard Hall 2016-06-24 18:43:06 +09:30 committed by Randy Mackay
parent b61ae1a4a1
commit 80bda572ba
1 changed files with 1 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -515,9 +515,8 @@ Vector3f AC_AttitudeControl::euler_accel_limit(Vector3f euler_rad, Vector3f eule
void AC_AttitudeControl::shift_ef_yaw_target(float yaw_shift_cd) void AC_AttitudeControl::shift_ef_yaw_target(float yaw_shift_cd)
{ {
float yaw_shift = radians(yaw_shift_cd*0.01f); float yaw_shift = radians(yaw_shift_cd*0.01f);
_attitude_target_euler_angle.z = wrap_2PI(_attitude_target_euler_angle.z + yaw_shift);
Quaternion _attitude_target_update_quat; Quaternion _attitude_target_update_quat;
_attitude_target_update_quat.from_axis_angle(Vector3f(0.0f, 0.0f, _attitude_target_euler_angle.z)); _attitude_target_update_quat.from_axis_angle(Vector3f(0.0f, 0.0f, yaw_shift));
_attitude_target_quat = _attitude_target_update_quat*_attitude_target_quat; _attitude_target_quat = _attitude_target_update_quat*_attitude_target_quat;
} }