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