blob: 6222f1afaeeadd25f3005042e623b293120362c7 [file] [log] [blame]
Patrick Ventured8012182018-03-08 08:21:38 -08001/**
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
Josh Lehande745422020-11-07 02:14:09 -080019#include "../tuning.hpp"
20#include "logging.hpp"
21
Patrick Venturea0764872020-08-08 07:48:43 -070022namespace pid_control
23{
Patrick Ventured8012182018-03-08 08:21:38 -080024namespace ec
25{
26
27/********************************
28 * clamp
29 *
30 */
Patrick Venture5f59c0f2018-11-11 12:55:14 -080031static double clamp(double x, double min, double max)
Patrick Ventured8012182018-03-08 08:21:38 -080032{
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 */
Josh Lehande745422020-11-07 02:14:09 -080048double pid(pid_info_t* pidinfoptr, double input, double setpoint,
49 const std::string* nameptr)
Patrick Ventured8012182018-03-08 08:21:38 -080050{
Josh Lehande745422020-11-07 02:14:09 -080051 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
Patrick Venture5f59c0f2018-11-11 12:55:14 -080072 double error;
Patrick Ventured8012182018-03-08 08:21:38 -080073
Patrick Venturea23468e2019-02-11 09:25:56 -080074 double proportionalTerm;
75 double integralTerm = 0.0f;
Bonnie Lo0e8fc392022-10-05 10:20:55 +080076 double derivativeTerm = 0.0f;
Patrick Venturea23468e2019-02-11 09:25:56 -080077 double feedFwdTerm = 0.0f;
Patrick Ventured8012182018-03-08 08:21:38 -080078
Patrick Venture5f59c0f2018-11-11 12:55:14 -080079 double output;
Patrick Ventured8012182018-03-08 08:21:38 -080080
81 // calculate P, I, D, FF
82
83 // Pid
Patrick Ventureda4a5dd2018-08-31 09:42:48 -070084 error = setpoint - input;
Patrick Venturea23468e2019-02-11 09:25:56 -080085 proportionalTerm = pidinfoptr->proportionalCoeff * error;
Patrick Ventured8012182018-03-08 08:21:38 -080086
Josh Lehande745422020-11-07 02:14:09 -080087 coreContext.error = error;
88 coreContext.proportionalTerm = proportionalTerm;
89 coreContext.integralTerm1 = 0.0;
90
Patrick Ventured8012182018-03-08 08:21:38 -080091 // pId
Patrick Venture4b0df322019-02-11 09:04:57 -080092 if (0.0f != pidinfoptr->integralCoeff)
Patrick Ventured8012182018-03-08 08:21:38 -080093 {
Patrick Venturea23468e2019-02-11 09:25:56 -080094 integralTerm = pidinfoptr->integral;
95 integralTerm += error * pidinfoptr->integralCoeff * pidinfoptr->ts;
Josh Lehande745422020-11-07 02:14:09 -080096
97 coreContext.integralTerm1 = integralTerm;
98
Patrick Venturea23468e2019-02-11 09:25:56 -080099 integralTerm = clamp(integralTerm, pidinfoptr->integralLimit.min,
100 pidinfoptr->integralLimit.max);
Patrick Ventured8012182018-03-08 08:21:38 -0800101 }
102
Josh Lehande745422020-11-07 02:14:09 -0800103 coreContext.integralTerm2 = integralTerm;
104
Bonnie Lo0e8fc392022-10-05 10:20:55 +0800105 // piD
106 derivativeTerm = pidinfoptr->derivativeCoeff *
107 ((error - pidinfoptr->lastError) / pidinfoptr->ts);
108
Josh Lehande745422020-11-07 02:14:09 -0800109 coreContext.derivativeTerm = derivativeTerm;
110
Patrick Ventured8012182018-03-08 08:21:38 -0800111 // FF
Patrick Venturea23468e2019-02-11 09:25:56 -0800112 feedFwdTerm =
113 (setpoint + pidinfoptr->feedFwdOffset) * pidinfoptr->feedFwdGain;
Patrick Ventured8012182018-03-08 08:21:38 -0800114
Josh Lehande745422020-11-07 02:14:09 -0800115 coreContext.feedFwdTerm = feedFwdTerm;
116
Bonnie Lo0e8fc392022-10-05 10:20:55 +0800117 output = proportionalTerm + integralTerm + derivativeTerm + feedFwdTerm;
Josh Lehande745422020-11-07 02:14:09 -0800118
119 coreContext.output1 = output;
120
Patrick Venture4b0df322019-02-11 09:04:57 -0800121 output = clamp(output, pidinfoptr->outLim.min, pidinfoptr->outLim.max);
Patrick Ventured8012182018-03-08 08:21:38 -0800122
Josh Lehande745422020-11-07 02:14:09 -0800123 coreContext.output2 = output;
124
125 coreContext.minOut = 0.0;
126 coreContext.maxOut = 0.0;
127
Patrick Ventured8012182018-03-08 08:21:38 -0800128 // slew rate
129 // TODO(aarena) - Simplify logic as Andy suggested by creating dynamic
Patrick Venture7442c372019-02-11 10:21:05 -0800130 // outLim_min/max that are affected by slew rate control and just clamping
Patrick Ventured8012182018-03-08 08:21:38 -0800131 // to those instead of effectively clamping twice.
132 if (pidinfoptr->initialized)
133 {
Patrick Venture4b0df322019-02-11 09:04:57 -0800134 if (pidinfoptr->slewNeg != 0.0f)
Patrick Ventured8012182018-03-08 08:21:38 -0800135 {
136 // Don't decrease too fast
Patrick Venturea23468e2019-02-11 09:25:56 -0800137 double minOut =
Patrick Venture4b0df322019-02-11 09:04:57 -0800138 pidinfoptr->lastOutput + pidinfoptr->slewNeg * pidinfoptr->ts;
Josh Lehande745422020-11-07 02:14:09 -0800139
140 coreContext.minOut = minOut;
141
Patrick Venturea23468e2019-02-11 09:25:56 -0800142 if (output < minOut)
Patrick Ventured8012182018-03-08 08:21:38 -0800143 {
Patrick Venturea23468e2019-02-11 09:25:56 -0800144 output = minOut;
Patrick Ventured8012182018-03-08 08:21:38 -0800145 }
146 }
Patrick Venture4b0df322019-02-11 09:04:57 -0800147 if (pidinfoptr->slewPos != 0.0f)
Patrick Ventured8012182018-03-08 08:21:38 -0800148 {
149 // Don't increase too fast
Patrick Venturea23468e2019-02-11 09:25:56 -0800150 double maxOut =
Patrick Venture4b0df322019-02-11 09:04:57 -0800151 pidinfoptr->lastOutput + pidinfoptr->slewPos * pidinfoptr->ts;
Josh Lehande745422020-11-07 02:14:09 -0800152
153 coreContext.maxOut = maxOut;
154
Patrick Venturea23468e2019-02-11 09:25:56 -0800155 if (output > maxOut)
Patrick Ventured8012182018-03-08 08:21:38 -0800156 {
Patrick Venturea23468e2019-02-11 09:25:56 -0800157 output = maxOut;
Patrick Ventured8012182018-03-08 08:21:38 -0800158 }
159 }
160
Patrick Venture4b0df322019-02-11 09:04:57 -0800161 if (pidinfoptr->slewNeg != 0.0f || pidinfoptr->slewPos != 0.0f)
Patrick Ventured8012182018-03-08 08:21:38 -0800162 {
163 // Back calculate integral term for the cases where we limited the
164 // output
Patrick Venturea23468e2019-02-11 09:25:56 -0800165 integralTerm = output - proportionalTerm;
Patrick Ventured8012182018-03-08 08:21:38 -0800166 }
167 }
168
Josh Lehande745422020-11-07 02:14:09 -0800169 coreContext.output3 = output;
170 coreContext.integralTerm3 = integralTerm;
171
Patrick Ventured8012182018-03-08 08:21:38 -0800172 // Clamp again because having limited the output may result in a
173 // larger integral term
Patrick Venturea23468e2019-02-11 09:25:56 -0800174 integralTerm = clamp(integralTerm, pidinfoptr->integralLimit.min,
175 pidinfoptr->integralLimit.max);
176 pidinfoptr->integral = integralTerm;
Patrick Ventureda4a5dd2018-08-31 09:42:48 -0700177 pidinfoptr->initialized = true;
Bonnie Lo0e8fc392022-10-05 10:20:55 +0800178 pidinfoptr->lastError = error;
Patrick Venture4b0df322019-02-11 09:04:57 -0800179 pidinfoptr->lastOutput = output;
Patrick Ventured8012182018-03-08 08:21:38 -0800180
Josh Lehande745422020-11-07 02:14:09 -0800181 coreContext.integralTerm = pidinfoptr->integral;
182 coreContext.output = pidinfoptr->lastOutput;
183
184 if (logPtr)
185 {
186 LogContext(*logPtr, msNow, coreContext);
187 }
188
Patrick Ventured8012182018-03-08 08:21:38 -0800189 return output;
190}
191
Patrick Ventureda4a5dd2018-08-31 09:42:48 -0700192} // namespace ec
Patrick Venturea0764872020-08-08 07:48:43 -0700193} // namespace pid_control