From 06e06438b33c8aa50301994eee23b57d63376b37 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 22 Aug 2014 22:50:33 +0900 Subject: [PATCH] AC_AttControlHeli: integrate div-by-zero check for bf-to-ef conversion --- libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp index 1b2f817557..9ae1b3ef8f 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Heli.cpp @@ -91,10 +91,11 @@ void AC_AttitudeControl_Heli::passthrough_bf_roll_pitch_rate_yaw(float roll_pass // update our earth-frame angle targets Vector3f angle_ef_error; - 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.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); + 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.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); + } // handle flipping over pitch axis if (_angle_ef_target.y > 9000.0f) {