From 247b23114d4782d9e6b2d3f4410f76fb85b31fb4 Mon Sep 17 00:00:00 2001 From: Craig Tiller Date: Mon, 9 Oct 2017 09:16:19 -0700 Subject: C++ize PidController --- .../ext/transport/chttp2/transport/flow_control.cc | 23 ++--- src/core/ext/transport/chttp2/transport/internal.h | 2 +- src/core/lib/transport/pid_controller.cc | 53 ++++------ src/core/lib/transport/pid_controller.h | 112 ++++++++++++++------- 4 files changed, 108 insertions(+), 82 deletions(-) (limited to 'src/core') diff --git a/src/core/ext/transport/chttp2/transport/flow_control.cc b/src/core/ext/transport/chttp2/transport/flow_control.cc index d0e80c4bd5..716cd71490 100644 --- a/src/core/ext/transport/chttp2/transport/flow_control.cc +++ b/src/core/ext/transport/chttp2/transport/flow_control.cc @@ -399,22 +399,19 @@ static double get_pid_controller_guess(grpc_exec_ctx* exec_ctx, if (!tfc->pid_controller_initialized) { tfc->last_pid_update = now; tfc->pid_controller_initialized = true; - grpc_pid_controller_args args; - memset(&args, 0, sizeof(args)); - args.gain_p = 4; - args.gain_i = 8; - args.gain_d = 0; - args.initial_control_value = target; - args.min_control_value = -1; - args.max_control_value = 25; - args.integral_range = 10; - grpc_pid_controller_init(&tfc->pid_controller, args); + tfc->pid_controller.Init(grpc_core::PidController::Args() + .set_gain_p(4) + .set_gain_i(8) + .set_gain_d(0) + .set_initial_control_value(target) + .set_min_control_value(-1) + .set_max_control_value(25) + .set_integral_range(10)); return pow(2, target); } - double bdp_error = target - grpc_pid_controller_last(&tfc->pid_controller); + double bdp_error = target - tfc->pid_controller->last_control_value(); double dt = (double)(now - tfc->last_pid_update) * 1e-3; - double log2_bdp_guess = - grpc_pid_controller_update(&tfc->pid_controller, bdp_error, dt); + double log2_bdp_guess = tfc->pid_controller->Update(bdp_error, dt); tfc->last_pid_update = now; return pow(2, log2_bdp_guess); } diff --git a/src/core/ext/transport/chttp2/transport/internal.h b/src/core/ext/transport/chttp2/transport/internal.h index b4eb033a47..f0a75dfb45 100644 --- a/src/core/ext/transport/chttp2/transport/internal.h +++ b/src/core/ext/transport/chttp2/transport/internal.h @@ -273,7 +273,7 @@ typedef struct { /* pid controller */ bool pid_controller_initialized; - grpc_pid_controller pid_controller; + grpc_core::ManualConstructor pid_controller; grpc_millis last_pid_update; // pointer back to transport for tracing diff --git a/src/core/lib/transport/pid_controller.cc b/src/core/lib/transport/pid_controller.cc index 4b304f17b2..9f7750d693 100644 --- a/src/core/lib/transport/pid_controller.cc +++ b/src/core/lib/transport/pid_controller.cc @@ -19,45 +19,30 @@ #include "src/core/lib/transport/pid_controller.h" #include -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); -} +namespace grpc_core { -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; -} +PidController::PidController(const Args &args) + : last_control_value_(args.initial_control_value()), args_(args) {} -double grpc_pid_controller_update(grpc_pid_controller *pid_controller, - double error, double dt) { - if (dt == 0) return pid_controller->last_control_value; +double PidController::Update(double error, double dt) { + if (dt <= 0) return 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; + error_integral_ += dt * (last_error_ + error) * 0.5; + error_integral_ = GPR_CLAMP(error_integral_, -args_.integral_range(), + args_.integral_range()); + double diff_error = (error - 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; + double dc_dt = args_.gain_p() * error + args_.gain_i() * error_integral_ + + 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; + double new_control_value = + last_control_value_ + dt * (last_dc_dt_ + dc_dt) * 0.5; + new_control_value = GPR_CLAMP(new_control_value, args_.min_control_value(), + args_.max_control_value()); + last_error_ = error; + last_dc_dt_ = dc_dt; + 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; -} +} // namespace grpc_core diff --git a/src/core/lib/transport/pid_controller.h b/src/core/lib/transport/pid_controller.h index 17feabfd39..87e59a1a90 100644 --- a/src/core/lib/transport/pid_controller.h +++ b/src/core/lib/transport/pid_controller.h @@ -19,9 +19,7 @@ #ifndef GRPC_CORE_LIB_TRANSPORT_PID_CONTROLLER_H #define GRPC_CORE_LIB_TRANSPORT_PID_CONTROLLER_H -#ifdef __cplusplus -extern "C" { -#endif +#include /* \file Simple PID controller. Implements a proportional-integral-derivative controller. @@ -30,41 +28,87 @@ extern "C" { Gains can be set to adjust sensitivity to current error (p), the integral of error (i), and the derivative of error (d). */ -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; +namespace grpc_core { -typedef struct { - double last_error; - double error_integral; - double last_control_value; - double last_dc_dt; - grpc_pid_controller_args args; -} grpc_pid_controller; +class PidController { + public: + class Args { + public: + double gain_p() const { return gain_p_; } + double gain_i() const { return gain_i_; } + double gain_d() const { return gain_d_; } + double initial_control_value() const { return initial_control_value_; } + double min_control_value() const { return min_control_value_; } + double max_control_value() const { return max_control_value_; } + double integral_range() const { return integral_range_; } -/** Initialize the controller */ -void grpc_pid_controller_init(grpc_pid_controller *pid_controller, - grpc_pid_controller_args args); + Args& set_gain_p(double gain_p) { + gain_p_ = gain_p; + return *this; + } + Args& set_gain_i(double gain_i) { + gain_i_ = gain_i; + return *this; + } + Args& set_gain_d(double gain_d) { + gain_d_ = gain_d; + return *this; + } + Args& set_initial_control_value(double initial_control_value) { + initial_control_value_ = initial_control_value; + return *this; + } + Args& set_min_control_value(double min_control_value) { + min_control_value_ = min_control_value; + return *this; + } + Args& set_max_control_value(double max_control_value) { + max_control_value_ = max_control_value; + return *this; + } + Args& set_integral_range(double integral_range) { + integral_range_ = integral_range; + return *this; + } -/** Reset the controller: useful when things have changed significantly */ -void grpc_pid_controller_reset(grpc_pid_controller *pid_controller); + private: + double gain_p_ = 0.0; + double gain_i_ = 0.0; + double gain_d_ = 0.0; + double initial_control_value_ = 0.0; + double min_control_value_ = std::numeric_limits::min(); + double max_control_value_ = std::numeric_limits::max(); + double integral_range_ = std::numeric_limits::max(); + }; -/** Update the controller: given a current error estimate, and the time since - the last update, returns a new control value */ -double grpc_pid_controller_update(grpc_pid_controller *pid_controller, - double error, double dt); + explicit PidController(const Args& args); -/** Returns the last control value calculated */ -double grpc_pid_controller_last(grpc_pid_controller *pid_controller); + /// Reset the controller internal state: useful when the environment has + /// changed significantly + void Reset() { + last_error_ = 0.0; + last_dc_dt_ = 0.0; + error_integral_ = 0.0; + } -#ifdef __cplusplus -} -#endif + /// Update the controller: given a current error estimate, and the time since + /// the last update, returns a new control value + double Update(double error, double dt); -#endif /* GRPC_CORE_LIB_TRANSPORT_PID_CONTROLLER_H */ \ No newline at end of file + /// Returns the last control value calculated + double last_control_value() const { return last_control_value_; } + + /// Returns the current error integral (mostly for testing) + double error_integral() const { return error_integral_; } + + private: + double last_error_ = 0.0; + double error_integral_ = 0.0; + double last_control_value_; + double last_dc_dt_ = 0.0; + const Args args_; +}; + +} // namespace grpc_core + +#endif /* GRPC_CORE_LIB_TRANSPORT_PID_CONTROLLER_H */ -- cgit v1.2.3