diff --git a/libraries/AP_WheelEncoder/AP_WheelRateControl.h b/libraries/AP_WheelEncoder/AP_WheelRateControl.h index f62d981bfb6863..960322a6a49219 100644 --- a/libraries/AP_WheelEncoder/AP_WheelRateControl.h +++ b/libraries/AP_WheelEncoder/AP_WheelRateControl.h @@ -61,8 +61,22 @@ class AP_WheelRateControl { // parameters AP_Int8 _enabled; // top level enable/disable control AP_Float _rate_max; // wheel maximum rotation rate in rad/s - AC_PID _rate_pid0{AP_WHEEL_RATE_CONTROL_P, AP_WHEEL_RATE_CONTROL_I, AP_WHEEL_RATE_CONTROL_D, AP_WHEEL_RATE_CONTROL_FF, AP_WHEEL_RATE_CONTROL_IMAX, AP_WHEEL_RATE_CONTROL_FILT, AP_WHEEL_RATE_CONTROL_FILT, AP_WHEEL_RATE_CONTROL_FILT, AP_WHEEL_RATE_CONTROL_DT}; - AC_PID _rate_pid1{AP_WHEEL_RATE_CONTROL_P, AP_WHEEL_RATE_CONTROL_I, AP_WHEEL_RATE_CONTROL_D, AP_WHEEL_RATE_CONTROL_FF, AP_WHEEL_RATE_CONTROL_IMAX, AP_WHEEL_RATE_CONTROL_FILT, AP_WHEEL_RATE_CONTROL_FILT, AP_WHEEL_RATE_CONTROL_FILT, AP_WHEEL_RATE_CONTROL_DT}; + + const AC_PID::Defaults rate_pid_defaults { + .p = AP_WHEEL_RATE_CONTROL_P, + .i = AP_WHEEL_RATE_CONTROL_I, + .d = AP_WHEEL_RATE_CONTROL_D, + .ff = AP_WHEEL_RATE_CONTROL_FF, + .imax = AP_WHEEL_RATE_CONTROL_IMAX, + .filt_T_hz = AP_WHEEL_RATE_CONTROL_FILT, + .filt_E_hz = AP_WHEEL_RATE_CONTROL_FILT, + .filt_D_hz = AP_WHEEL_RATE_CONTROL_FILT, + .srmax = 0, + .srtau = 1.0 + }; + + AC_PID _rate_pid0{rate_pid_defaults}; + AC_PID _rate_pid1{rate_pid_defaults}; // limit flags struct AP_MotorsUGV_limit {