aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AC_AttControl: bug fix for ef target during acro

Browse Source
This commit is contained in:
Randy Mackay 2014-08-23 21:42:23 +09:00
parent 3e0b573dfe
commit 5a66ff1ef9
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -387,12 +387,12 @@ void AC_AttitudeControl::rate_bf_roll_pitch_yaw(float roll_rate_bf, float pitch_
}
if (_angle_ef_target.y > 9000.0f) {
_angle_ef_target.x = wrap_180_cd_float(_angle_ef_target.x + 18000.0f);
_angle_ef_target.y = wrap_180_cd_float(18000.0f - _angle_ef_target.x);
_angle_ef_target.y = wrap_180_cd_float(18000.0f - _angle_ef_target.y);
_angle_ef_target.z = wrap_360_cd_float(_angle_ef_target.z + 18000.0f);
}
if (_angle_ef_target.y < -9000.0f) {
_angle_ef_target.x = wrap_180_cd_float(_angle_ef_target.x + 18000.0f);
_angle_ef_target.y = wrap_180_cd_float(-18000.0f - _angle_ef_target.x);
_angle_ef_target.y = wrap_180_cd_float(-18000.0f - _angle_ef_target.y);
_angle_ef_target.z = wrap_360_cd_float(_angle_ef_target.z + 18000.0f);
}
}