Copter: integrate PID input filter

ArduCopter/Parameters.h

@@ -509,17 +509,17 @@ public:
         rc_14               (CH_14),
 #endif
 
-        // PID controller	    initial P	            initial I		        initial D               initial imax
-        //----------------------------------------------------------------------------------------------------------
-        pid_rate_roll           (RATE_ROLL_P,           RATE_ROLL_I,            RATE_ROLL_D,            RATE_ROLL_IMAX),
-        pid_rate_pitch          (RATE_PITCH_P,          RATE_PITCH_I,           RATE_PITCH_D,           RATE_PITCH_IMAX),
-        pid_rate_yaw            (RATE_YAW_P,            RATE_YAW_I,             RATE_YAW_D,             RATE_YAW_IMAX),
+        // PID controller	    initial P	      initial I         initial D       initial imax        initial filt hz     pid rate
+        //---------------------------------------------------------------------------------------------------------------------------------
+        pid_rate_roll           (RATE_ROLL_P,     RATE_ROLL_I,      RATE_ROLL_D,    RATE_ROLL_IMAX,     RATE_ROLL_FILT_HZ,  MAIN_LOOP_SECONDS),
+        pid_rate_pitch          (RATE_PITCH_P,    RATE_PITCH_I,     RATE_PITCH_D,   RATE_PITCH_IMAX,    RATE_PITCH_FILT_HZ, MAIN_LOOP_SECONDS),
+        pid_rate_yaw            (RATE_YAW_P,      RATE_YAW_I,       RATE_YAW_D,     RATE_YAW_IMAX,      RATE_YAW_FILT_HZ,   MAIN_LOOP_SECONDS),
 
-        pid_loiter_rate_lat     (LOITER_RATE_P,         LOITER_RATE_I,          LOITER_RATE_D,          LOITER_RATE_IMAX),
-        pid_loiter_rate_lon     (LOITER_RATE_P,         LOITER_RATE_I,          LOITER_RATE_D,          LOITER_RATE_IMAX),
+        pid_loiter_rate_lat     (LOITER_RATE_P,   LOITER_RATE_I,    LOITER_RATE_D,  LOITER_RATE_IMAX,   LOITER_RATE_FILT_HZ,WPNAV_LOITER_UPDATE_TIME),
+        pid_loiter_rate_lon     (LOITER_RATE_P,   LOITER_RATE_I,    LOITER_RATE_D,  LOITER_RATE_IMAX,   LOITER_RATE_FILT_HZ,WPNAV_LOITER_UPDATE_TIME),
 
         p_throttle_rate         (THROTTLE_RATE_P),
-        pid_throttle_accel      (THROTTLE_ACCEL_P,      THROTTLE_ACCEL_I,       THROTTLE_ACCEL_D,       THROTTLE_ACCEL_IMAX),
+        pid_throttle_accel      (THROTTLE_ACCEL_P,THROTTLE_ACCEL_I, THROTTLE_ACCEL_D,THROTTLE_ACCEL_IMAX, THROTTLE_ACCEL_FILT_HZ, MAIN_LOOP_SECONDS),
 
         // P controller	        initial P
         //----------------------------------------------------------------------

ArduCopter/config.h

@@ -596,6 +596,9 @@
 #ifndef RATE_ROLL_IMAX
  # define RATE_ROLL_IMAX         	1000
 #endif
+#ifndef RATE_ROLL_FILT_HZ
+ # define RATE_ROLL_FILT_HZ         20.0f
+#endif
 
 #ifndef RATE_PITCH_P
  # define RATE_PITCH_P       		0.150f
@@ -609,6 +612,10 @@
 #ifndef RATE_PITCH_IMAX
  # define RATE_PITCH_IMAX        	1000
 #endif
+#ifndef RATE_PITCH_FILT_HZ
+ # define RATE_PITCH_FILT_HZ        20.0f
+#endif
+
 
 #ifndef RATE_YAW_P
  # define RATE_YAW_P              	0.200f
@@ -622,6 +629,9 @@
 #ifndef RATE_YAW_IMAX
  # define RATE_YAW_IMAX            	1000
 #endif
+#ifndef RATE_YAW_FILT_HZ
+ # define RATE_YAW_FILT_HZ          AC_PID_FILT_HZ_DEFAULT
+#endif
 
 //////////////////////////////////////////////////////////////////////////////
 // Loiter position control gains
@@ -645,6 +655,9 @@
 #ifndef LOITER_RATE_IMAX
  # define LOITER_RATE_IMAX          1000        // maximum acceleration from I term build-up in cm/s/s
 #endif
+#ifndef LOITER_RATE_FILT_HZ
+ # define LOITER_RATE_FILT_HZ       5.0f
+#endif
 
 //////////////////////////////////////////////////////////////////////////////
 // PosHold parameter defaults
@@ -703,6 +716,9 @@
 #ifndef THROTTLE_ACCEL_IMAX
  # define THROTTLE_ACCEL_IMAX   800
 #endif
+#ifndef THROTTLE_ACCEL_FILT_HZ
+ # define THROTTLE_ACCEL_FILT_HZ    20.0f
+#endif
 
 // default maximum vertical velocity and acceleration the pilot may request
 #ifndef PILOT_VELZ_MAX