blob: e31cc85f76296b47cf40b6283b3cb448f42043d3 (
plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
|
/*
*
* 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 "src/core/lib/transport/pid_controller.h"
#include <grpc/support/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
|
