AP_Gimbal: Re-implement the target low-pass filter

libraries/AP_Gimbal/AP_Gimbal.cpp

@@ -6,7 +6,6 @@
 #include <GCS.h>
 #include <AP_SmallEKF.h>
 
-uint16_t feedback_error_count;
 static float K_gimbalRate = 5.0f;
 
 void AP_Gimbal::receive_feedback(mavlink_channel_t chan, mavlink_message_t *msg)
@@ -164,6 +163,15 @@ void AP_Gimbal::update_failsafe(uint8_t failsafe)
     _failsafe = failsafe;
 }
 
+void AP_Gimbal::update_target(Vector3f newTarget)
+{
+    // Low-pass filter
+    _angle_ef_target_rad.y = _angle_ef_target_rad.y + 0.02f*(newTarget.y - _angle_ef_target_rad.y);
+    // Update tilt
+    _angle_ef_target_rad.y = constrain_float(_angle_ef_target_rad.y,radians(-45),radians(0));
+
+}
+
 
 uint8_t AP_Gimbal::isCopterFliped(){
     return fabs(_ahrs.roll)>1.0f || fabs(_ahrs.pitch)>1.0f;

libraries/AP_Gimbal/AP_Gimbal.h

@@ -30,6 +30,7 @@ public:
     AP_Gimbal(const AP_AHRS_NavEKF &ahrs, uint8_t sysid, uint8_t compid) :
         _ekf(ahrs),
         _ahrs(ahrs),
+        K_gimbalRate(5.0f),
         _joint_offsets(0.0f,0.0f,0.0f),
         vehicleYawRateFilt(0.0f),
         yawRateFiltPole(10.0f),
@@ -39,7 +40,8 @@ public:
         _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);
 
@@ -53,9 +55,11 @@ public:
     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
+    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;

libraries/AP_Mount/AP_Mount_MAVLink.cpp

@@ -95,7 +95,7 @@ void AP_Mount_MAVLink::status_msg(mavlink_channel_t chan)
  */
 void AP_Mount_MAVLink::handle_gimbal_report(mavlink_channel_t chan, mavlink_message_t *msg)
 {
-    _gimbal._angle_ef_target_rad = _angle_ef_target_rad;
+    _gimbal.update_target(_angle_ef_target_rad);
     _gimbal.receive_feedback(chan,msg);
 }