From ee8c92c850b824efa8a69f3cdef2009072ec986d Mon Sep 17 00:00:00 2001
From: Arthur Benemann <arthur.benemann@gmail.com>
Date: Fri, 20 Mar 2015 10:32:38 -0700
Subject: [PATCH] AP_Gimbal: clean-up for AP_Mount merge

---
 libraries/AP_Gimbal/AP_Gimbal.cpp | 34 ++------------------------
 libraries/AP_Gimbal/AP_Gimbal.h   | 40 ++++++++++---------------------
 2 files changed, 15 insertions(+), 59 deletions(-)

diff --git a/libraries/AP_Gimbal/AP_Gimbal.cpp b/libraries/AP_Gimbal/AP_Gimbal.cpp
index 4273ab34cd..c3ab050afc 100644
--- a/libraries/AP_Gimbal/AP_Gimbal.cpp
+++ b/libraries/AP_Gimbal/AP_Gimbal.cpp
@@ -2,24 +2,15 @@
 
 #include <stdio.h>
 #include <AP_Common.h>
-#include <AP_Progmem.h>
-#include <AP_Param.h>
 #include <AP_Gimbal.h>
 #include <GCS.h>
-#include <GCS_MAVLink.h>
 #include <AP_SmallEKF.h>
 
-const AP_Param::GroupInfo AP_Gimbal::var_info[] PROGMEM = {
-    AP_GROUPEND
-};
-
 uint16_t feedback_error_count;
 static float K_gimbalRate = 5.0f;
-static float angRateLimit = 0.5f;
 
 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() && !isCopterFliped()){
@@ -168,32 +159,11 @@ void AP_Gimbal::send_control(mavlink_channel_t chan)
 }
 
 
-void AP_Gimbal::update_failsafe(uint8_t rc_failsafe)
+void AP_Gimbal::update_failsafe(uint8_t failsafe)
 {
-    _rc_failsafe = rc_failsafe;
+    _failsafe = 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()
-{
-    // Get new tilt angle
-    float new_tilt = (_rc_failsafe)?0.0f:angle_input_rad(RC_Channel::rc_channel(tilt_rc_in-1), _tilt_angle_min, _tilt_angle_max);
-    // Low-pass filter
-    new_tilt = _angle_ef_target_rad.y + 0.09f*(new_tilt - _angle_ef_target_rad.y);
-    // Slew-rate constrain
-    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);
-    // 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
index 8f68cc136c..add921fd3c 100644
--- a/libraries/AP_Gimbal/AP_Gimbal.h
+++ b/libraries/AP_Gimbal/AP_Gimbal.h
@@ -28,29 +28,21 @@ class AP_Gimbal
 public:
     //Constructor
     AP_Gimbal(const AP_AHRS_NavEKF &ahrs, uint8_t sysid, uint8_t compid) :
-        _ahrs(ahrs),
         _ekf(ahrs),
+        _ahrs(ahrs),
         _joint_offsets(0.0f,0.0f,0.0f),
         vehicleYawRateFilt(0.0f),
         yawRateFiltPole(10.0f),
         yawErrorLimit(0.1f),
-        tilt_rc_in(6),
-        _tilt_angle_min(-45.0f),
-        _tilt_angle_max(0.0f),
-        _max_tilt_rate(0.5f),
-        _rc_failsafe(true)
+        _failsafe(true)
     {
-        AP_Param::setup_object_defaults(this, var_info);
         _sysid = sysid;
         _compid = compid;
     }
         
-    void    update_failsafe(uint8_t rc_failsafe);
+    void    update_failsafe(uint8_t failsafe);
     void    receive_feedback(mavlink_channel_t chan, mavlink_message_t *msg);
 
-    // hook for eeprom variables
-    static const struct AP_Param::GroupInfo        var_info[];
-
     struct Measurament {
         float delta_time;
         Vector3f delta_angles;
@@ -59,10 +51,15 @@ public:
     } _measurament;
 
     SmallEKF    _ekf;                   // state of small EKF for gimbal
+    const AP_AHRS_NavEKF    &_ahrs;     //  Main EKF    
     Vector3f    gimbalRateDemVec;       // degrees/s   
     Vector3f    _angle_ef_target_rad;   // desired earth-frame roll, tilt and pan angles in radians
 
 private:  
+
+    // These are corrections (in radians) applied to the to the gimbal joint (x,y,z = roll,pitch,yaw) measurements
+    Vector3f const _joint_offsets;
+
     // filtered yaw rate from the vehicle
     float vehicleYawRateFilt;
 
@@ -74,28 +71,17 @@ private:
 
     // amount of yaw angle that we permit the gimbal to lag the vehicle when operating in slave mode
     // reducing this makes the gimbal respond more to vehicle yaw disturbances
-    float const yawErrorLimit;
+    float const yawErrorLimit;   
+   
+    // status of the RC signal
+    uint8_t _failsafe;   
 
-    // These are corrections (in radians) applied to the to the gimbal joint (x,y,z = roll,pitch,yaw) measurements
-    Vector3f const _joint_offsets;
-
-
-    uint8_t const tilt_rc_in; 
-    float const _tilt_angle_min;   // min tilt in 0.01 degree units
-    float const _tilt_angle_max;     // max tilt in 0.01 degree units
-    float const _max_tilt_rate;          // max tilt rate in rad/s
-    
-    const AP_AHRS_NavEKF    &_ahrs;     //  Main EKF    
     uint8_t _sysid;                     
-    uint8_t _compid;
-
-
-    uint8_t _rc_failsafe;   // status of the RC signal
+    uint8_t _compid;    
 
     void send_control(mavlink_channel_t chan);
     void update_state();
     void decode_feedback(mavlink_message_t *msg);
-    void update_targets_from_rc();
 
     uint8_t isCopterFliped();