Built motion from commit 6a09e18b.|2.6.11

[motion2.git] / legacy-libs / grpc / deps / grpc / src / core / lib / transport / pid_controller.cc

diff --git a/legacy-libs/grpc/deps/grpc/src/core/lib/transport/pid_controller.cc b/legacy-libs/grpc/deps/grpc/src/core/lib/transport/pid_controller.cc
new file mode 100644 (file)
index 0000000..dbc98f4
--- /dev/null
+++ b/legacy-libs/grpc/deps/grpc/src/core/lib/transport/pid_controller.cc
@@ -0,0 +1,51 @@
+/*
+ *
+ * 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 <grpc/support/port_platform.h>
+
+#include "src/core/lib/transport/pid_controller.h"
+
+#include "src/core/lib/gpr/useful.h"
+
+namespace grpc_core {
+
+PidController::PidController(const Args& args)
+    : last_control_value_(args.initial_control_value()), args_(args) {}
+
+double PidController::Update(double error, double dt) {
+  if (dt <= 0) return last_control_value_;
+  /* integrate error using the trapezoid rule */
+  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 = args_.gain_p() * error + args_.gain_i() * error_integral_ +
+                 args_.gain_d() * diff_error;
+  /* and perform trapezoidal integration */
+  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;
+}
+
+}  // namespace grpc_core