From c45338f0806643e013932558be3ef2091703d03a Mon Sep 17 00:00:00 2001
From: Randy Mackay <rmackay9@yahoo.com>
Date: Fri, 22 Aug 2014 22:50:10 +0900
Subject: [PATCH] AC_AttControl: div-by-zero check for bf-to-ef conversion

---
 .../AC_AttitudeControl/AC_AttitudeControl.cpp  | 18 ++++++++++++------
 .../AC_AttitudeControl/AC_AttitudeControl.h    |  7 ++++---
 2 files changed, 16 insertions(+), 9 deletions(-)

diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
index d4c795f80..c6cf9e533 100644
--- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
+++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
@@ -382,10 +382,11 @@ void AC_AttitudeControl::rate_bf_roll_pitch_yaw(float roll_rate_bf, float pitch_
     } else {
         _acro_angle_switch = 4500;
         integrate_bf_rate_error_to_angle_errors();
-        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);
+        }
         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);
@@ -432,12 +433,17 @@ void AC_AttitudeControl::frame_conversion_ef_to_bf(const Vector3f& ef_vector, Ve
 }
 
 // frame_conversion_bf_to_ef - converts body frame vector to earth frame vector
-void AC_AttitudeControl::frame_conversion_bf_to_ef(const Vector3f& bf_vector, Vector3f& ef_vector)
+bool AC_AttitudeControl::frame_conversion_bf_to_ef(const Vector3f& bf_vector, Vector3f& ef_vector)
 {
-    // convert earth frame rates to body frame rates
+    // avoid divide by zero
+    if (_ahrs.cos_pitch() == 0.0f) {
+        return false;
+    }
+    // convert earth frame angle or rates to body frame
     ef_vector.x = bf_vector.x + _ahrs.sin_roll() * (_ahrs.sin_pitch()/_ahrs.cos_pitch()) * bf_vector.y + _ahrs.cos_roll() * (_ahrs.sin_pitch()/_ahrs.cos_pitch()) * bf_vector.z;
     ef_vector.y = _ahrs.cos_roll()  * bf_vector.y - _ahrs.sin_roll() * bf_vector.z;
     ef_vector.z = (_ahrs.sin_roll() / _ahrs.cos_pitch()) * bf_vector.y + (_ahrs.cos_roll() / _ahrs.cos_pitch()) * bf_vector.z;
+    return true;
 }
 
 //
diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h
index 9b22963f5..2f0c5a11d 100644
--- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h
+++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h
@@ -116,11 +116,12 @@ public:
     //
     // earth-frame <-> body-frame conversion functions
     //
-    // frame_conversion_ef_to_bf - converts earth frame rate targets to body frame rate targets
+    // frame_conversion_ef_to_bf - converts earth frame angles or rates to body frame
     void frame_conversion_ef_to_bf(const Vector3f& ef_vector, Vector3f &bf_vector);
 
-    // frame_conversion_bf_to_ef - converts body frame rate targets to earth frame rate targets
-    void frame_conversion_bf_to_ef(const Vector3f& bf_vector, Vector3f &ef_vector);
+    // frame_conversion_bf_to_ef - converts body frame angles or rates to earth frame
+    //  returns false if conversion fails due to gimbal lock
+    bool frame_conversion_bf_to_ef(const Vector3f& bf_vector, Vector3f &ef_vector);
 
     //
     // public accessor functions
-- 
GitLab