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