1 #include "failsafe_logger.hpp"
2
3 #include <chrono>
4 #include <iostream>
5
6 namespace pid_control
7 {
8
outputFailsafeLog(const int64_t zoneId,const bool newFailsafeState,const std::string location,const std::string reason)9 void FailsafeLogger::outputFailsafeLog(
10 const int64_t zoneId, const bool newFailsafeState,
11 const std::string location, const std::string reason)
12 {
13 // Remove outdated log entries.
14 const auto now = std::chrono::high_resolution_clock::now();
15 uint64_t nowMs = std::chrono::duration_cast<std::chrono::milliseconds>(
16 now.time_since_epoch())
17 .count();
18 // Limit the log output in 1 second.
19 constexpr uint64_t secondInMS = 1000; // 1 second in milliseconds
20 while (!_logTimestamps.empty() &&
21 nowMs - _logTimestamps.front() >= secondInMS)
22 {
23 _logTimestamps.pop_front();
24 }
25
26 // There is a failsafe state change, clear the logs in current state.
27 bool originFailsafeState = _currentFailsafeState;
28 if (newFailsafeState != _currentFailsafeState)
29 {
30 _logsInCurrentState.clear();
31 _currentFailsafeState = newFailsafeState;
32 }
33 // Do not output the log if the capacity is reached, or if the log is
34 // already encountered in the current state.
35 std::string locationReason = location + " @ " + reason;
36 if (_logTimestamps.size() >= _logMaxCountPerSecond ||
37 !_logsInCurrentState.contains(locationReason))
38 {
39 return;
40 }
41 _logsInCurrentState.insert(locationReason);
42
43 // Only output the log if the zone enters, stays in, or leaves failsafe
44 // mode. No need to output the log if the zone stays in non-failsafe mode.
45 if (newFailsafeState)
46 {
47 std::cerr << "Zone `" << zoneId
48 << "` is in failsafe mode.\t\tWith update at `" << location
49 << "`: " << reason << "\n";
50 }
51 else if (!newFailsafeState && originFailsafeState)
52 {
53 std::cerr << "Zone `" << zoneId
54 << "` leaves failsafe mode.\t\tWith update at `" << location
55 << "`: " << reason << "\n";
56 }
57
58 _logTimestamps.push_back(nowMs);
59 }
60
61 } // namespace pid_control
62