Skip to content

Commit

Permalink
AP_WheelEncoder: correct initialisation of WheelEncoder object
Browse files Browse the repository at this point in the history
the existing constructer sets a slew rate limit to 0.2

This is essentially a missing patch from a previous series of PRs which moved dt from being a member variable to being passed into the update call for the PID
  • Loading branch information
peterbarker committed Sep 20, 2024
1 parent 0e72fc7 commit 5dd0b7a
Showing 1 changed file with 16 additions and 2 deletions.
18 changes: 16 additions & 2 deletions libraries/AP_WheelEncoder/AP_WheelRateControl.h
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand Down

0 comments on commit 5dd0b7a

Please sign in to comment.