diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index a366b057bb..d67fb13d9e 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -529,9 +529,15 @@ public: // PID controller initial P initial I initial D initial imax initial filt hz pid rate //--------------------------------------------------------------------------------------------------------------------------------- +#if FRAME_CONFIG == HELI_FRAME + pid_rate_roll (RATE_ROLL_P, RATE_ROLL_I, RATE_ROLL_D, RATE_ROLL_IMAX, RATE_ROLL_FILT_HZ, MAIN_LOOP_SECONDS, RATE_ROLL_FF), + pid_rate_pitch (RATE_PITCH_P, RATE_PITCH_I, RATE_PITCH_D, RATE_PITCH_IMAX, RATE_PITCH_FILT_HZ, MAIN_LOOP_SECONDS, RATE_PITCH_FF), + pid_rate_yaw (RATE_YAW_P, RATE_YAW_I, RATE_YAW_D, RATE_YAW_IMAX, RATE_YAW_FILT_HZ, MAIN_LOOP_SECONDS, RATE_YAW_FF), +#else 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), +#endif pi_vel_xy (VEL_XY_P, VEL_XY_I, VEL_XY_IMAX, VEL_XY_FILT_HZ, WPNAV_LOITER_UPDATE_TIME), diff --git a/ArduCopter/config.h b/ArduCopter/config.h index 0048ff38ce..3e36f0ea16 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -104,9 +104,24 @@ #if FRAME_CONFIG == HELI_FRAME # define RC_FAST_SPEED 125 # define WP_YAW_BEHAVIOR_DEFAULT WP_YAW_BEHAVIOR_LOOK_AHEAD - # define RATE_ROLL_D 0 - # define RATE_PITCH_D 0 - # define MPU6K_FILTER 10 + # define RATE_ROLL_P 0.02 + # define RATE_ROLL_I 0.5 + # define RATE_ROLL_D 0.001 + # define RATE_ROLL_IMAX 4500 + # define RATE_ROLL_FF 0.05 + # define RATE_ROLL_FILT_HZ 20.0f + # define RATE_PITCH_P 0.02 + # define RATE_PITCH_I 0.5 + # define RATE_PITCH_D 0.001 + # define RATE_PITCH_IMAX 4500 + # define RATE_PITCH_FF 0.05 + # define RATE_PITCH_FILT_HZ 20.0f + # define RATE_YAW_P 0.15 + # define RATE_YAW_I 0.100 + # define RATE_YAW_D 0.003 + # define RATE_YAW_IMAX 4500 + # define RATE_YAW_FF 0.02 + # define RATE_YAW_FILT_HZ 20.0f # define HELI_STAB_COLLECTIVE_MIN_DEFAULT 0 # define HELI_STAB_COLLECTIVE_MAX_DEFAULT 1000 # define THR_MIN_DEFAULT 0