From 4bdae02cfd7d32d8ef7fbde18bdebdde86f4a0c5 Mon Sep 17 00:00:00 2001
From: Arthur Benemann <arthur.benemann@gmail.com>
Date: Tue, 10 Mar 2015 15:48:15 -0700
Subject: [PATCH] AP_Gimbal: disable gimbal motors if copter is fliped

---
 libraries/AP_Gimbal/AP_Gimbal.cpp | 6 +++++-
 libraries/AP_Gimbal/AP_Gimbal.h   | 2 ++
 2 files changed, 7 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_Gimbal/AP_Gimbal.cpp b/libraries/AP_Gimbal/AP_Gimbal.cpp
index 0a06938f11..39f0ff4c97 100644
--- a/libraries/AP_Gimbal/AP_Gimbal.cpp
+++ b/libraries/AP_Gimbal/AP_Gimbal.cpp
@@ -22,7 +22,7 @@ void AP_Gimbal::receive_feedback(mavlink_channel_t chan, mavlink_message_t *msg)
     update_targets_from_rc();
     decode_feedback(msg);
     update_state();
-    if (_ekf.getStatus()){
+    if (_ekf.getStatus() && !isCopterFliped()){
         send_control(chan);
     }
 
@@ -177,3 +177,7 @@ void AP_Gimbal::update_targets_from_rc()
     // Update tilt
     _angle_ef_target_rad.y = constrain_float(_angle_ef_target_rad.y + tilt_change,_tilt_angle_min,_tilt_angle_max);
 }
+
+uint8_t AP_Gimbal::isCopterFliped(){
+    return fabs(_ahrs.roll)>1.0f || fabs(_ahrs.pitch)>1.0f;
+}
\ No newline at end of file
diff --git a/libraries/AP_Gimbal/AP_Gimbal.h b/libraries/AP_Gimbal/AP_Gimbal.h
index df2497337b..8f68cc136c 100644
--- a/libraries/AP_Gimbal/AP_Gimbal.h
+++ b/libraries/AP_Gimbal/AP_Gimbal.h
@@ -97,6 +97,8 @@ private:
     void decode_feedback(mavlink_message_t *msg);
     void update_targets_from_rc();
 
+    uint8_t isCopterFliped();
+
     // Control loop functions
     Vector3f getGimbalRateDemVecYaw(Quaternion quatEst);
     Vector3f getGimbalRateDemVecTilt(Quaternion quatEst);