AP_Gimbal: revert to original behavior

3drobotics · Nov 13, 2015 · 64c9735 · 64c9735
1 parent 2eecaf5
commit 64c9735
Show file tree

Hide file tree

Showing 2 changed files with 20 additions and 12 deletions.
diff --git a/libraries/AP_Mount/AP_Gimbal.cpp b/libraries/AP_Mount/AP_Gimbal.cpp
@@ -260,28 +260,35 @@ void AP_Gimbal::update_joint_angle_est()
 
 Vector3f AP_Gimbal::getGimbalRateDemVecYaw(const Quaternion &quatEst)
 {
-    // Get filtered vehicle turn rate in body frame
-    static const float tc = 0.05f;
-    static const float yawErrorLimit = radians(5);
+    static const float tc = 0.1f;
+    static const float yawErrorLimit = radians(5.7f);
     float dt = _measurement.delta_time;
     float alpha = dt/(dt+tc);
 
     Matrix3f Tve = _ahrs.get_dcm_matrix();
     Matrix3f Teg;
     quatEst.inverse().rotation_matrix(Teg);
 
-    // calculate the maximum steady state rate error corresponding to the maximum permitted yaw angle error
+
+    //_vehicle_yaw_rate_ef_filt = _ahrs.get_yaw_rate_earth();
+
+    // filter the vehicle yaw rate to remove noise
+    _vehicle_yaw_rate_ef_filt += (_ahrs.get_yaw_rate_earth() - _vehicle_yaw_rate_ef_filt) * alpha;
+
     float yaw_rate_ff = 0;
-    if (_ahrs.get_yaw_rate_earth() > _gimbalParams.get_K_rate()*yawErrorLimit) {
-        yaw_rate_ff = _ahrs.get_yaw_rate_earth()-_gimbalParams.get_K_rate()*yawErrorLimit;
-    } else if (_ahrs.get_yaw_rate_earth() < -_gimbalParams.get_K_rate()*yawErrorLimit) {
-        yaw_rate_ff = _ahrs.get_yaw_rate_earth()+_gimbalParams.get_K_rate()*yawErrorLimit;
+
+    // calculate an earth-frame yaw rate feed-forward that prevents gimbal from exceeding the maximum yaw error
+    if (_vehicle_yaw_rate_ef_filt > _gimbalParams.get_K_rate()*yawErrorLimit) {
+        yaw_rate_ff = _vehicle_yaw_rate_ef_filt-_gimbalParams.get_K_rate()*yawErrorLimit;
+    } else if (_vehicle_yaw_rate_ef_filt < -_gimbalParams.get_K_rate()*yawErrorLimit) {
+        yaw_rate_ff = _vehicle_yaw_rate_ef_filt+_gimbalParams.get_K_rate()*yawErrorLimit;
     }
 
-    yaw_rate_ff_filt += (yaw_rate_ff - yaw_rate_ff_filt) * alpha;
+    // filter the feed-forward to remove noise
+    //_yaw_rate_ff_ef_filt += (yaw_rate_ff - _yaw_rate_ff_ef_filt) * alpha;
 
     Vector3f gimbalRateDemVecYaw;
-    gimbalRateDemVecYaw.z = yaw_rate_ff_filt - _gimbalParams.get_K_rate() * filtered_joint_angles.z / constrain_float(Tve.c.z,0.5f,1.0f);
+    gimbalRateDemVecYaw.z = yaw_rate_ff - _gimbalParams.get_K_rate() * filtered_joint_angles.z / constrain_float(Tve.c.z,0.5f,1.0f);
     gimbalRateDemVecYaw.z /= constrain_float(Tve.c.z,0.5f,1.0f);
 
     // rotate the rate demand into gimbal frame

diff --git a/libraries/AP_Mount/AP_Gimbal.h b/libraries/AP_Mount/AP_Gimbal.h
@@ -46,7 +46,7 @@ class AP_Gimbal : AP_AccelCal_Client
         _ahrs(ahrs),
         _state(GIMBAL_STATE_NOT_PRESENT),
         _gimbalParams(parameters),
-        yaw_rate_ff_filt(0.0f),
+        _yaw_rate_ff_ef_filt(0.0f),
         lockedToBody(false),
         vehicle_delta_angles(),
         vehicle_to_gimbal_quat(),
@@ -86,7 +86,8 @@ class AP_Gimbal : AP_AccelCal_Client
 private:
     gimbal_state_t _state;
 
-    float yaw_rate_ff_filt;
+    float _yaw_rate_ff_ef_filt;
+    float _vehicle_yaw_rate_ef_filt;
 
     static const uint8_t _compid = MAV_COMP_ID_GIMBAL;