1 // SPDX-License-Identifier: Apache-2.0
2 // SPDX-FileCopyrightText: Copyright 2022 Google Inc
3
4 #include "logging.hpp"
5
6 #include "../tuning.hpp"
7 #include "pid.hpp"
8
9 #include <chrono>
10 #include <cstddef>
11 #include <fstream>
12 #include <iostream>
13 #include <map>
14 #include <string>
15 #include <utility>
16
17 namespace pid_control
18 {
19 namespace ec
20 {
21
22 // Redundant log entries only once every 60 seconds
23 static constexpr int logThrottle = 60 * 1000;
24
25 static std::map<std::string, PidCoreLog> nameToLog;
26
CharValid(const std::string::value_type & ch)27 static bool CharValid(const std::string::value_type& ch)
28 {
29 // Intentionally avoiding invoking locale support here
30 if ((ch >= 'A') && (ch <= 'Z'))
31 {
32 return true;
33 }
34 if ((ch >= 'a') && (ch <= 'z'))
35 {
36 return true;
37 }
38 if ((ch >= '0') && (ch <= '9'))
39 {
40 return true;
41 }
42 return false;
43 }
44
StrClean(const std::string & str)45 static std::string StrClean(const std::string& str)
46 {
47 std::string res;
48 size_t len = str.size();
49 for (size_t i = 0; i < len; ++i)
50 {
51 const auto& c = str[i];
52 if (!(CharValid(c)))
53 {
54 continue;
55 }
56 res += c;
57 }
58 return res;
59 }
60
DumpContextHeader(std::ofstream & file)61 static void DumpContextHeader(std::ofstream& file)
62 {
63 file << "epoch_ms,input,setpoint,error";
64 file << ",proportionalTerm";
65 file << ",integralTerm1,integralTerm2";
66 file << ",derivativeTerm";
67 file << ",feedFwdTerm,output1,output2";
68 file << ",minOut,maxOut";
69 file << ",integralTerm3,output3";
70 file << ",integralTerm,output";
71 file << "\n" << std::flush;
72 }
73
DumpContextData(std::ofstream & file,const std::chrono::milliseconds & msNow,const PidCoreContext & pc)74 static void DumpContextData(std::ofstream& file,
75 const std::chrono::milliseconds& msNow,
76 const PidCoreContext& pc)
77 {
78 file << msNow.count();
79 file << "," << pc.input << "," << pc.setpoint << "," << pc.error;
80 file << "," << pc.proportionalTerm;
81 file << "," << pc.integralTerm1 << "," << pc.integralTerm2;
82 file << "," << pc.derivativeTerm;
83 file << "," << pc.feedFwdTerm << "," << pc.output1 << "," << pc.output2;
84 file << "," << pc.minOut << "," << pc.maxOut;
85 file << "," << pc.integralTerm3 << "," << pc.output3;
86 file << "," << pc.integralTerm << "," << pc.output;
87 file << "\n" << std::flush;
88 }
89
DumpCoeffsHeader(std::ofstream & file)90 static void DumpCoeffsHeader(std::ofstream& file)
91 {
92 file << "epoch_ms,ts,integral,lastOutput";
93 file << ",proportionalCoeff,integralCoeff";
94 file << ",derivativeCoeff";
95 file << ",feedFwdOffset,feedFwdGain";
96 file << ",integralLimit.min,integralLimit.max";
97 file << ",outLim.min,outLim.max";
98 file << ",slewNeg,slewPos";
99 file << ",positiveHysteresis,negativeHysteresis";
100 file << "\n" << std::flush;
101 }
102
DumpCoeffsData(std::ofstream & file,const std::chrono::milliseconds & msNow,pid_info_t * pidinfoptr)103 static void DumpCoeffsData(std::ofstream& file,
104 const std::chrono::milliseconds& msNow,
105 pid_info_t* pidinfoptr)
106 {
107 // Save some typing
108 const auto& p = *pidinfoptr;
109
110 file << msNow.count();
111 file << "," << p.ts << "," << p.integral << "," << p.lastOutput;
112 file << "," << p.proportionalCoeff << "," << p.integralCoeff;
113 file << "," << p.derivativeCoeff;
114 file << "," << p.feedFwdOffset << "," << p.feedFwdGain;
115 file << "," << p.integralLimit.min << "," << p.integralLimit.max;
116 file << "," << p.outLim.min << "," << p.outLim.max;
117 file << "," << p.slewNeg << "," << p.slewPos;
118 file << "," << p.positiveHysteresis << "," << p.negativeHysteresis;
119 file << "\n" << std::flush;
120 }
121
LogInit(const std::string & name,pid_info_t * pidinfoptr)122 void LogInit(const std::string& name, pid_info_t* pidinfoptr)
123 {
124 if (!coreLoggingEnabled)
125 {
126 // PID logging not enabled by configuration, silently do nothing
127 return;
128 }
129
130 if (name.empty())
131 {
132 std::cerr << "PID logging disabled because PID does not have a name\n";
133 return;
134 }
135
136 std::string cleanName = StrClean(name);
137 if (cleanName.empty())
138 {
139 std::cerr << "PID logging disabled because PID name is unusable: "
140 << name << "\n";
141 return;
142 }
143
144 auto iterExisting = nameToLog.find(name);
145
146 if (iterExisting != nameToLog.end())
147 {
148 std::cerr << "PID logging reusing existing file: " << name << "\n";
149 }
150 else
151 {
152 // Multiple names could collide to the same clean name
153 // Make sure clean name is not already used
154 for (const auto& iter : nameToLog)
155 {
156 if (iter.second.nameClean == cleanName)
157 {
158 std::cerr << "PID logging disabled because of name collision: "
159 << name << "\n";
160 return;
161 }
162 }
163
164 std::string filec = loggingPath + "/pidcore." + cleanName;
165 std::string filef = loggingPath + "/pidcoeffs." + cleanName;
166
167 std::ofstream outc;
168 std::ofstream outf;
169
170 outc.open(filec);
171 if (!(outc.good()))
172 {
173 std::cerr << "PID logging disabled because unable to open file: "
174 << filec << "\n";
175 return;
176 }
177
178 outf.open(filef);
179 if (!(outf.good()))
180 {
181 // Be sure to clean up all previous initialization
182 outf.close();
183
184 std::cerr << "PID logging disabled because unable to open file: "
185 << filef << "\n";
186 return;
187 }
188
189 PidCoreLog newLog;
190
191 // All good, commit to doing logging by moving into the map
192 newLog.nameOriginal = name;
193 newLog.nameClean = cleanName;
194 newLog.fileContext = std::move(outc);
195 newLog.fileCoeffs = std::move(outf);
196
197 // The streams within this object are not copyable, must move them
198 nameToLog[name] = std::move(newLog);
199
200 // This must now succeed, as it must be in the map
201 iterExisting = nameToLog.find(name);
202
203 // Write headers only when creating files for the first time
204 DumpContextHeader(iterExisting->second.fileContext);
205 DumpCoeffsHeader(iterExisting->second.fileCoeffs);
206
207 std::cerr << "PID logging initialized: " << name << "\n";
208 }
209
210 auto msNow = LogTimestamp();
211
212 // Write the coefficients only once per PID loop initialization
213 // If they change, caller will reinitialize the PID loops
214 DumpCoeffsData(iterExisting->second.fileCoeffs, msNow, pidinfoptr);
215
216 // Force the next logging line to be logged
217 iterExisting->second.lastLog = iterExisting->second.lastLog.zero();
218 iterExisting->second.lastContext = PidCoreContext();
219 }
220
LogPeek(const std::string & name)221 PidCoreLog* LogPeek(const std::string& name)
222 {
223 auto iter = nameToLog.find(name);
224 if (iter != nameToLog.end())
225 {
226 return &(iter->second);
227 }
228
229 return nullptr;
230 }
231
LogContext(PidCoreLog & pidLog,const std::chrono::milliseconds & msNow,const PidCoreContext & coreContext)232 void LogContext(PidCoreLog& pidLog, const std::chrono::milliseconds& msNow,
233 const PidCoreContext& coreContext)
234 {
235 bool shouldLog = false;
236
237 if (pidLog.lastLog == pidLog.lastLog.zero())
238 {
239 // It is the first time
240 shouldLog = true;
241 }
242 else
243 {
244 auto since = msNow - pidLog.lastLog;
245 if (since.count() >= logThrottle)
246 {
247 // It has been long enough since the last time
248 shouldLog = true;
249 }
250 }
251
252 if (pidLog.lastContext != coreContext)
253 {
254 // The content is different
255 shouldLog = true;
256 }
257
258 if (!shouldLog)
259 {
260 return;
261 }
262
263 pidLog.lastLog = msNow;
264 pidLog.lastContext = coreContext;
265
266 DumpContextData(pidLog.fileContext, msNow, coreContext);
267 }
268
LogTimestamp(void)269 std::chrono::milliseconds LogTimestamp(void)
270 {
271 auto clockNow = std::chrono::high_resolution_clock::now();
272 auto msNow = std::chrono::duration_cast<std::chrono::milliseconds>(
273 clockNow.time_since_epoch());
274 return msNow;
275 }
276
277 } // namespace ec
278 } // namespace pid_control
279