blob: 7d8b4032c3d2f63fe319d7ddd3abb1690a21622b [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
19namespace ec
20{
21
22/********************************
23 * clamp
24 *
25 */
Patrick Venture5f59c0f2018-11-11 12:55:14 -080026static double clamp(double x, double min, double max)
Patrick Ventured8012182018-03-08 08:21:38 -080027{
28 if (x < min)
29 {
30 return min;
31 }
32 else if (x > max)
33 {
34 return max;
35 }
36 return x;
37}
38
39/********************************
40 * pid code
41 * Note: Codes assumes the ts field is non-zero
42 */
Patrick Venture5f59c0f2018-11-11 12:55:14 -080043double pid(pid_info_t* pidinfoptr, double input, double setpoint)
Patrick Ventured8012182018-03-08 08:21:38 -080044{
Patrick Venture5f59c0f2018-11-11 12:55:14 -080045 double error;
Patrick Ventured8012182018-03-08 08:21:38 -080046
Patrick Venturea23468e2019-02-11 09:25:56 -080047 double proportionalTerm;
48 double integralTerm = 0.0f;
49 double feedFwdTerm = 0.0f;
Patrick Ventured8012182018-03-08 08:21:38 -080050
Patrick Venture5f59c0f2018-11-11 12:55:14 -080051 double output;
Patrick Ventured8012182018-03-08 08:21:38 -080052
53 // calculate P, I, D, FF
54
55 // Pid
Patrick Ventureda4a5dd2018-08-31 09:42:48 -070056 error = setpoint - input;
Patrick Venturea23468e2019-02-11 09:25:56 -080057 proportionalTerm = pidinfoptr->proportionalCoeff * error;
Patrick Ventured8012182018-03-08 08:21:38 -080058
59 // pId
Patrick Venture4b0df322019-02-11 09:04:57 -080060 if (0.0f != pidinfoptr->integralCoeff)
Patrick Ventured8012182018-03-08 08:21:38 -080061 {
Patrick Venturea23468e2019-02-11 09:25:56 -080062 integralTerm = pidinfoptr->integral;
63 integralTerm += error * pidinfoptr->integralCoeff * pidinfoptr->ts;
64 integralTerm = clamp(integralTerm, pidinfoptr->integralLimit.min,
65 pidinfoptr->integralLimit.max);
Patrick Ventured8012182018-03-08 08:21:38 -080066 }
67
68 // FF
Patrick Venturea23468e2019-02-11 09:25:56 -080069 feedFwdTerm =
70 (setpoint + pidinfoptr->feedFwdOffset) * pidinfoptr->feedFwdGain;
Patrick Ventured8012182018-03-08 08:21:38 -080071
Patrick Venturea23468e2019-02-11 09:25:56 -080072 output = proportionalTerm + integralTerm + feedFwdTerm;
Patrick Venture4b0df322019-02-11 09:04:57 -080073 output = clamp(output, pidinfoptr->outLim.min, pidinfoptr->outLim.max);
Patrick Ventured8012182018-03-08 08:21:38 -080074
75 // slew rate
76 // TODO(aarena) - Simplify logic as Andy suggested by creating dynamic
Patrick Venture7442c372019-02-11 10:21:05 -080077 // outLim_min/max that are affected by slew rate control and just clamping
Patrick Ventured8012182018-03-08 08:21:38 -080078 // to those instead of effectively clamping twice.
79 if (pidinfoptr->initialized)
80 {
Patrick Venture4b0df322019-02-11 09:04:57 -080081 if (pidinfoptr->slewNeg != 0.0f)
Patrick Ventured8012182018-03-08 08:21:38 -080082 {
83 // Don't decrease too fast
Patrick Venturea23468e2019-02-11 09:25:56 -080084 double minOut =
Patrick Venture4b0df322019-02-11 09:04:57 -080085 pidinfoptr->lastOutput + pidinfoptr->slewNeg * pidinfoptr->ts;
Patrick Venturea23468e2019-02-11 09:25:56 -080086 if (output < minOut)
Patrick Ventured8012182018-03-08 08:21:38 -080087 {
Patrick Venturea23468e2019-02-11 09:25:56 -080088 output = minOut;
Patrick Ventured8012182018-03-08 08:21:38 -080089 }
90 }
Patrick Venture4b0df322019-02-11 09:04:57 -080091 if (pidinfoptr->slewPos != 0.0f)
Patrick Ventured8012182018-03-08 08:21:38 -080092 {
93 // Don't increase too fast
Patrick Venturea23468e2019-02-11 09:25:56 -080094 double maxOut =
Patrick Venture4b0df322019-02-11 09:04:57 -080095 pidinfoptr->lastOutput + pidinfoptr->slewPos * pidinfoptr->ts;
Patrick Venturea23468e2019-02-11 09:25:56 -080096 if (output > maxOut)
Patrick Ventured8012182018-03-08 08:21:38 -080097 {
Patrick Venturea23468e2019-02-11 09:25:56 -080098 output = maxOut;
Patrick Ventured8012182018-03-08 08:21:38 -080099 }
100 }
101
Patrick Venture4b0df322019-02-11 09:04:57 -0800102 if (pidinfoptr->slewNeg != 0.0f || pidinfoptr->slewPos != 0.0f)
Patrick Ventured8012182018-03-08 08:21:38 -0800103 {
104 // Back calculate integral term for the cases where we limited the
105 // output
Patrick Venturea23468e2019-02-11 09:25:56 -0800106 integralTerm = output - proportionalTerm;
Patrick Ventured8012182018-03-08 08:21:38 -0800107 }
108 }
109
110 // Clamp again because having limited the output may result in a
111 // larger integral term
Patrick Venturea23468e2019-02-11 09:25:56 -0800112 integralTerm = clamp(integralTerm, pidinfoptr->integralLimit.min,
113 pidinfoptr->integralLimit.max);
114 pidinfoptr->integral = integralTerm;
Patrick Ventureda4a5dd2018-08-31 09:42:48 -0700115 pidinfoptr->initialized = true;
Patrick Venture4b0df322019-02-11 09:04:57 -0800116 pidinfoptr->lastOutput = output;
Patrick Ventured8012182018-03-08 08:21:38 -0800117
118 return output;
119}
120
Patrick Ventureda4a5dd2018-08-31 09:42:48 -0700121} // namespace ec