From 1635e1115c8b6a2cf200123d37c59ae629d9944d Mon Sep 17 00:00:00 2001
From: Arthur Benemann <arthur.benemann@gmail.com>
Date: Tue, 28 Apr 2015 13:37:21 -0700
Subject: [PATCH] AP_Gimbal: exponential control law for yaw angle

compensates for bad yaw joint angle
---
 libraries/AP_Mount/AP_Gimbal.cpp | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/libraries/AP_Mount/AP_Gimbal.cpp b/libraries/AP_Mount/AP_Gimbal.cpp
index b161b49ad0..e300b4ba1e 100644
--- a/libraries/AP_Mount/AP_Gimbal.cpp
+++ b/libraries/AP_Mount/AP_Gimbal.cpp
@@ -89,6 +89,11 @@ Vector3f AP_Gimbal::getGimbalRateDemVecYaw(const 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 = - _gimbalParams.K_gimbalRate * _measurement.joint_angles.z;
+        if (gimbalRateDemVecYaw.z > 0.0f){
+            gimbalRateDemVecYaw.z = +0.2f*gimbalRateDemVecYaw.z*gimbalRateDemVecYaw.z;
+        }else{
+            gimbalRateDemVecYaw.z = -0.2f*gimbalRateDemVecYaw.z*gimbalRateDemVecYaw.z;
+        }
 
         // Get filtered vehicle turn rate in earth frame
         vehicleYawRateFilt = (1.0f - yawRateFiltPole * _measurement.delta_time) * vehicleYawRateFilt + yawRateFiltPole * _measurement.delta_time * _ahrs.get_yaw_rate_earth();