diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index 7fb570010c..f742cc082b 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -59,6 +59,16 @@ const AP_Param::GroupInfo AC_AttitudeControl::var_info[] PROGMEM = { // high level controllers // +void AC_AttitudeControl::set_dt(float delta_sec) +{ + _dt = delta_sec; + + // set attitude controller's D term filters + _pid_rate_roll.set_d_lpf_alpha(AC_ATTITUDE_RATE_RP_PID_DTERM_FILTER, _dt); + _pid_rate_pitch.set_d_lpf_alpha(AC_ATTITUDE_RATE_RP_PID_DTERM_FILTER, _dt); + _pid_rate_yaw.set_d_lpf_alpha(AC_ATTITUDE_RATE_Y_PID_DTERM_FILTER, _dt); +} + // init_targets - resets target angles to current angles void AC_AttitudeControl::init_targets() { diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index aa1942bad6..4e70a1a289 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -37,6 +37,9 @@ #define AC_ATTITUDE_100HZ_DT 0.0100f // delta time in seconds for 100hz update rate #define AC_ATTITUDE_400HZ_DT 0.0025f // delta time in seconds for 400hz update rate +#define AC_ATTITUDE_RATE_RP_PID_DTERM_FILTER 20 // D-term filter rate cutoff frequency for Roll and Pitch rate controllers +#define AC_ATTITUDE_RATE_Y_PID_DTERM_FILTER 5 // D-term filter rate cutoff frequency for Yaw rate controller + class AC_AttitudeControl { public: AC_AttitudeControl( AP_AHRS &ahrs, @@ -70,7 +73,7 @@ public: // // set_dt - sets time delta in seconds for all controllers (i.e. 100hz = 0.01, 400hz = 0.0025) - void set_dt(float delta_sec) { _dt = delta_sec; } + void set_dt(float delta_sec); // init_targets - resets target angles to current angles void init_targets();