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

AP_Winch: support for upgrade to PID object

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

2
libraries/AP_Winch/AP_Winch.h
View File

@ -82,7 +82,7 @@ private:
AP_Int8 type; // winch type
AP_Float rate_max; // deploy or retract rate maximum (in m/s).
AP_Float pos_p; // position error P gain
AC_PID rate_pid = AC_PID(AP_WINCH_RATE_P, AP_WINCH_RATE_I, AP_WINCH_RATE_D, AP_WINCH_RATE_IMAX, AP_WINCH_RATE_FILT, AP_WINCH_RATE_DT); // rate control PID
AC_PID rate_pid = AC_PID(AP_WINCH_RATE_P, AP_WINCH_RATE_I, AP_WINCH_RATE_D, 0.0f, AP_WINCH_RATE_IMAX, 0.0f, AP_WINCH_RATE_FILT, 0.0f, AP_WINCH_RATE_DT); // rate control PID
winch_state state; // state of winch control (using target position or target rate)
float length_curr; // current length of the line (in meters) that has been deployed
float length_desired; // target desired length (in meters)

18
libraries/AP_Winch/AP_Winch_Servo.cpp
View File

@ -58,22 +58,6 @@ void AP_Winch_Servo::update()
rate_desired = config.rate_desired;
}
// calculate rate error and pass to pid controller
float rate_error = rate_desired - rate;
config.rate_pid.set_input_filter_all(rate_error);
// get p
float p = config.rate_pid.get_p();
// get i unless winch hit limit on last iteration
float i = config.rate_pid.get_integrator();
if (((is_negative(rate_error) && !limit_low) || (is_positive(rate_error) && !limit_high))) {
i = config.rate_pid.get_i();
}
// get d
float d = config.rate_pid.get_d();
// calculate base output
float base = 0.0f;
if (is_positive(config.rate_max)) {
@ -81,7 +65,7 @@ void AP_Winch_Servo::update()
}
// constrain and set limit flags
float output = base + p + i + d;
float output = base + config.rate_pid.update_all(rate_desired, rate, (limit_low || limit_high));
limit_low = (output <= -1.0f);
limit_high = (output >= 1.0f);
output = constrain_float(output, -1.0f, 1.0f);