update Diagnostics

include/lidarlib/diagnostics.hpp

@@ -0,0 +1,108 @@
+#pragma once
+#include <cstdint>
+#include <cstdio>
+#include <optional>
+#include <string>
+
+namespace lidarlib {
+
+struct ExtraInfo;  // lidar.hpp
+
+// Family A (0xFAF0) error_status bits, header byte [5]. Bits 3-7 are reserved
+// on the wire; a nonzero reserved bit is still reported as a fault.
+inline constexpr uint8_t kFaultMonitor     = 1u << 0;  // monitor / motor abnormal
+inline constexpr uint8_t kFaultVoltage     = 1u << 1;  // supply voltage out of range
+inline constexpr uint8_t kFaultTemperature = 1u << 2;  // internal temperature abnormal
+
+// SICK TiM LMDscandata device status (low word; Telegram Listing).
+inline constexpr uint16_t kSickStatusError            = 1u << 0;
+inline constexpr uint16_t kSickStatusPollutionWarning = 1u << 1;
+inline constexpr uint16_t kSickStatusPollutionError   = 1u << 2;
+
+// SICK nanoScan3 General System State byte 0 (layout from sick_safetyscanners;
+// NOT verified on real hardware).
+inline constexpr uint8_t kNanoStateRunMode              = 1u << 0;
+inline constexpr uint8_t kNanoStateStandby              = 1u << 1;
+inline constexpr uint8_t kNanoStateContaminationWarning = 1u << 2;
+inline constexpr uint8_t kNanoStateContaminationError   = 1u << 3;
+inline constexpr uint8_t kNanoStateReferenceContour     = 1u << 4;
+inline constexpr uint8_t kNanoStateManipulation         = 1u << 5;
+
+// Device self-diagnostics decoded from the data stream. Fields the family
+// doesn't carry stay std::nullopt (see docs/diagnostics.md for the per-family
+// wire layout). valid stays false until the driver has decoded one full scan.
+struct Diagnostics {
+    bool        valid = false;
+    std::string model = "AUTO";
+    uint32_t    device_timestamp_ms = 0;  // device clock; 0 if not on the wire
+
+    // Family A error byte (0 = no fault; Family B/C don't carry it)
+    uint8_t error_status = 0;
+
+    bool monitor_fault()     const { return (error_status & kFaultMonitor)     != 0; }
+    bool voltage_fault()     const { return (error_status & kFaultVoltage)     != 0; }
+    bool temperature_fault() const { return (error_status & kFaultTemperature) != 0; }
+
+    // Family A only — raw motor speed field, unit unverified
+    std::optional<uint16_t> rotation_raw;
+
+    // Family C / V3 (GS1-5) only — raw passthroughs, bit meanings unverified
+    std::optional<uint16_t> scan_frequency_raw;
+    std::optional<uint16_t> input_status;
+    std::optional<uint16_t> output_status;
+    std::optional<uint32_t> field_status;
+    std::optional<uint32_t> status_flags;
+
+    // SICK TiM — LMDscandata status pair (word0<<8)|word1
+    std::optional<uint16_t> sick_device_status;
+
+    bool sick_error()        const { return sick_device_status && (*sick_device_status & kSickStatusError); }
+    bool pollution_warning() const { return sick_device_status && (*sick_device_status & kSickStatusPollutionWarning); }
+    bool pollution_error()   const { return sick_device_status && (*sick_device_status & kSickStatusPollutionError); }
+
+    // SICK nanoScan3 — General System State byte 0
+    std::optional<uint8_t> nano_general_state;
+
+    bool contamination_warning() const { return nano_general_state && (*nano_general_state & kNanoStateContaminationWarning); }
+    bool contamination_error()   const { return nano_general_state && (*nano_general_state & kNanoStateContaminationError); }
+    bool manipulation()          const { return nano_general_state && (*nano_general_state & kNanoStateManipulation); }
+
+    // Fault = device says something is wrong now; warning = degraded but
+    // still measuring (dirty optics) — schedule cleaning/service.
+    bool has_fault() const {
+        return error_status != 0 || sick_error() || pollution_error()
+            || contamination_error() || manipulation();
+    }
+    bool has_warning() const { return pollution_warning() || contamination_warning(); }
+    bool healthy()     const { return valid && !has_fault(); }
+};
+
+// Decode the diagnostic fields of one scan; sets valid = true.
+Diagnostics decode_diagnostics(const ExtraInfo& info);
+
+// One-line log summary: "no data" / "ok" / "WARN: pollution" /
+// "FAULT: voltage temperature".
+inline std::string to_string(const Diagnostics& d) {
+    if (!d.valid)       return "no data";
+    if (!d.has_fault()) return d.has_warning()
+        ? std::string("WARN:") + (d.pollution_warning() ? " pollution" : "")
+                                + (d.contamination_warning() ? " contamination" : "")
+        : "ok";
+
+    std::string s = "FAULT:";
+    if (d.monitor_fault())       s += " monitor";
+    if (d.voltage_fault())       s += " voltage";
+    if (d.temperature_fault())   s += " temperature";
+    if (d.sick_error())          s += " device";
+    if (d.pollution_error())     s += " pollution";
+    if (d.contamination_error()) s += " contamination";
+    if (d.manipulation())        s += " manipulation";
+    if (uint8_t rest = d.error_status & ~(kFaultMonitor | kFaultVoltage | kFaultTemperature)) {
+        char buf[24];
+        std::snprintf(buf, sizeof(buf), " reserved(0x%02X)", rest);
+        s += buf;
+    }
+    return s;
+}
+
+} // namespace lidarlib