AP_Gimbal: tighten yaw

3drobotics · Oct 30, 2015 · 3bf61db · 3bf61db
1 parent 29e4062
commit 3bf61db
Show file tree

Hide file tree

Showing 2 changed files with 29 additions and 47 deletions.
diff --git a/libraries/AP_Mount/AP_Gimbal.cpp b/libraries/AP_Mount/AP_Gimbal.cpp
@@ -260,39 +260,34 @@ void AP_Gimbal::update_joint_angle_est()
 
 Vector3f AP_Gimbal::getGimbalRateDemVecYaw(const Quaternion &quatEst)
 {
-        // define the rotation from vehicle to earth
-        Matrix3f Tve = _ahrs.get_dcm_matrix();
-        Matrix3f Teg;
-        quatEst.inverse().rotation_matrix(Teg);
-
-        // multiply the yaw joint angle by a gain to calculate a demanded vehicle frame relative rate vector required to keep the yaw joint centered
-        Vector3f gimbalRateDemVecYaw;
-        gimbalRateDemVecYaw.z = - _gimbalParams.get_K_rate() * filtered_joint_angles.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();
-        Vector3f vehicle_rate_ef(0,0,vehicleYawRateFilt);
-
-         // calculate the maximum steady state rate error corresponding to the maximum permitted yaw angle error
-        float maxRate = _gimbalParams.get_K_rate() * yawErrorLimit;
-        float vehicle_rate_mag_ef = vehicle_rate_ef.length();
-        float excess_rate_correction = fabsf(vehicle_rate_mag_ef) - maxRate;
-        if (vehicle_rate_mag_ef > maxRate) {
-            if (vehicle_rate_ef.z>0.0f){
-                gimbalRateDemVecYaw += _ahrs.get_dcm_matrix().transposed()*Vector3f(0,0,excess_rate_correction);
-            } else {
-                gimbalRateDemVecYaw -= _ahrs.get_dcm_matrix().transposed()*Vector3f(0,0,excess_rate_correction);
-            }
-        }
+    // Get filtered vehicle turn rate in body frame
+    static const float tc = 0.03f;
+    static const float yawErrorLimit = radians(3);
+    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
+    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;
+    }
+
+    yaw_rate_ff_filt += (yaw_rate_ff - yaw_rate_ff_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 /= constrain_float(Tve.c.z,0.5f,1.0f);
+
+    // rotate the rate demand into gimbal frame
+    gimbalRateDemVecYaw = Teg * gimbalRateDemVecYaw;
 
-        // rotate the rate demand into earth frame
-        gimbalRateDemVecYaw = Tve * gimbalRateDemVecYaw;
-        // zero the tilt components
-        gimbalRateDemVecYaw.x = 0;
-        gimbalRateDemVecYaw.y = 0;
-        // rotate the rate demand into gimbal frame
-        gimbalRateDemVecYaw = Teg * gimbalRateDemVecYaw;
-        return gimbalRateDemVecYaw;
+    return gimbalRateDemVecYaw;
 }
 
 Vector3f AP_Gimbal::getGimbalRateDemVecTilt(const Quaternion &quatEst)

diff --git a/libraries/AP_Mount/AP_Gimbal.h b/libraries/AP_Mount/AP_Gimbal.h
@@ -46,10 +46,8 @@ class AP_Gimbal : AP_AccelCal_Client
         _ahrs(ahrs),
         _state(GIMBAL_STATE_NOT_PRESENT),
         _gimbalParams(parameters),
-        vehicleYawRateFilt(0.0f),
-        yawRateFiltPole(10.0f),
+        yaw_rate_ff_filt(0.0f),
         lockedToBody(false),
-        yawErrorLimit(0.1f),
         vehicle_delta_angles(),
         vehicle_to_gimbal_quat(),
         vehicle_to_gimbal_quat_filt(),
@@ -88,18 +86,7 @@ class AP_Gimbal : AP_AccelCal_Client
 private:
     gimbal_state_t _state;
 
-    // filtered yaw rate from the vehicle
-    float vehicleYawRateFilt;
-
-    // circular frequency (rad/sec) constant of filter applied to forward path vehicle yaw rate
-    // this frequency must not be larger than the update rate (Hz).
-    // reducing it makes the gimbal yaw less responsive to vehicle yaw
-    // increasing it makes the gimbal yawe more responsive to vehicle yaw
-    float const yawRateFiltPole;
-
-    // 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 yaw_rate_ff_filt;
 
     static const uint8_t _compid = MAV_COMP_ID_GIMBAL;