1 // SPDX-License-Identifier: Apache-2.0
2 // SPDX-FileCopyrightText: Copyright 2017 Google Inc
3
4 #include "pidcontroller.hpp"
5
6 #include "ec/pid.hpp"
7
8 #include <cmath>
9
10 namespace pid_control
11 {
12
calPIDOutput(double setpt,double input,ec::pid_info_t * info)13 double PIDController::calPIDOutput(double setpt, double input,
14 ec::pid_info_t* info)
15 {
16 double output;
17 auto name = getID();
18
19 if (info->checkHysterWithSetpt)
20 {
21 // Over the hysteresis bounds, keep counting pid
22 if (input > (setpt + info->positiveHysteresis))
23 {
24 // Calculate new output
25 output = ec::pid(info, input, setpt, &name);
26
27 // this variable isn't actually used in this context, but we're
28 // setting it here in case somebody uses it later it's the correct
29 // value
30 lastInput = input;
31 }
32 // Under the hysteresis bounds, initialize pid
33 else if (input < (setpt - info->negativeHysteresis))
34 {
35 lastInput = setpt;
36 info->integral = 0;
37 output = 0;
38 }
39 // inside the hysteresis bounds, keep last output
40 else
41 {
42 lastInput = input;
43 output = info->lastOutput;
44 }
45
46 info->lastOutput = output;
47 }
48 else
49 {
50 // if no hysteresis, maintain previous behavior
51 if (info->positiveHysteresis == 0 && info->negativeHysteresis == 0)
52 {
53 // Calculate new output
54 output = ec::pid(info, input, setpt, &name);
55
56 // this variable isn't actually used in this context, but we're
57 // setting it here in case somebody uses it later it's the correct
58 // value
59 lastInput = input;
60 }
61 else
62 {
63 // initialize if the value is not set (NAN) or abnormal (+INF or
64 // -INF)
65 if (!(std::isfinite(lastInput)))
66 {
67 lastInput = input;
68 }
69
70 // if reading is outside of hysteresis bounds, use it for reading,
71 // otherwise use last reading without updating it first
72 else if ((input - lastInput) > info->positiveHysteresis)
73 {
74 lastInput = input;
75 }
76 else if ((lastInput - input) > info->negativeHysteresis)
77 {
78 lastInput = input;
79 }
80
81 output = ec::pid(info, lastInput, setpt, &name);
82 }
83 }
84
85 return output;
86 }
87
process(void)88 void PIDController::process(void)
89 {
90 double input;
91 double setpt;
92 double output;
93
94 // Get setpt value
95 setpt = setptProc();
96
97 // Get input value
98 input = inputProc();
99
100 auto info = getPIDInfo();
101
102 // Calculate output value
103 output = calPIDOutput(setpt, input, info);
104
105 info->lastOutput = output;
106
107 // Output new value
108 outputProc(output);
109
110 return;
111 }
112
113 } // namespace pid_control
114