diff options
Diffstat (limited to 'src/core/lib/transport/pid_controller.cc')
-rw-r--r-- | src/core/lib/transport/pid_controller.cc | 63 |
1 files changed, 63 insertions, 0 deletions
diff --git a/src/core/lib/transport/pid_controller.cc b/src/core/lib/transport/pid_controller.cc new file mode 100644 index 0000000000..4b304f17b2 --- /dev/null +++ b/src/core/lib/transport/pid_controller.cc @@ -0,0 +1,63 @@ +/* + * + * Copyright 2016 gRPC authors. + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ + +#include "src/core/lib/transport/pid_controller.h" +#include <grpc/support/useful.h> + +void grpc_pid_controller_init(grpc_pid_controller *pid_controller, + grpc_pid_controller_args args) { + pid_controller->args = args; + pid_controller->last_control_value = args.initial_control_value; + grpc_pid_controller_reset(pid_controller); +} + +void grpc_pid_controller_reset(grpc_pid_controller *pid_controller) { + pid_controller->last_error = 0.0; + pid_controller->last_dc_dt = 0.0; + pid_controller->error_integral = 0.0; +} + +double grpc_pid_controller_update(grpc_pid_controller *pid_controller, + double error, double dt) { + if (dt == 0) return pid_controller->last_control_value; + /* 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->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; + return new_control_value; +} + +double grpc_pid_controller_last(grpc_pid_controller *pid_controller) { + return pid_controller->last_control_value; +} |