From 534790634b307e0e4589484a46070d976cf54331 Mon Sep 17 00:00:00 2001
From: Arthur Benemann <arthur.benemann@gmail.com>
Date: Tue, 10 Mar 2015 14:48:42 -0700
Subject: [PATCH] AP_Gimbal: add a radio failsafe state to the gimbal

---
 libraries/AP_Gimbal/AP_Gimbal.cpp | 9 +++++++--
 libraries/AP_Gimbal/AP_Gimbal.h   | 7 +++++--
 2 files changed, 12 insertions(+), 4 deletions(-)

diff --git a/libraries/AP_Gimbal/AP_Gimbal.cpp b/libraries/AP_Gimbal/AP_Gimbal.cpp
index 7f20678b44..fac8c98e00 100644
--- a/libraries/AP_Gimbal/AP_Gimbal.cpp
+++ b/libraries/AP_Gimbal/AP_Gimbal.cpp
@@ -150,19 +150,24 @@ void AP_Gimbal::send_control(mavlink_channel_t chan)
         gimbalRateDemVec.x, gimbalRateDemVec.y, gimbalRateDemVec.z);
 }
 
+
+void AP_Gimbal::update_failsafe(uint8_t rc_failsafe)
+{
+    _rc_failsafe = rc_failsafe;
+}
+
 // returns the angle (radians) that the RC_Channel input is receiving
 float angle_input_rad(RC_Channel* rc, float angle_min, float angle_max)
 {
     float input =rc->norm_input();
     float angle = input*(angle_max - angle_min) + angle_min;
-    
     return radians(angle);
 }
 
 // update_targets_from_rc - updates angle targets using input from receiver
 void AP_Gimbal::update_targets_from_rc()
 {
-    float new_tilt = angle_input_rad(RC_Channel::rc_channel(tilt_rc_in-1), _tilt_angle_min, _tilt_angle_max);
+    float new_tilt = (_rc_failsafe)?0.0f:angle_input_rad(RC_Channel::rc_channel(tilt_rc_in-1), _tilt_angle_min, _tilt_angle_max);
     float max_change_rads =_max_tilt_rate * _measurament.delta_time;
     float tilt_change = constrain_float(new_tilt - _angle_ef_target_rad.y,-max_change_rads,+max_change_rads);
     _angle_ef_target_rad.y += tilt_change;
diff --git a/libraries/AP_Gimbal/AP_Gimbal.h b/libraries/AP_Gimbal/AP_Gimbal.h
index d508fde1dc..df2497337b 100644
--- a/libraries/AP_Gimbal/AP_Gimbal.h
+++ b/libraries/AP_Gimbal/AP_Gimbal.h
@@ -37,14 +37,15 @@ public:
         tilt_rc_in(6),
         _tilt_angle_min(-45.0f),
         _tilt_angle_max(0.0f),
-        _max_tilt_rate(0.5f)
+        _max_tilt_rate(0.5f),
+        _rc_failsafe(true)
     {
         AP_Param::setup_object_defaults(this, var_info);
         _sysid = sysid;
         _compid = compid;
     }
         
-    // MAVLink methods
+    void    update_failsafe(uint8_t rc_failsafe);
     void    receive_feedback(mavlink_channel_t chan, mavlink_message_t *msg);
 
     // hook for eeprom variables
@@ -89,6 +90,8 @@ private:
     uint8_t _compid;
 
 
+    uint8_t _rc_failsafe;   // status of the RC signal
+
     void send_control(mavlink_channel_t chan);
     void update_state();
     void decode_feedback(mavlink_message_t *msg);