update Diagnostics

src/olei_lidar.cpp

@@ -39,6 +39,22 @@ static constexpr uint16_t FRAME_ID_A = 0xFAF0;  // 2D Ethernet (VB, VF, LR-1F)
 static constexpr uint16_t FRAME_ID_B = 0xFEF0;  // LR-1BS5 / LR-1BS2 Ethernet variant
 static constexpr uint16_t FRAME_ID_C = 0xFEAC;  // Protocol V3 (GS1-5)
 
+Diagnostics decode_diagnostics(const ExtraInfo& info) {
+    Diagnostics d;
+    d.valid              = true;
+    d.model              = info.detected_model;
+    d.error_status       = info.error_status;
+    d.rotation_raw       = info.rotation_raw;
+    d.scan_frequency_raw = info.scan_frequency_raw;
+    d.input_status       = info.input_status;
+    d.output_status      = info.output_status;
+    d.field_status       = info.field_status;
+    d.status_flags       = info.status_flags;
+    d.sick_device_status = info.sick_device_status;
+    d.nano_general_state = info.nano_general_state;
+    return d;
+}
+
 Driver::Driver(const ModelConfig& cfg, const std::string& ip, uint16_t port, bool inverted)
     : cfg_(cfg), ip_(ip), port_(port), inverted_(inverted)
 {
@@ -78,6 +94,7 @@ ErrorCode Driver::open() {
     pending_dist_m_.clear();
     pending_intensity_.clear();
     pending_info_ = ExtraInfo{};
+    latest_diag_  = Diagnostics{};
     last_angle_   = -1.f;
     scan_ready_   = false;
 
@@ -176,6 +193,9 @@ void Driver::flush_scan() {
     info.detected_model = detected_model_name_;
     info.error_status    = pending_err_;
 
+    latest_diag_ = decode_diagnostics(info);
+    latest_diag_.device_timestamp_ms = scan.timestamp_ms;
+
     pending_angle_deg_.clear();
     pending_dist_m_.clear();
     pending_intensity_.clear();

src/sick_lidar.cpp

@@ -103,6 +103,7 @@ ErrorCode SickDriver::open() {
     ::setsockopt(sock_fd_, IPPROTO_TCP, TCP_NODELAY, &nodelay, sizeof(nodelay));
 
     recv_buf_.clear();
+    latest_diag_ = Diagnostics{};
 
     // Device is passive until told to stream.
     if (!send_telegram("sEN LMDscandata 1")) {
@@ -290,13 +291,16 @@ bool SickDriver::parse_lmdscandata(const std::string& telegram, ScanResult& out)
 
     ExtraInfo& info = out.info;
     info = ExtraInfo{};
-    info.detected_model   = cfg_.name;
-    info.error_status      = static_cast<uint8_t>(status0 & 0xFF);
+    info.detected_model     = cfg_.name;
+    info.sick_device_status = static_cast<uint16_t>(((status0 & 0xFF) << 8) | (status1 & 0xFF));
     info.status_flags       = (status0 << 8) | status1;
     info.scan_frequency_raw = static_cast<uint16_t>(scanning_frequency);
     info.input_status        = static_cast<uint16_t>((in0 << 8) | in1);
     info.output_status        = static_cast<uint16_t>((out0 << 8) | out1);
 
+    latest_diag_ = decode_diagnostics(info);
+    latest_diag_.device_timestamp_ms = scan.timestamp_ms;
+
     return true;
 }
 
@@ -332,6 +336,7 @@ ErrorCode NanoScanDriver::open() {
         return set_error((err == EADDRINUSE || err == EACCES) ? ErrorCode::PortInUse
                                                               : ErrorCode::BindFailed);
     }
+    latest_diag_ = Diagnostics{};
     return set_error(ErrorCode::Ok);
 }
 
@@ -424,6 +429,8 @@ bool NanoScanDriver::spin_once() {
 bool NanoScanDriver::parse_packet(const uint8_t* buf, int len, ScanResult& out) {
     if (len < 52) return false;
 
+    uint16_t gss_off = le16(buf + 32);  // General System State block
+    uint16_t gss_size = le16(buf + 34);
     uint16_t dv_off  = le16(buf + 36);
     uint16_t dv_size = le16(buf + 38);
     uint16_t md_off  = le16(buf + 40);
@@ -486,6 +493,13 @@ bool NanoScanDriver::parse_packet(const uint8_t* buf, int len, ScanResult& out)
     ExtraInfo& info = out.info;
     info = ExtraInfo{};
     info.detected_model = cfg_.name;
+    // Byte 0 holds the run/standby/contamination/manipulation flags
+    // (kNanoState*); the block is absent when not configured in the sensor.
+    if (gss_off != 0 && gss_size != 0 && static_cast<int>(gss_off) < len)
+        info.nano_general_state = buf[gss_off];
+
+    latest_diag_ = decode_diagnostics(info);
+    latest_diag_.device_timestamp_ms = scan.timestamp_ms;
 
     return true;
 }