Merge pull request #12903 from ctiller/pid++

C++ize PidController

2 files changed, 96 insertions, 67 deletions

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 <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);
-}
+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 80899e9a20..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 <limits>
 
 /* \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<double>::min();
+    double max_control_value_ = std::numeric_limits<double>::max();
+    double integral_range_ = std::numeric_limits<double>::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);
+
+  /// 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 */