Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_WheelEncoder: Support changing update period

This commit is contained in:
Leonard Hall 2022-11-30 15:54:33 +10:30 committed by Andrew Tridgell
parent 951c818927
commit 3d1959e159
1 changed files with 2 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
libraries/AP_WheelEncoder/AP_WheelRateControl.cpp
View File

@ -189,9 +189,6 @@ float AP_WheelRateControl::get_rate_controlled_throttle(uint8_t instance, float
// determine which PID instance to use // determine which PID instance to use
AC_PID& rate_pid = (instance == 0) ? _rate_pid0 : _rate_pid1; AC_PID& rate_pid = (instance == 0) ? _rate_pid0 : _rate_pid1;
// set PID's dt
rate_pid.set_dt(dt);
// check for timeout // check for timeout
uint32_t now = AP_HAL::millis(); uint32_t now = AP_HAL::millis();
if (now - _last_update_ms > AP_WHEEL_RATE_CONTROL_TIMEOUT_MS) { if (now - _last_update_ms > AP_WHEEL_RATE_CONTROL_TIMEOUT_MS) {
@ -205,11 +202,11 @@ float AP_WheelRateControl::get_rate_controlled_throttle(uint8_t instance, float
// convert desired rate as a percentage to radians/sec // convert desired rate as a percentage to radians/sec
float desired_rate = desired_rate_pct * 0.01f * get_rate_max_rads(); float desired_rate = desired_rate_pct * 0.01f * get_rate_max_rads();
// get actual rate from wheeel encoder // get actual rate from wheel encoder
float actual_rate = _wheel_encoder.get_rate(instance); float actual_rate = _wheel_encoder.get_rate(instance);
// constrain and set limit flags // constrain and set limit flags
float output = rate_pid.update_all(desired_rate, actual_rate, (_limit[instance].lower || _limit[instance].upper)); float output = rate_pid.update_all(desired_rate, actual_rate, dt, (_limit[instance].lower || _limit[instance].upper));
output += rate_pid.get_ff(); output += rate_pid.get_ff();
// set limits for next iteration // set limits for next iteration