AP_Gimbal: disable gimbal motors if copter is fliped

3drobotics · Mar 10, 2015 · 9e791de · 9e791de
1 parent 02c3981
commit 9e791de
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 1 deletion.
diff --git 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;
+}
diff --git a/libraries/AP_Gimbal/AP_Gimbal.h b/libraries/AP_Gimbal/AP_Gimbal.h
@@ -97,6 +97,8 @@ class AP_Gimbal
     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);