blob: 10db1ddf59d135c2328ab04e9136e9be9df2217f [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
Patrick Venturea0764872020-08-08 07:48:43 -070019namespace pid_control
20{
Patrick Ventured8012182018-03-08 08:21:38 -080021namespace ec
22{
23
24/********************************
25 * clamp
26 *
27 */
Patrick Venture5f59c0f2018-11-11 12:55:14 -080028static double clamp(double x, double min, double max)
Patrick Ventured8012182018-03-08 08:21:38 -080029{
30 if (x < min)
31 {
32 return min;
33 }
34 else if (x > max)
35 {
36 return max;
37 }
38 return x;
39}
40
41/********************************
42 * pid code
43 * Note: Codes assumes the ts field is non-zero
44 */
Patrick Venture5f59c0f2018-11-11 12:55:14 -080045double pid(pid_info_t* pidinfoptr, double input, double setpoint)
Patrick Ventured8012182018-03-08 08:21:38 -080046{
Patrick Venture5f59c0f2018-11-11 12:55:14 -080047 double error;
Patrick Ventured8012182018-03-08 08:21:38 -080048
Patrick Venturea23468e2019-02-11 09:25:56 -080049 double proportionalTerm;
50 double integralTerm = 0.0f;
Bonnie Lo0e8fc392022-10-05 10:20:55 +080051 double derivativeTerm = 0.0f;
Patrick Venturea23468e2019-02-11 09:25:56 -080052 double feedFwdTerm = 0.0f;
Patrick Ventured8012182018-03-08 08:21:38 -080053
Patrick Venture5f59c0f2018-11-11 12:55:14 -080054 double output;
Patrick Ventured8012182018-03-08 08:21:38 -080055
56 // calculate P, I, D, FF
57
58 // Pid
Patrick Ventureda4a5dd2018-08-31 09:42:48 -070059 error = setpoint - input;
Patrick Venturea23468e2019-02-11 09:25:56 -080060 proportionalTerm = pidinfoptr->proportionalCoeff * error;
Patrick Ventured8012182018-03-08 08:21:38 -080061
62 // pId
Patrick Venture4b0df322019-02-11 09:04:57 -080063 if (0.0f != pidinfoptr->integralCoeff)
Patrick Ventured8012182018-03-08 08:21:38 -080064 {
Patrick Venturea23468e2019-02-11 09:25:56 -080065 integralTerm = pidinfoptr->integral;
66 integralTerm += error * pidinfoptr->integralCoeff * pidinfoptr->ts;
67 integralTerm = clamp(integralTerm, pidinfoptr->integralLimit.min,
68 pidinfoptr->integralLimit.max);
Patrick Ventured8012182018-03-08 08:21:38 -080069 }
70
Bonnie Lo0e8fc392022-10-05 10:20:55 +080071 // piD
72 derivativeTerm = pidinfoptr->derivativeCoeff *
73 ((error - pidinfoptr->lastError) / pidinfoptr->ts);
74
Patrick Ventured8012182018-03-08 08:21:38 -080075 // FF
Patrick Venturea23468e2019-02-11 09:25:56 -080076 feedFwdTerm =
77 (setpoint + pidinfoptr->feedFwdOffset) * pidinfoptr->feedFwdGain;
Patrick Ventured8012182018-03-08 08:21:38 -080078
Bonnie Lo0e8fc392022-10-05 10:20:55 +080079 output = proportionalTerm + integralTerm + derivativeTerm + feedFwdTerm;
Patrick Venture4b0df322019-02-11 09:04:57 -080080 output = clamp(output, pidinfoptr->outLim.min, pidinfoptr->outLim.max);
Patrick Ventured8012182018-03-08 08:21:38 -080081
82 // slew rate
83 // TODO(aarena) - Simplify logic as Andy suggested by creating dynamic
Patrick Venture7442c372019-02-11 10:21:05 -080084 // outLim_min/max that are affected by slew rate control and just clamping
Patrick Ventured8012182018-03-08 08:21:38 -080085 // to those instead of effectively clamping twice.
86 if (pidinfoptr->initialized)
87 {
Patrick Venture4b0df322019-02-11 09:04:57 -080088 if (pidinfoptr->slewNeg != 0.0f)
Patrick Ventured8012182018-03-08 08:21:38 -080089 {
90 // Don't decrease too fast
Patrick Venturea23468e2019-02-11 09:25:56 -080091 double minOut =
Patrick Venture4b0df322019-02-11 09:04:57 -080092 pidinfoptr->lastOutput + pidinfoptr->slewNeg * pidinfoptr->ts;
Patrick Venturea23468e2019-02-11 09:25:56 -080093 if (output < minOut)
Patrick Ventured8012182018-03-08 08:21:38 -080094 {
Patrick Venturea23468e2019-02-11 09:25:56 -080095 output = minOut;
Patrick Ventured8012182018-03-08 08:21:38 -080096 }
97 }
Patrick Venture4b0df322019-02-11 09:04:57 -080098 if (pidinfoptr->slewPos != 0.0f)
Patrick Ventured8012182018-03-08 08:21:38 -080099 {
100 // Don't increase too fast
Patrick Venturea23468e2019-02-11 09:25:56 -0800101 double maxOut =
Patrick Venture4b0df322019-02-11 09:04:57 -0800102 pidinfoptr->lastOutput + pidinfoptr->slewPos * pidinfoptr->ts;
Patrick Venturea23468e2019-02-11 09:25:56 -0800103 if (output > maxOut)
Patrick Ventured8012182018-03-08 08:21:38 -0800104 {
Patrick Venturea23468e2019-02-11 09:25:56 -0800105 output = maxOut;
Patrick Ventured8012182018-03-08 08:21:38 -0800106 }
107 }
108
Patrick Venture4b0df322019-02-11 09:04:57 -0800109 if (pidinfoptr->slewNeg != 0.0f || pidinfoptr->slewPos != 0.0f)
Patrick Ventured8012182018-03-08 08:21:38 -0800110 {
111 // Back calculate integral term for the cases where we limited the
112 // output
Patrick Venturea23468e2019-02-11 09:25:56 -0800113 integralTerm = output - proportionalTerm;
Patrick Ventured8012182018-03-08 08:21:38 -0800114 }
115 }
116
117 // Clamp again because having limited the output may result in a
118 // larger integral term
Patrick Venturea23468e2019-02-11 09:25:56 -0800119 integralTerm = clamp(integralTerm, pidinfoptr->integralLimit.min,
120 pidinfoptr->integralLimit.max);
121 pidinfoptr->integral = integralTerm;
Patrick Ventureda4a5dd2018-08-31 09:42:48 -0700122 pidinfoptr->initialized = true;
Bonnie Lo0e8fc392022-10-05 10:20:55 +0800123 pidinfoptr->lastError = error;
Patrick Venture4b0df322019-02-11 09:04:57 -0800124 pidinfoptr->lastOutput = output;
Patrick Ventured8012182018-03-08 08:21:38 -0800125
126 return output;
127}
128
Patrick Ventureda4a5dd2018-08-31 09:42:48 -0700129} // namespace ec
Patrick Venturea0764872020-08-08 07:48:43 -0700130} // namespace pid_control