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

AC_AttControlHeli: integrate div-by-zero check for bf-to-ef conversion

This commit is contained in:
Randy Mackay 2014-08-22 22:50:33 +09:00
parent 91817b0884
commit 06e06438b3
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_Heli.cpp
View File

@ -91,10 +91,11 @@ void AC_AttitudeControl_Heli::passthrough_bf_roll_pitch_rate_yaw(float roll_pass
// update our earth-frame angle targets // update our earth-frame angle targets
Vector3f angle_ef_error; Vector3f angle_ef_error;
frame_conversion_bf_to_ef(_angle_bf_error, angle_ef_error); if (frame_conversion_bf_to_ef(_angle_bf_error, angle_ef_error)) {
_angle_ef_target.x = wrap_180_cd_float(angle_ef_error.x + _ahrs.roll_sensor); _angle_ef_target.x = wrap_180_cd_float(angle_ef_error.x + _ahrs.roll_sensor);
_angle_ef_target.y = wrap_180_cd_float(angle_ef_error.y + _ahrs.pitch_sensor); _angle_ef_target.y = wrap_180_cd_float(angle_ef_error.y + _ahrs.pitch_sensor);
_angle_ef_target.z = wrap_360_cd_float(angle_ef_error.z + _ahrs.yaw_sensor); _angle_ef_target.z = wrap_360_cd_float(angle_ef_error.z + _ahrs.yaw_sensor);
}
// handle flipping over pitch axis // handle flipping over pitch axis
if (_angle_ef_target.y > 9000.0f) { if (_angle_ef_target.y > 9000.0f) {