legacy-libs/grpc/deps/grpc/src/core/lib/transport/pid_controller.cc

   1 /*
   2  *
   3  * Copyright 2016 gRPC authors.
   4  *
   5  * Licensed under the Apache License, Version 2.0 (the "License");
   6  * you may not use this file except in compliance with the License.
   7  * You may obtain a copy of the License at
   8  *
   9  *     http://www.apache.org/licenses/LICENSE-2.0
  10  *
  11  * Unless required by applicable law or agreed to in writing, software
  12  * distributed under the License is distributed on an "AS IS" BASIS,
  13  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  14  * See the License for the specific language governing permissions and
  15  * limitations under the License.
  16  *
  17  */
  18
  19 #include <grpc/support/port_platform.h>
  20
  21 #include "src/core/lib/transport/pid_controller.h"
  22
  23 #include "src/core/lib/gpr/useful.h"
  24
  25 namespace grpc_core {
  26
  27 PidController::PidController(const Args& args)
  28     : last_control_value_(args.initial_control_value()), args_(args) {}
  29
  30 double PidController::Update(double error, double dt) {
  31   if (dt <= 0) return last_control_value_;
  32   /* integrate error using the trapezoid rule */
  33   error_integral_ += dt * (last_error_ + error) * 0.5;
  34   error_integral_ = GPR_CLAMP(error_integral_, -args_.integral_range(),
  35                               args_.integral_range());
  36   double diff_error = (error - last_error_) / dt;
  37   /* calculate derivative of control value vs time */
  38   double dc_dt = args_.gain_p() * error + args_.gain_i() * error_integral_ +
  39                  args_.gain_d() * diff_error;
  40   /* and perform trapezoidal integration */
  41   double new_control_value =
  42       last_control_value_ + dt * (last_dc_dt_ + dc_dt) * 0.5;
  43   new_control_value = GPR_CLAMP(new_control_value, args_.min_control_value(),
  44                                 args_.max_control_value());
  45   last_error_ = error;
  46   last_dc_dt_ = dc_dt;
  47   last_control_value_ = new_control_value;
  48   return new_control_value;
  49 }
  50
  51 }  // namespace grpc_core