diff options
| -rw-r--r-- | src/core/lib/transport/pid_controller.c | 57 | ||||
| -rw-r--r-- | src/core/lib/transport/pid_controller.h | 53 |
2 files changed, 110 insertions, 0 deletions
diff --git a/src/core/lib/transport/pid_controller.c b/src/core/lib/transport/pid_controller.c new file mode 100644 index 0000000000..a661116916 --- /dev/null +++ b/src/core/lib/transport/pid_controller.c @@ -0,0 +1,57 @@ +/* + * + * Copyright 2016, Google Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following disclaimer + * in the documentation and/or other materials provided with the + * distribution. + * * Neither the name of Google Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include "src/core/lib/transport/pid_controller.h" + +void grpc_pid_controller_init(grpc_pid_controller *pid_controller, + double gain_p, double gain_i, double gain_d) { + pid_controller->gain_p = gain_p; + pid_controller->gain_i = gain_i; + pid_controller->gain_d = gain_d; + grpc_pid_controller_reset(pid_controller); +} + +void grpc_pid_controller_reset(grpc_pid_controller *pid_controller) { + pid_controller->last_error = 0.0; + pid_controller->error_integral = 0.0; +} + +double grpc_pid_controller_update(grpc_pid_controller *pid_controller, + double error, double dt) { + pid_controller->error_integral += error * dt; + double diff_error = (error - pid_controller->last_error) / dt; + pid_controller->last_error = error; + return pid_controller->gain_p * error + + pid_controller->gain_i * pid_controller->error_integral + + pid_controller->gain_d * diff_error; +} diff --git a/src/core/lib/transport/pid_controller.h b/src/core/lib/transport/pid_controller.h new file mode 100644 index 0000000000..aa3d0a77f7 --- /dev/null +++ b/src/core/lib/transport/pid_controller.h @@ -0,0 +1,53 @@ +/* + * + * Copyright 2016, Google Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are + * met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following disclaimer + * in the documentation and/or other materials provided with the + * distribution. + * * Neither the name of Google Inc. nor the names of its + * contributors may be used to endorse or promote products derived from + * this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR + * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT + * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT + * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, + * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY + * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT + * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef GRPC_CORE_LIB_TRANSPORT_PID_CONTROLLER_H +#define GRPC_CORE_LIB_TRANSPORT_PID_CONTROLLER_H + +typedef struct { + double gain_p; + double gain_i; + double gain_d; + double last_error; + double error_integral; +} grpc_pid_controller; + +void grpc_pid_controller_init(grpc_pid_controller *pid_controller, + double gain_p, double gain_i, double gain_d); + +void grpc_pid_controller_reset(grpc_pid_controller *pid_controller); + +double grpc_pid_controller_update(grpc_pid_controller *pid_controller, + double error, double dt); + +#endif |