From cf2377d1137e2fae4f1924139b3fa22ef992a5ad Mon Sep 17 00:00:00 2001
From: Arthur Benemann <arthur.benemann@gmail.com>
Date: Mon, 30 Mar 2015 15:46:19 -0700
Subject: [PATCH] AP_Gimbal: Move gimbal parameters to a structure

---
 libraries/AP_Gimbal/AP_Gimbal.cpp | 20 +++++++-------------
 libraries/AP_Gimbal/AP_Gimbal.h   | 17 ++++-------------
 2 files changed, 11 insertions(+), 26 deletions(-)

diff --git a/libraries/AP_Gimbal/AP_Gimbal.cpp b/libraries/AP_Gimbal/AP_Gimbal.cpp
index aef165215a..83877ff0a2 100644
--- a/libraries/AP_Gimbal/AP_Gimbal.cpp
+++ b/libraries/AP_Gimbal/AP_Gimbal.cpp
@@ -6,13 +6,11 @@
 #include <GCS.h>
 #include <AP_SmallEKF.h>
 
-static float K_gimbalRate = 5.0f;
-
 void AP_Gimbal::receive_feedback(mavlink_channel_t chan, mavlink_message_t *msg)
 {
     decode_feedback(msg);
     update_state();
-    if (_ekf.getStatus() && !isCopterFliped()){
+    if (_ekf.getStatus() && !isCopterFliped() && _gimbalParams.K_gimbalRate!=0.0f){
         send_control(chan);
     }
 
@@ -44,7 +42,9 @@ void AP_Gimbal::decode_feedback(mavlink_message_t *msg)
     _measurament.joint_angles.z = report_msg.joint_az;
 
     //apply joint angle compensation
-    _measurament.joint_angles -= _joint_offsets;
+    _measurament.joint_angles -= _gimbalParams.joint_angles_offsets;
+    _measurament.delta_velocity -= _gimbalParams.delta_velocity_offsets;
+    _measurament.delta_angles -= _gimbalParams.delta_angles_offsets;
 }
 
 void AP_Gimbal::update_state()
@@ -74,14 +74,14 @@ Vector3f AP_Gimbal::getGimbalRateDemVecYaw(Quaternion quatEst)
 
         // multiply the yaw joint angle by a gain to calculate a demanded vehicle frame relative rate vector required to keep the yaw joint centred
         Vector3f gimbalRateDemVecYaw;
-        gimbalRateDemVecYaw.z = - K_gimbalRate * _measurament.joint_angles.z;
+        gimbalRateDemVecYaw.z = - _gimbalParams.K_gimbalRate * _measurament.joint_angles.z;
 
         // Get filtered vehicle turn rate in earth frame
         vehicleYawRateFilt = (1.0f - yawRateFiltPole * _measurament.delta_time) * vehicleYawRateFilt + yawRateFiltPole * _measurament.delta_time * _ahrs.get_yaw_rate_earth();
         Vector3f vehicle_rate_ef(0,0,vehicleYawRateFilt);
 
          // calculate the maximum steady state rate error corresponding to the maximum permitted yaw angle error
-        float maxRate = K_gimbalRate * yawErrorLimit;
+        float maxRate = _gimbalParams.K_gimbalRate * yawErrorLimit;
         float vehicle_rate_mag_ef = vehicle_rate_ef.length();
         float excess_rate_correction = fabs(vehicle_rate_mag_ef) - maxRate; 
         if (vehicle_rate_mag_ef > maxRate) {
@@ -126,7 +126,7 @@ Vector3f AP_Gimbal::getGimbalRateDemVecTilt(Quaternion quatEst)
         deltaAngErr.z = scaler * quatErr[3];
 
         // multiply the angle error vector by a gain to calculate a demanded gimbal rate required to control tilt
-        Vector3f gimbalRateDemVecTilt = deltaAngErr * K_gimbalRate;
+        Vector3f gimbalRateDemVecTilt = deltaAngErr * _gimbalParams.K_gimbalRate;
         return gimbalRateDemVecTilt;
 }
 
@@ -157,12 +157,6 @@ void AP_Gimbal::send_control(mavlink_channel_t chan)
         gimbalRateDemVec.x, gimbalRateDemVec.y, gimbalRateDemVec.z);
 }
 
-
-void AP_Gimbal::update_failsafe(uint8_t failsafe)
-{
-    _failsafe = failsafe;
-}
-
 void AP_Gimbal::update_target(Vector3f newTarget)
 {
     // Low-pass filter
diff --git a/libraries/AP_Gimbal/AP_Gimbal.h b/libraries/AP_Gimbal/AP_Gimbal.h
index 91672f4a62..2329747469 100644
--- a/libraries/AP_Gimbal/AP_Gimbal.h
+++ b/libraries/AP_Gimbal/AP_Gimbal.h
@@ -21,28 +21,26 @@
 #include <GCS_MAVLink.h>
 #include <AP_SmallEKF.h>
 #include <AP_NavEKF.h>
+#include <AP_Mount.h>
 
 
 class AP_Gimbal
 {
 public:
     //Constructor
-    AP_Gimbal(const AP_AHRS_NavEKF &ahrs, uint8_t sysid, uint8_t compid) :
+    AP_Gimbal(const AP_AHRS_NavEKF &ahrs, uint8_t sysid, uint8_t compid, const AP_Mount::gimbal_params &gimbalParams) :
         _ekf(ahrs),
         _ahrs(ahrs),
-        K_gimbalRate(5.0f),
-        _joint_offsets(0.0f,0.0f,0.0f),
         vehicleYawRateFilt(0.0f),
         yawRateFiltPole(10.0f),
         yawErrorLimit(0.1f),
-        _failsafe(true)
+        _gimbalParams(gimbalParams)
     {
         _sysid = sysid;
         _compid = compid;
     }
 
     void    update_target(Vector3f newTarget);
-    void    update_failsafe(uint8_t failsafe);
     void    receive_feedback(mavlink_channel_t chan, mavlink_message_t *msg);
 
     struct Measurament {
@@ -58,11 +56,7 @@ public:
     Vector3f    _angle_ef_target_rad;   // desired earth-frame roll, tilt and pan angles in radians    
 
 private:  
-    // K gain for the pointing loop
-    float const K_gimbalRate;
-
-    // These are corrections (in radians) applied to the to the gimbal joint (x,y,z = roll,pitch,yaw) measurements
-    Vector3f const _joint_offsets;
+    const AP_Mount::gimbal_params &_gimbalParams;
 
     // filtered yaw rate from the vehicle
     float vehicleYawRateFilt;
@@ -76,9 +70,6 @@ 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;   
-   
-    // status of the RC signal
-    uint8_t _failsafe;   
 
     uint8_t _sysid;                     
     uint8_t _compid;