From 2b0fb45fe801375bc940af7b3d1679b11b4cfc13 Mon Sep 17 00:00:00 2001 From: Leonard Hall Date: Wed, 4 Mar 2015 12:47:05 +0900 Subject: [PATCH] Copter: integrate PID input filter --- ArduCopter/Parameters.h | 16 ++++++++-------- ArduCopter/config.h | 16 ++++++++++++++++ 2 files changed, 24 insertions(+), 8 deletions(-) diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index ec9b7fc90d..53ae962fed 100644 --- a/ArduCopter/Parameters.h +++ b/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 //---------------------------------------------------------------------- diff --git a/ArduCopter/config.h b/ArduCopter/config.h index e20b6b6fd4..0f7cbaef2f 100644 --- a/ArduCopter/config.h +++ b/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