1 #include "failsafe_logger.hpp" 2 3 #include <chrono> 4 #include <iostream> 5 6 namespace pid_control 7 { 8 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