1 // SPDX-License-Identifier: Apache-2.0
2 // SPDX-FileCopyrightText: Copyright 2017 Google Inc
3
4 #include "pid.hpp"
5
6 #include "logging.hpp"
7
8 #include <chrono>
9 #include <string>
10
11 namespace pid_control
12 {
13 namespace ec
14 {
15
16 /********************************
17 * clamp
18 *
19 */
clamp(double x,double min,double max)20 static double clamp(double x, double min, double max)
21 {
22 if (x < min)
23 {
24 return min;
25 }
26 if (x > max)
27 {
28 return max;
29 }
30 return x;
31 }
32
33 /********************************
34 * pid code
35 * Note: Codes assumes the ts field is non-zero
36 */
pid(pid_info_t * pidinfoptr,double input,double setpoint,const std::string * nameptr)37 double pid(pid_info_t* pidinfoptr, double input, double setpoint,
38 const std::string* nameptr)
39 {
40 if (nameptr)
41 {
42 if (!(pidinfoptr->initialized))
43 {
44 LogInit(*nameptr, pidinfoptr);
45 }
46 }
47
48 auto logPtr = nameptr ? LogPeek(*nameptr) : nullptr;
49
50 PidCoreContext coreContext;
51 std::chrono::milliseconds msNow;
52
53 if (logPtr)
54 {
55 msNow = LogTimestamp();
56 }
57
58 coreContext.input = input;
59 coreContext.setpoint = setpoint;
60
61 double error;
62
63 double proportionalTerm;
64 double integralTerm = 0.0f;
65 double derivativeTerm = 0.0f;
66 double feedFwdTerm = 0.0f;
67
68 double output;
69
70 // calculate P, I, D, FF
71
72 // Pid
73 error = setpoint - input;
74 proportionalTerm = pidinfoptr->proportionalCoeff * error;
75
76 coreContext.error = error;
77 coreContext.proportionalTerm = proportionalTerm;
78 coreContext.integralTerm1 = 0.0;
79
80 // pId
81 if (0.0f != pidinfoptr->integralCoeff)
82 {
83 integralTerm = pidinfoptr->integral;
84 integralTerm += error * pidinfoptr->integralCoeff * pidinfoptr->ts;
85
86 coreContext.integralTerm1 = integralTerm;
87
88 integralTerm = clamp(integralTerm, pidinfoptr->integralLimit.min,
89 pidinfoptr->integralLimit.max);
90 }
91
92 coreContext.integralTerm2 = integralTerm;
93
94 // piD
95 derivativeTerm = pidinfoptr->derivativeCoeff *
96 ((error - pidinfoptr->lastError) / pidinfoptr->ts);
97
98 coreContext.derivativeTerm = derivativeTerm;
99
100 // FF
101 feedFwdTerm = (setpoint + pidinfoptr->feedFwdOffset) *
102 pidinfoptr->feedFwdGain;
103
104 coreContext.feedFwdTerm = feedFwdTerm;
105
106 output = proportionalTerm + integralTerm + derivativeTerm + feedFwdTerm;
107
108 coreContext.output1 = output;
109
110 output = clamp(output, pidinfoptr->outLim.min, pidinfoptr->outLim.max);
111
112 coreContext.output2 = output;
113
114 coreContext.minOut = 0.0;
115 coreContext.maxOut = 0.0;
116
117 // slew rate
118 // TODO(aarena) - Simplify logic as Andy suggested by creating dynamic
119 // outLim_min/max that are affected by slew rate control and just clamping
120 // to those instead of effectively clamping twice.
121 if (pidinfoptr->initialized)
122 {
123 if (pidinfoptr->slewNeg != 0.0f)
124 {
125 // Don't decrease too fast
126 double minOut = pidinfoptr->lastOutput +
127 pidinfoptr->slewNeg * pidinfoptr->ts;
128
129 coreContext.minOut = minOut;
130
131 if (output < minOut)
132 {
133 output = minOut;
134 }
135 }
136 if (pidinfoptr->slewPos != 0.0f)
137 {
138 // Don't increase too fast
139 double maxOut = pidinfoptr->lastOutput +
140 pidinfoptr->slewPos * pidinfoptr->ts;
141
142 coreContext.maxOut = maxOut;
143
144 if (output > maxOut)
145 {
146 output = maxOut;
147 }
148 }
149
150 if (pidinfoptr->slewNeg != 0.0f || pidinfoptr->slewPos != 0.0f)
151 {
152 // Back calculate integral term for the cases where we limited the
153 // output
154 integralTerm = output - proportionalTerm;
155 }
156 }
157
158 coreContext.output3 = output;
159 coreContext.integralTerm3 = integralTerm;
160
161 // Clamp again because having limited the output may result in a
162 // larger integral term
163 integralTerm = clamp(integralTerm, pidinfoptr->integralLimit.min,
164 pidinfoptr->integralLimit.max);
165 pidinfoptr->integral = integralTerm;
166 pidinfoptr->initialized = true;
167 pidinfoptr->lastError = error;
168 pidinfoptr->lastOutput = output;
169
170 coreContext.integralTerm = pidinfoptr->integral;
171 coreContext.output = pidinfoptr->lastOutput;
172
173 if (logPtr)
174 {
175 LogContext(*logPtr, msNow, coreContext);
176 }
177
178 return output;
179 }
180
181 } // namespace ec
182 } // namespace pid_control
183