Adding new feature of core PID loop logging
This differs from the normal logging,
as this focuses on the core of the PID loop computations.
All variables within the core pid/ec/pid.cpp pid()
function are now logged, so math can be debugged.
Output is throttled to only log a new line
when it changes, or when 60 seconds have elapsed.
Creates 2 files for each PID loop,
one showing coefficients that were configured for it,
and one showing the variables changing over time.
Enable by --corelogging command line option,
or by creating /etc/thermal.d/corelogging file.
Tested:
Log files appear as expected, when enabled.
No changes noticed, when this feature is disabled.
Signed-off-by: Josh Lehan <krellan@google.com>
Change-Id: I3f37fe918e7cbc6fb885ffa2f268600d5a317d32
diff --git a/pid/ec/logging.cpp b/pid/ec/logging.cpp
new file mode 100644
index 0000000..57f1e7e
--- /dev/null
+++ b/pid/ec/logging.cpp
@@ -0,0 +1,289 @@
+/**
+ * Copyright 2022 Google Inc.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "logging.hpp"
+
+#include "../tuning.hpp"
+#include "pid.hpp"
+
+#include <chrono>
+#include <fstream>
+#include <iostream>
+#include <map>
+#include <string>
+
+namespace pid_control
+{
+namespace ec
+{
+
+// Redundant log entries only once every 60 seconds
+static constexpr int logThrottle = 60 * 1000;
+
+static std::map<std::string, PidCoreLog> nameToLog;
+
+static bool CharValid(const std::string::value_type& ch)
+{
+ // Intentionally avoiding invoking locale support here
+ if ((ch >= 'A') && (ch <= 'Z'))
+ {
+ return true;
+ }
+ if ((ch >= 'a') && (ch <= 'z'))
+ {
+ return true;
+ }
+ if ((ch >= '0') && (ch <= '9'))
+ {
+ return true;
+ }
+ return false;
+}
+
+static std::string StrClean(const std::string& str)
+{
+ std::string res;
+ size_t len = str.size();
+ for (size_t i = 0; i < len; ++i)
+ {
+ const auto& c = str[i];
+ if (!(CharValid(c)))
+ {
+ continue;
+ }
+ res += c;
+ }
+ return res;
+}
+
+static void DumpContextHeader(std::ofstream& file)
+{
+ file << "epoch_ms,input,setpoint,error";
+ file << ",proportionalTerm";
+ file << ",integralTerm1,integralTerm2";
+ file << ",derivativeTerm";
+ file << ",feedFwdTerm,output1,output2";
+ file << ",minOut,maxOut";
+ file << ",integralTerm3,output3";
+ file << ",integralTerm,output";
+ file << "\n" << std::flush;
+}
+
+static void DumpContextData(std::ofstream& file,
+ const std::chrono::milliseconds& msNow,
+ const PidCoreContext& pc)
+{
+ file << msNow.count();
+ file << "," << pc.input << "," << pc.setpoint << "," << pc.error;
+ file << "," << pc.proportionalTerm;
+ file << "," << pc.integralTerm1 << "," << pc.integralTerm2;
+ file << "," << pc.derivativeTerm;
+ file << "," << pc.feedFwdTerm << "," << pc.output1 << "," << pc.output2;
+ file << "," << pc.minOut << "," << pc.maxOut;
+ file << "," << pc.integralTerm3 << "," << pc.output3;
+ file << "," << pc.integralTerm << "," << pc.output;
+ file << "\n" << std::flush;
+}
+
+static void DumpCoeffsHeader(std::ofstream& file)
+{
+ file << "epoch_ms,ts,integral,lastOutput";
+ file << ",proportionalCoeff,integralCoeff";
+ file << ",derivativeCoeff";
+ file << ",feedFwdOffset,feedFwdGain";
+ file << ",integralLimit.min,integralLimit.max";
+ file << ",outLim.min,outLim.max";
+ file << ",slewNeg,slewPos";
+ file << ",positiveHysteresis,negativeHysteresis";
+ file << "\n" << std::flush;
+}
+
+static void DumpCoeffsData(std::ofstream& file,
+ const std::chrono::milliseconds& msNow,
+ pid_info_t* pidinfoptr)
+{
+ // Save some typing
+ const auto& p = *pidinfoptr;
+
+ file << msNow.count();
+ file << "," << p.ts << "," << p.integral << "," << p.lastOutput;
+ file << "," << p.proportionalCoeff << "," << p.integralCoeff;
+ file << "," << p.derivativeCoeff;
+ file << "," << p.feedFwdOffset << "," << p.feedFwdGain;
+ file << "," << p.integralLimit.min << "," << p.integralLimit.max;
+ file << "," << p.outLim.min << "," << p.outLim.max;
+ file << "," << p.slewNeg << "," << p.slewPos;
+ file << "," << p.positiveHysteresis << "," << p.negativeHysteresis;
+ file << "\n" << std::flush;
+}
+
+void LogInit(const std::string& name, pid_info_t* pidinfoptr)
+{
+ if (!coreLoggingEnabled)
+ {
+ // PID logging not enabled by configuration, silently do nothing
+ return;
+ }
+
+ if (name.empty())
+ {
+ std::cerr << "PID logging disabled because PID does not have a name\n";
+ return;
+ }
+
+ std::string cleanName = StrClean(name);
+ if (cleanName.empty())
+ {
+ std::cerr << "PID logging disabled because PID name is unusable: "
+ << name << "\n";
+ return;
+ }
+
+ auto iterExisting = nameToLog.find(name);
+
+ if (iterExisting != nameToLog.end())
+ {
+ std::cerr << "PID logging reusing existing file: " << name << "\n";
+ }
+ else
+ {
+ // Multiple names could collide to the same clean name
+ // Make sure clean name is not already used
+ for (const auto& iter : nameToLog)
+ {
+ if (iter.second.nameClean == cleanName)
+ {
+ std::cerr << "PID logging disabled because of name collision: "
+ << name << "\n";
+ return;
+ }
+ }
+
+ std::string filec = loggingPath + "/pidcore." + cleanName;
+ std::string filef = loggingPath + "/pidcoeffs." + cleanName;
+
+ std::ofstream outc;
+ std::ofstream outf;
+
+ outc.open(filec);
+ if (!(outc.good()))
+ {
+ std::cerr << "PID logging disabled because unable to open file: "
+ << filec << "\n";
+ return;
+ }
+
+ outf.open(filef);
+ if (!(outf.good()))
+ {
+ // Be sure to clean up all previous initialization
+ outf.close();
+
+ std::cerr << "PID logging disabled because unable to open file: "
+ << filef << "\n";
+ return;
+ }
+
+ PidCoreLog newLog;
+
+ // All good, commit to doing logging by moving into the map
+ newLog.nameOriginal = name;
+ newLog.nameClean = cleanName;
+ newLog.fileContext = std::move(outc);
+ newLog.fileCoeffs = std::move(outf);
+
+ // The streams within this object are not copyable, must move them
+ nameToLog[name] = std::move(newLog);
+
+ // This must now succeed, as it must be in the map
+ iterExisting = nameToLog.find(name);
+
+ // Write headers only when creating files for the first time
+ DumpContextHeader(iterExisting->second.fileContext);
+ DumpCoeffsHeader(iterExisting->second.fileCoeffs);
+
+ std::cerr << "PID logging initialized: " << name << "\n";
+ }
+
+ auto msNow = LogTimestamp();
+
+ // Write the coefficients only once per PID loop initialization
+ // If they change, caller will reinitialize the PID loops
+ DumpCoeffsData(iterExisting->second.fileCoeffs, msNow, pidinfoptr);
+
+ // Force the next logging line to be logged
+ iterExisting->second.lastLog = iterExisting->second.lastLog.zero();
+ iterExisting->second.lastContext = PidCoreContext();
+}
+
+PidCoreLog* LogPeek(const std::string& name)
+{
+ auto iter = nameToLog.find(name);
+ if (iter != nameToLog.end())
+ {
+ return &(iter->second);
+ }
+
+ return nullptr;
+}
+
+void LogContext(PidCoreLog& pidLog, const std::chrono::milliseconds& msNow,
+ const PidCoreContext& coreContext)
+{
+ bool shouldLog = false;
+
+ if (pidLog.lastLog == pidLog.lastLog.zero())
+ {
+ // It is the first time
+ shouldLog = true;
+ }
+ else
+ {
+ auto since = msNow - pidLog.lastLog;
+ if (since.count() >= logThrottle)
+ {
+ // It has been long enough since the last time
+ shouldLog = true;
+ }
+ }
+
+ if (pidLog.lastContext != coreContext)
+ {
+ // The content is different
+ shouldLog = true;
+ }
+
+ if (!shouldLog)
+ {
+ return;
+ }
+
+ pidLog.lastLog = msNow;
+ pidLog.lastContext = coreContext;
+
+ DumpContextData(pidLog.fileContext, msNow, coreContext);
+}
+
+std::chrono::milliseconds LogTimestamp(void)
+{
+ auto clockNow = std::chrono::high_resolution_clock::now();
+ auto msNow = std::chrono::duration_cast<std::chrono::milliseconds>(
+ clockNow.time_since_epoch());
+ return msNow;
+}
+
+} // namespace ec
+} // namespace pid_control
diff --git a/pid/ec/logging.hpp b/pid/ec/logging.hpp
new file mode 100644
index 0000000..3cbd714
--- /dev/null
+++ b/pid/ec/logging.hpp
@@ -0,0 +1,110 @@
+#pragma once
+
+#include "pid.hpp"
+
+#include <chrono>
+#include <cstring>
+#include <fstream>
+#include <iostream>
+#include <string>
+
+namespace pid_control
+{
+namespace ec
+{
+
+// Trivial class for information exported from core PID loop function
+struct PidCoreContext
+{
+ double input;
+ double setpoint;
+ double error;
+
+ double proportionalTerm;
+ double integralTerm1;
+ double integralTerm2;
+
+ double derivativeTerm;
+
+ double feedFwdTerm;
+ double output1;
+ double output2;
+
+ double minOut;
+ double maxOut;
+
+ double integralTerm3;
+ double output3;
+
+ double integralTerm;
+ double output;
+
+ bool operator!=(const PidCoreContext& rhs) const = default;
+ bool operator==(const PidCoreContext& rhs) const = default;
+};
+
+// Optional decorator class for each PID loop, to support logging
+// Although this is a trivial class, it ended up needing the Six Horsemen
+struct PidCoreLog
+{
+ std::string nameOriginal;
+ std::string nameClean;
+ std::ofstream fileContext;
+ std::ofstream fileCoeffs;
+ std::chrono::milliseconds lastLog;
+ PidCoreContext lastContext;
+ bool moved;
+
+ PidCoreLog() :
+ nameOriginal(), nameClean(), fileContext(), fileCoeffs(), lastLog(),
+ lastContext(), moved(false)
+ {}
+
+ PidCoreLog(const PidCoreLog& copy) = delete;
+
+ PidCoreLog& operator=(const PidCoreLog& copy) = delete;
+
+ PidCoreLog(PidCoreLog&& move)
+ {
+ // Reuse assignment operator below
+ *this = std::move(move);
+ }
+
+ PidCoreLog& operator=(PidCoreLog&& move)
+ {
+ if (this != &move)
+ {
+ *this = std::move(move);
+
+ // Mark the moved object, so destructor knows it was moved
+ move.moved = true;
+ }
+ return *this;
+ }
+
+ ~PidCoreLog()
+ {
+ // Do not close files if ownership was moved to another object
+ if (!moved)
+ {
+ fileContext.close();
+ fileCoeffs.close();
+ }
+ }
+};
+
+// Initializes logging files, call once per PID loop initialization
+void LogInit(const std::string& name, pid_info_t* pidinfoptr);
+
+// Returns PidCoreLog pointer, or nullptr if this PID loop not being logged
+PidCoreLog* LogPeek(const std::string& name);
+
+// Logs a line of logging, if different, or it has been long enough
+void LogContext(PidCoreLog& pidLog, const std::chrono::milliseconds& msNow,
+ const PidCoreContext& coreLog);
+
+// Takes a timestamp, suitable for column 1 of logging file output
+std::chrono::milliseconds LogTimestamp(void);
+
+} // namespace ec
+} // namespace pid_control
diff --git a/pid/ec/pid.cpp b/pid/ec/pid.cpp
index 10db1dd..6222f1a 100644
--- a/pid/ec/pid.cpp
+++ b/pid/ec/pid.cpp
@@ -16,6 +16,9 @@
#include "pid.hpp"
+#include "../tuning.hpp"
+#include "logging.hpp"
+
namespace pid_control
{
namespace ec
@@ -42,8 +45,30 @@
* pid code
* Note: Codes assumes the ts field is non-zero
*/
-double pid(pid_info_t* pidinfoptr, double input, double setpoint)
+double pid(pid_info_t* pidinfoptr, double input, double setpoint,
+ const std::string* nameptr)
{
+ if (nameptr)
+ {
+ if (!(pidinfoptr->initialized))
+ {
+ LogInit(*nameptr, pidinfoptr);
+ }
+ }
+
+ auto logPtr = nameptr ? LogPeek(*nameptr) : nullptr;
+
+ PidCoreContext coreContext;
+ std::chrono::milliseconds msNow;
+
+ if (logPtr)
+ {
+ msNow = LogTimestamp();
+ }
+
+ coreContext.input = input;
+ coreContext.setpoint = setpoint;
+
double error;
double proportionalTerm;
@@ -59,26 +84,47 @@
error = setpoint - input;
proportionalTerm = pidinfoptr->proportionalCoeff * error;
+ coreContext.error = error;
+ coreContext.proportionalTerm = proportionalTerm;
+ coreContext.integralTerm1 = 0.0;
+
// pId
if (0.0f != pidinfoptr->integralCoeff)
{
integralTerm = pidinfoptr->integral;
integralTerm += error * pidinfoptr->integralCoeff * pidinfoptr->ts;
+
+ coreContext.integralTerm1 = integralTerm;
+
integralTerm = clamp(integralTerm, pidinfoptr->integralLimit.min,
pidinfoptr->integralLimit.max);
}
+ coreContext.integralTerm2 = integralTerm;
+
// piD
derivativeTerm = pidinfoptr->derivativeCoeff *
((error - pidinfoptr->lastError) / pidinfoptr->ts);
+ coreContext.derivativeTerm = derivativeTerm;
+
// FF
feedFwdTerm =
(setpoint + pidinfoptr->feedFwdOffset) * pidinfoptr->feedFwdGain;
+ coreContext.feedFwdTerm = feedFwdTerm;
+
output = proportionalTerm + integralTerm + derivativeTerm + feedFwdTerm;
+
+ coreContext.output1 = output;
+
output = clamp(output, pidinfoptr->outLim.min, pidinfoptr->outLim.max);
+ coreContext.output2 = output;
+
+ coreContext.minOut = 0.0;
+ coreContext.maxOut = 0.0;
+
// slew rate
// TODO(aarena) - Simplify logic as Andy suggested by creating dynamic
// outLim_min/max that are affected by slew rate control and just clamping
@@ -90,6 +136,9 @@
// Don't decrease too fast
double minOut =
pidinfoptr->lastOutput + pidinfoptr->slewNeg * pidinfoptr->ts;
+
+ coreContext.minOut = minOut;
+
if (output < minOut)
{
output = minOut;
@@ -100,6 +149,9 @@
// Don't increase too fast
double maxOut =
pidinfoptr->lastOutput + pidinfoptr->slewPos * pidinfoptr->ts;
+
+ coreContext.maxOut = maxOut;
+
if (output > maxOut)
{
output = maxOut;
@@ -114,6 +166,9 @@
}
}
+ coreContext.output3 = output;
+ coreContext.integralTerm3 = integralTerm;
+
// Clamp again because having limited the output may result in a
// larger integral term
integralTerm = clamp(integralTerm, pidinfoptr->integralLimit.min,
@@ -123,6 +178,14 @@
pidinfoptr->lastError = error;
pidinfoptr->lastOutput = output;
+ coreContext.integralTerm = pidinfoptr->integral;
+ coreContext.output = pidinfoptr->lastOutput;
+
+ if (logPtr)
+ {
+ LogContext(*logPtr, msNow, coreContext);
+ }
+
return output;
}
diff --git a/pid/ec/pid.hpp b/pid/ec/pid.hpp
index 255c911..a6b6c4e 100644
--- a/pid/ec/pid.hpp
+++ b/pid/ec/pid.hpp
@@ -1,6 +1,7 @@
#pragma once
#include <cstdint>
+#include <string>
namespace pid_control
{
@@ -39,7 +40,8 @@
double negativeHysteresis;
} pid_info_t;
-double pid(pid_info_t* pidinfoptr, double input, double setpoint);
+double pid(pid_info_t* pidinfoptr, double input, double setpoint,
+ const std::string* nameptr = nullptr);
/* Condensed version for use by the configuration. */
struct pidinfo