diff options
author | Craig Tiller <ctiller@google.com> | 2017-01-03 11:31:34 -0800 |
---|---|---|
committer | Craig Tiller <ctiller@google.com> | 2017-01-03 11:31:34 -0800 |
commit | 68c9dbe69414bb19e0d810304c48dc417d82a4c8 (patch) | |
tree | f50f02f89b7d4e3e96a8dc1c1da1bef14e383bec /src/core/lib/transport | |
parent | 88d81bffd181d047eefd131acde8b018ee85671b (diff) |
Add clamping to pid controller, make arguments more readable
Diffstat (limited to 'src/core/lib/transport')
-rw-r--r-- | src/core/lib/transport/pid_controller.c | 23 | ||||
-rw-r--r-- | src/core/lib/transport/pid_controller.h | 11 |
2 files changed, 23 insertions, 11 deletions
diff --git a/src/core/lib/transport/pid_controller.c b/src/core/lib/transport/pid_controller.c index ba56874503..3a4845d7ef 100644 --- a/src/core/lib/transport/pid_controller.c +++ b/src/core/lib/transport/pid_controller.c @@ -32,14 +32,12 @@ */ #include "src/core/lib/transport/pid_controller.h" +#include <grpc/support/useful.h> void grpc_pid_controller_init(grpc_pid_controller *pid_controller, - double initial_control_value, double gain_p, - double gain_i, double gain_d) { - pid_controller->last_control_value = initial_control_value; - pid_controller->gain_p = gain_p; - pid_controller->gain_i = gain_i; - pid_controller->gain_d = gain_d; + grpc_pid_controller_args args) { + pid_controller->args = args; + pid_controller->last_control_value = args.initial_control_value; grpc_pid_controller_reset(pid_controller); } @@ -53,13 +51,20 @@ double grpc_pid_controller_update(grpc_pid_controller *pid_controller, /* integrate error using the trapezoid rule */ pid_controller->error_integral += dt * (pid_controller->last_error + error) * 0.5; + pid_controller->error_integral = GPR_CLAMP( + pid_controller->error_integral, -pid_controller->args.integral_range, + pid_controller->args.integral_range); double diff_error = (error - pid_controller->last_error) / dt; /* calculate derivative of control value vs time */ - double dc_dt = pid_controller->gain_p * error + - pid_controller->gain_i * pid_controller->error_integral + - pid_controller->gain_d * diff_error; + double dc_dt = pid_controller->args.gain_p * error + + pid_controller->args.gain_i * pid_controller->error_integral + + pid_controller->args.gain_d * diff_error; + /* and perform trapezoidal integration */ double new_control_value = pid_controller->last_control_value + dt * (pid_controller->last_dc_dt + dc_dt) * 0.5; + new_control_value = + GPR_CLAMP(new_control_value, pid_controller->args.min_control_value, + pid_controller->args.max_control_value); pid_controller->last_error = error; pid_controller->last_dc_dt = dc_dt; pid_controller->last_control_value = new_control_value; diff --git a/src/core/lib/transport/pid_controller.h b/src/core/lib/transport/pid_controller.h index dd2b120052..1bf2fbc564 100644 --- a/src/core/lib/transport/pid_controller.h +++ b/src/core/lib/transport/pid_controller.h @@ -45,16 +45,23 @@ typedef struct { double gain_p; double gain_i; double gain_d; + double initial_control_value; + double min_control_value; + double max_control_value; + double integral_range; +} grpc_pid_controller_args; + +typedef struct { double last_error; double error_integral; double last_control_value; double last_dc_dt; + grpc_pid_controller_args args; } grpc_pid_controller; /** Initialize the controller */ void grpc_pid_controller_init(grpc_pid_controller *pid_controller, - double initial_control_value, double gain_p, - double gain_i, double gain_d); + grpc_pid_controller_args args); /** Reset the controller: useful when things have changed significantly */ void grpc_pid_controller_reset(grpc_pid_controller *pid_controller); |