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

AP_WheelEncoder: support for upgrade to PID object

This commit is contained in:
Leonard Hall 2019-06-27 19:05:32 +09:30 committed by Randy Mackay
parent c7196a4232
commit 3d831e4c04
2 changed files with 4 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
libraries/AP_WheelEncoder/AP_WheelRateControl.cpp
View File

@ -144,31 +144,9 @@ float AP_WheelRateControl::get_rate_controlled_throttle(uint8_t instance, float
// get actual rate from wheeel encoder
float actual_rate = _wheel_encoder.get_rate(instance);
// calculate rate error and pass to pid controller
float rate_error = desired_rate - actual_rate;
rate_pid.set_input_filter_all(rate_error);
// store desired and actual for logging purposes
rate_pid.set_desired_rate(desired_rate);
rate_pid.set_actual_rate(actual_rate);
// get ff
float ff = rate_pid.get_ff(desired_rate);
// get p
float p = rate_pid.get_p();
// get i unless we hit limit on last iteration
float i = rate_pid.get_integrator();
if (((is_negative(rate_error) && !_limit[instance].lower) || (is_positive(rate_error) && !_limit[instance].upper))) {
i = rate_pid.get_i();
}
// get d
float d = rate_pid.get_d();
// constrain and set limit flags
float output = ff + p + i + d;
float output = rate_pid.update_all(desired_rate, actual_rate, (_limit[instance].lower || _limit[instance].upper));
output += rate_pid.get_ff();
// set limits for next iteration
_limit[instance].upper = output >= 100.0f;

4
libraries/AP_WheelEncoder/AP_WheelRateControl.h
View File

@ -58,8 +58,8 @@ private:
// parameters
AP_Int8 _enabled; // top level enable/disable control
AP_Float _rate_max; // wheel maximum rotation rate in rad/s
AC_PID _rate_pid0 = AC_PID(AP_WHEEL_RATE_CONTROL_P, AP_WHEEL_RATE_CONTROL_I, AP_WHEEL_RATE_CONTROL_D, AP_WHEEL_RATE_CONTROL_IMAX, AP_WHEEL_RATE_CONTROL_FILT, AP_WHEEL_RATE_CONTROL_DT, AP_WHEEL_RATE_CONTROL_FF);
AC_PID _rate_pid1 = AC_PID(AP_WHEEL_RATE_CONTROL_P, AP_WHEEL_RATE_CONTROL_I, AP_WHEEL_RATE_CONTROL_D, AP_WHEEL_RATE_CONTROL_IMAX, AP_WHEEL_RATE_CONTROL_FILT, AP_WHEEL_RATE_CONTROL_DT, AP_WHEEL_RATE_CONTROL_FF);
AC_PID _rate_pid0 = AC_PID(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 = AC_PID(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);
// limit flags
struct AP_MotorsUGV_limit {