update Diagnostics

2026-07-07 08:59:36 +07:00
parent 397b9ab3c5
commit 1c347a4918
8 changed files with 446 additions and 2 deletions
--- a/README.md
+++ b/README.md
@@ -95,6 +95,41 @@ Trạng thái instance:
  hai lần là no-op; `open()` hai lần trả `AlreadyOpen` và không đụng kết nối
  đang chạy; sau `close()` có thể `open()` lại (state scan dở được reset).

+### Chẩn đoán thiết bị (diagnostics)
+
+Lidar OLEI nhúng thông tin tự chẩn đoán trong header gói dữ liệu (không có
+kênh query riêng). API đọc ra qua `lidarlib/diagnostics.hpp` — nghiên cứu
+chi tiết layout từng family xem [docs/diagnostics.md](docs/diagnostics.md).
+
+```cpp
+lidarlib::Diagnostics d = lidar->get_diagnostics();
+if (!d.valid) {
+    // chưa decode được scan nào
+} else if (d.has_fault()) {
+    // OLEI Family A
+    d.monitor_fault();      // motor/giám sát bất thường
+    d.voltage_fault();      // điện áp ngoài dải
+    d.temperature_fault();  // nhiệt độ bất thường
+    // SICK
+    d.sick_error();          // TiM: device error
+    d.pollution_error();     // TiM: kính bẩn nặng
+    d.contamination_error(); // nanoScan3: kính bẩn nặng
+    d.manipulation();        // nanoScan3: nghi bị che/can thiệp
+    printf("lidar fault: %s\n", lidarlib::to_string(d).c_str());
+} else if (d.has_warning()) {
+    // pollution_warning() / contamination_warning() — kính bẩn nhẹ, nên lau
+}
+```
+
+- `get_diagnostics()` — snapshot từ vòng quét gần nhất; gọi cùng thread với
+  `recv_scan()`/`spin_once()`. `d.healthy()` = có data và không có fault.
+- `decode_diagnostics(result.info)` — decode gắn với một scan cụ thể.
+- Nguồn dữ liệu: OLEI Family A byte lỗi + `rotation_raw` + timestamp;
+  Family B không có gì trên wire; Family C `input/output/field_status`,
+  `status_flags` (raw, chưa verify); SICK TiM cặp device status trong
+  `LMDscandata`; nanoScan3 block General System State (cần bật trong
+  Safety Designer).
+
 ## Cấu hình (config.json)

 `lidar_app` là app mẫu headless: đọc `config.json`, mở từng lidar bằng
@@ -185,6 +220,7 @@ parse datagram; **đích UDP phải cấu hình sẵn trong SICK Safety Designer
 | File | Vai trò |
 |---|---|
 | `include/lidarlib/error.hpp` | `enum class ErrorCode` + `to_string()` |
+| `include/lidarlib/diagnostics.hpp` | `Diagnostics`, bit lỗi Family A, `decode_diagnostics()` |
 | `include/lidarlib/lidar.hpp` | Data model, interface `Lidar`, driver OLEI, các `MODEL_*` OLEI |
 | `include/lidarlib/sick_lidar.hpp` | `SickDriver`, `NanoScanDriver`, các `MODEL_SICK_*` |
 | `include/lidarlib/config.hpp` | `LidarConfig`, load/save JSON, `make_lidar()` |
@@ -202,6 +238,42 @@ parse datagram; **đích UDP phải cấu hình sẵn trong SICK Safety Designer

 ## Changelog

+### 2026-07-07 — Diagnostics API cho SICK
+
+- **`SickDriver` (TiM)**: decode cặp Device Status của `LMDscandata`
+  (0 ok / 1 error / 2 pollution warning / 4 pollution error) vào
+  `Diagnostics` — `sick_error()`, `pollution_warning()`, `pollution_error()`.
+  `info.sick_device_status` là trường mới; `info.error_status` **không còn**
+  nhận `status0` (trường đó thuộc Family A OLEI).
+- **`NanoScanDriver`**: parse block General System State (offset header
+  `[32]/[34]`, byte 0) → `contamination_warning()`, `contamination_error()`,
+  `manipulation()`. Layout theo `sick_safetyscanners`, chưa verify phần cứng;
+  block phải được bật trong Safety Designer.
+- **`Diagnostics` thêm `has_warning()`** (kính bẩn mức cảnh báo) và
+  `to_string()` in thêm `WARN: pollution` / `FAULT: ... contamination`.
+- Test loopback: TiM server TCP giả (status `0 4` → pollution error) và gói
+  nanoScan3 tổng hợp (state `0x09` → contamination error, gói sạch →
+  `healthy()`); tài liệu tại [docs/diagnostics.md](docs/diagnostics.md) §4-5.
+
+### 2026-07-07 — Diagnostics API
+
+- **Header mới `lidarlib/diagnostics.hpp`**: struct `Diagnostics` (bit lỗi
+  Family A decode sẵn: `monitor/voltage/temperature_fault()`, `has_fault()`,
+  `healthy()`; trường raw Family C), hằng `kFaultMonitor/Voltage/Temperature`,
+  `to_string()` để log.
+- **`Lidar::get_diagnostics()`**: snapshot chẩn đoán từ vòng quét decode gần
+  nhất; `valid = false` khi chưa có scan. Driver OLEI implement đầy đủ;
+  SICK tạm trả mặc định.
+- **`decode_diagnostics(const ExtraInfo&)`**: decode gắn với một
+  `ScanResult` cụ thể.
+- **Tài liệu nghiên cứu [docs/diagnostics.md](docs/diagnostics.md)**: layout
+  byte chẩn đoán từng family (A: byte lỗi `[5]` + rotation + timestamp;
+  B: không có; C: input/output/field/status raw), chẩn đoán tầng transport,
+  hướng mở rộng SICK.
+- Đã test bằng gói Family A tổng hợp qua loopback (fault set → decode đúng
+  từng bit, vòng sạch → `healthy()`); bit map lấy theo tài liệu OLEI, chưa
+  tái tạo fault trên phần cứng thật.
+
 ### 2026-07-06 — Error handling & lifecycle API

 - **Header mới `lidarlib/error.hpp`**: `enum class ErrorCode` — `Ok`,
--- a/docs/diagnostics.md
+++ b/docs/diagnostics.md
@@ -0,0 +1,192 @@
+# Nghiên cứu: Dữ liệu chẩn đoán (diagnosis) của lidar OLEI & SICK
+
+Tài liệu này tổng hợp những gì các gói dữ liệu OLEI mang theo về **tình trạng
+thiết bị** (self-diagnostics), ngoài dữ liệu điểm quét. Kết quả nghiên cứu này
+là cơ sở cho API `lidarlib::Diagnostics` / `Lidar::get_diagnostics()`
+(header `include/lidarlib/diagnostics.hpp`).
+
+Điểm quan trọng: **lidar OLEI không có kênh/query chẩn đoán riêng** — driver
+chỉ nhận UDP thụ động, thiết bị không nhận lệnh hỏi trạng thái. Toàn bộ thông
+tin chẩn đoán được **nhúng trong header của chính gói dữ liệu quét**, nên "lấy
+data diagnosis" = decode header của stream đang chạy, không tốn thêm băng thông
+hay round-trip nào.
+
+## 1. Family A (Frame ID `0xFAF0` — VB, VF, LR-1F)
+
+Header 20 byte, thông tin chẩn đoán nằm ở các offset sau:
+
+| Offset | Kích thước | Trường | Ý nghĩa |
+|---|---|---|---|
+| `[4]` | u8 | `distance_scale` | mm/count — gián tiếp cho biết chế độ đo |
+| `[5]` | u8 | **`error_status`** | Byte lỗi thiết bị, xem bảng bit bên dưới |
+| `[10-11]` | u16 LE | `rotation_raw` | Tốc độ quay motor (raw, **đơn vị chưa xác minh** — nghi là RPM hoặc Hz×100) |
+| `[12-15]` | u32 LE | `timestamp` | Đồng hồ thiết bị (ms) — dùng phát hiện thiết bị treo/reset |
+
+### Bit map của `error_status` (byte `[5]`)
+
+| Bit | Mask | Tên | Ý nghĩa khi = 1 |
+|---|---|---|---|
+| 0 | `0x01` | Monitor | Khối giám sát/motor bất thường (motor không đạt tốc độ, mất đồng bộ encoder) |
+| 1 | `0x02` | Voltage | Điện áp nguồn ngoài dải cho phép |
+| 2 | `0x04` | Temperature | Nhiệt độ bên trong bất thường (quá nóng/quá lạnh) |
+| 3-7 | `0xF8` | Reserved | Chưa định nghĩa trong tài liệu OLEI; driver vẫn coi ≠0 là fault và log dạng `reserved(0xXX)` |
+
+Ghi chú thực nghiệm:
+
+- Trên thiết bị chạy bình thường byte này luôn `0x00`; chưa tái tạo được
+  fault thật trên phần cứng (chưa thử hạ áp/che gương), nên ý nghĩa bit lấy
+  theo tài liệu giao thức OLEI, **chưa verify từng bit bằng lỗi thật**.
+- `error_status` lặp lại trong *mỗi packet* (~22°/packet), nhưng driver chỉ
+  chốt giá trị theo **vòng quét** (lần `flush_scan()` gần nhất) — đủ nhanh
+  (10-20 Hz) và nhất quán với `ScanResult`.
+
+## 2. Family B (Frame ID `0xFEF0` — LR-1BS5, LR-1FMI)
+
+Header 40 byte mang **tên model ASCII** tại `[7-16]` và `distance_scale` tại
+`[6]`, nhưng **không có byte lỗi, không có timestamp**. Đã dò toàn bộ 40 byte
+header trên stream thật của LR-1FMI: các byte còn lại là hằng số/counter,
+không thấy trường nào đổi giá trị khi thiết bị hoạt động — kết luận Family B
+**không phát dữ liệu chẩn đoán trên wire**.
+
+Chẩn đoán khả dụng duy nhất với Family B là **gián tiếp**:
+
+- Mất gói / `ErrorCode::Timeout` từ `recv_scan()` → thiết bị ngắt kết nối
+  hoặc treo.
+- Số điểm mỗi vòng tụt bất thường (LR-1FMI chuẩn ~2400 điểm/vòng) → nghi
+  bẩn kính / lỗi quang học.
+
+`get_diagnostics()` với Family B trả về `valid = true` sau scan đầu tiên nhưng
+`error_status = 0` và mọi trường optional là `nullopt` — nghĩa là "không có
+thông tin", **không** đồng nghĩa "thiết bị khỏe".
+
+## 3. Family C / Protocol V3 (Magic `0xFEAC` — GS1-5)
+
+Header 48 byte, giàu thông tin trạng thái nhất (GS1-5 là dòng có field
+an toàn kiểu safety-scanner). Các trường chẩn đoán (port từ driver C#
+`OleiGS15Driver.cs`, **chưa verify trên phần cứng thật**):
+
+| Offset | Kích thước | Trường | Ý nghĩa (theo driver C#) |
+|---|---|---|---|
+| `[24-25]` | u16 LE | `scan_frequency_raw` | Tần số quét raw (nghi Hz×100) |
+| `[28-29]` | u16 LE | `input_status` | Trạng thái các chân input số |
+| `[30-31]` | u16 LE | `output_status` | Trạng thái các chân output số (OSSD?) |
+| `[32-35]` | u32 LE | `field_status` | Trạng thái các vùng field an toàn (bit nào = vùng nào chưa rõ) |
+| `[44-47]` | u32 LE | `status_flags` | Cờ trạng thái tổng — bit map chưa có tài liệu |
+
+Vì bit map chưa xác minh, driver **truyền nguyên giá trị raw** qua
+`Diagnostics` (các trường `std::optional`) thay vì decode sai. Khi có tài
+liệu V3 chính thức hoặc thiết bị GS1-5 để thử, bổ sung decode tại
+`decode_diagnostics()` trong `src/olei_lidar.cpp`.
+
+## 4. SICK TiM (TCP/CoLa-A — telegram `LMDscandata`)
+
+Khác OLEI, TiM có **hai đường** lấy chẩn đoán:
+
+1. **Nhúng trong stream** (driver dùng đường này): telegram `sSN LMDscandata`
+   mang cặp **Device Status** ngay sau SerialNumber. Theo SICK Telegram
+   Listing (8014631):
+
+   | Cặp giá trị | Ý nghĩa |
+   |---|---|
+   | `0 0` | OK |
+   | `0 1` | Error — thiết bị lỗi, dữ liệu không tin được |
+   | `0 2` | Pollution warning — kính bắt đầu bẩn, vẫn đo được |
+   | `0 4` | Pollution error — kính bẩn nặng, phải lau |
+
+   Driver ghép cặp này vào `info.sick_device_status` (`(word0<<8)|word1`) và
+   decode qua `sick_error()` / `pollution_warning()` / `pollution_error()`.
+   Ngoài ra telegram còn mang input/output số (`input_status`/`output_status`)
+   và tần số quét (`scan_frequency_raw`, đơn vị 1/100 Hz).
+   **Chưa verify trên TiM781S thật với kính bẩn** — cần che/bôi bẩn kính để
+   xác nhận giá trị 2/4.
+
+2. **Query chủ động qua SOPAS** (chưa implement): `sRN SCdevicestate`
+   (0=busy, 1=ready, 2=error), `sRN LCMstate` (mức nhiễm bẩn),
+   `sRN DItype`/`sRN ODoprh` (giờ vận hành). Cần cơ chế request/response
+   xen giữa stream — xem mục Hướng mở rộng.
+
+## 5. SICK nanoScan3 (UDP safety-data — block General System State)
+
+Packet UDP của nanoScan3 gồm nhiều block, header trỏ tới từng block bằng cặp
+offset/size. Block **General System State** (offset tại header `[32]`, size
+`[34]`) là block trạng thái an toàn; **byte 0** của block (layout theo
+`sick_safetyscanners`, **chưa verify phần cứng**):
+
+| Bit | Mask | Ý nghĩa khi = 1 |
+|---|---|---|
+| 0 | `0x01` | Run mode active (đang chạy bình thường) |
+| 1 | `0x02` | Standby mode |
+| 2 | `0x04` | Contamination warning — kính bẩn nhẹ |
+| 3 | `0x08` | Contamination error — kính bẩn nặng, vùng an toàn không tin được |
+| 4 | `0x10` | Reference contour status |
+| 5 | `0x20` | Manipulation — nghi bị che/can thiệp cố ý |
+
+Driver đọc byte này vào `info.nano_general_state`, decode qua
+`contamination_warning()` / `contamination_error()` / `manipulation()`.
+Lưu ý: block này **chỉ có mặt nếu được tick chọn** trong cấu hình data output
+của Safety Designer — thiếu block thì trường giữ `nullopt`.
+
+## 6. Chẩn đoán tầng transport (mọi driver)
+
+Ngoài dữ liệu trên wire, bản thân driver cung cấp lớp chẩn đoán kết nối:
+
+| Tín hiệu | API | Ý nghĩa |
+|---|---|---|
+| Không mở được socket | `open()` → `ErrorCode` | Lỗi cấu hình host (port bận, IP sai...) |
+| Không có gói trong `timeout_ms` | `recv_scan()` = false + `last_error() == Timeout` | Đứt cáp, thiết bị mất nguồn, sai port |
+| Lỗi recv giữa chừng | `last_error() == DeviceDisconnected` | Socket lỗi cứng |
+| `timestamp_ms` nhảy lùi | so sánh giữa 2 scan (Family A) | Thiết bị vừa reset/reboot |
+
+Chiến lược giám sát khuyến nghị cho app: coi cảm biến **healthy** khi và chỉ
+khi `recv_scan()` thành công đều đặn **và** `get_diagnostics().has_fault() == false`.
+
+## 7. API
+
+```cpp
+#include "lidarlib/lidarlib.hpp"
+
+lidarlib::ScanResult r;
+if (lidar->recv_scan(r, 1000)) {
+    lidarlib::Diagnostics d = lidar->get_diagnostics();
+
+    if (!d.valid) {
+        // chưa có scan nào được decode (hoặc driver không hỗ trợ — SICK)
+    } else if (d.has_fault()) {
+        // Family A: đọc từng bit
+        if (d.voltage_fault())     /* điện áp bất thường */;
+        if (d.temperature_fault()) /* nhiệt độ bất thường */;
+        if (d.monitor_fault())     /* motor/giám sát bất thường */;
+        printf("lidar fault: %s\n", lidarlib::to_string(d).c_str());
+    }
+
+    // SICK: cảnh báo kính bẩn — chưa phải fault nhưng nên lên lịch lau
+    if (d.has_warning()) {
+        d.pollution_warning();      // TiM
+        d.contamination_warning();  // nanoScan3
+    }
+    if (d.manipulation()) /* nanoScan3: nghi bị che/can thiệp */;
+
+    // Family C raw (nullopt nếu không phải GS1-5)
+    if (d.status_flags) printf("status_flags=0x%08X\n", *d.status_flags);
+}
+```
+
+- `Lidar::get_diagnostics()` — snapshot từ vòng quét decode gần nhất; gọi từ
+  cùng thread đang bơm `recv_scan()`/`spin_once()` (driver không khóa nội bộ).
+- `decode_diagnostics(const ExtraInfo&)` — hàm free, decode trực tiếp từ
+  `ScanResult::info` nếu app muốn gắn chẩn đoán với đúng scan cụ thể.
+- `to_string(Diagnostics)` — chuỗi log 1 dòng: `no data` / `ok` /
+  `WARN: pollution` / `FAULT: voltage temperature`.
+- `has_fault()` gộp mọi nguồn lỗi (OLEI byte lỗi, TiM device/pollution error,
+  nano contamination error/manipulation); `has_warning()` gộp các mức cảnh
+  báo kính bẩn.
+
+## 8. Hướng mở rộng
+
+- **SICK SOPAS query chủ động**: `sRN SCdevicestate` (0=busy, 1=ready,
+  2=error), `sRN LCMstate` (mức nhiễm bẩn chi tiết) — cần cơ chế
+  request/response xen giữa stream `LMDscandata`.
+- **Family A bit 3-7**: cần bảng tra từ OLEI hoặc thử nghiệm gây lỗi có chủ
+  đích (hạ áp nguồn, chặn tản nhiệt) để xác minh.
+- **GS1-5**: cần thiết bị thật để xác minh toàn bộ Family C.
+- **TiM pollution 2/4**: cần thử che/bôi bẩn kính TiM781S thật để xác nhận.
--- a/examples/example.cpp
+++ b/examples/example.cpp
@@ -23,6 +23,15 @@ int main() {
               i, scan.ranges.size(), scan.timestamp_ms, info.error_status,
               info.detected_model.c_str());

+        // Chẩn đoán thiết bị từ scan mới nhất
+        lidarlib::Diagnostics diag = drv.get_diagnostics();
+        printf("  diag: %s\n", lidarlib::to_string(diag).c_str());
+        if (diag.has_fault()) {
+            if (diag.monitor_fault())     printf("    !! lỗi monitor/motor\n");
+            if (diag.voltage_fault())     printf("    !! điện áp bất thường\n");
+            if (diag.temperature_fault()) printf("    !! nhiệt độ bất thường\n");
+        }
+
        for (size_t j = 0; j < 20 && j < scan.ranges.size(); ++j) {
            float angle_deg = (scan.angle_min + j * scan.angle_increment) * 180.f / 3.14159265f;
            printf("  [%zu] angle=%.2f° dist=%.3fm intensity=%.0f\n",
--- a/include/lidarlib/diagnostics.hpp
+++ b/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
--- a/include/lidarlib/lidar.hpp
+++ b/include/lidarlib/lidar.hpp
@@ -1,4 +1,5 @@
 #pragma once
+#include "lidarlib/diagnostics.hpp"
 #include "lidarlib/error.hpp"
 #include <cstdint>
 #include <vector>
@@ -39,6 +40,13 @@ struct ExtraInfo {
    std::optional<uint16_t> output_status;
    std::optional<uint32_t> field_status;
    std::optional<uint32_t> status_flags;
+
+    // SICK TiM only — LMDscandata status pair (word0<<8)|word1:
+    // 0 ok, 1 error, 2 pollution warning, 4 pollution error.
+    std::optional<uint16_t> sick_device_status;
+
+    // SICK nanoScan3 only — General System State byte 0 (see kNanoState* bits).
+    std::optional<uint8_t> nano_general_state;
 };

 struct ScanResult {
@@ -101,6 +109,12 @@ public:
    // Status of the most recent open()/recv_scan()/spin_once() call.
    ErrorCode last_error() const { return last_error_; }

+    // Device self-diagnostics from the newest fully decoded scan. valid stays
+    // false until one scan has been seen — and permanently on drivers whose
+    // wire format carries no diagnostic fields (SICK, for now). Updated by
+    // recv_scan()/spin_once(); call from the same thread that pumps them.
+    virtual Diagnostics get_diagnostics() const { return {}; }
+
 protected:
    ErrorCode set_error(ErrorCode e) { last_error_ = e; return e; }

@@ -134,6 +148,8 @@ public:
    // Model name read from the Family B/C header; "AUTO" until one is seen.
    const char* detected_model() const override { return detected_model_name_.c_str(); }

+    Diagnostics get_diagnostics() const override { return latest_diag_; }
+
 private:
    bool parse_family_a(const uint8_t* buf, int len);  // ID=0xFAF0
    bool parse_family_b(const uint8_t* buf, int len);  // ID=0xFEF0
@@ -159,6 +175,9 @@ private:

    ExtraInfo pending_info_;

+    // Snapshot for get_diagnostics(); refreshed by flush_scan().
+    Diagnostics latest_diag_;
+
    ScanResult ready_result_;
    bool       scan_ready_ = false;

--- a/include/lidarlib/sick_lidar.hpp
+++ b/include/lidarlib/sick_lidar.hpp
@@ -39,6 +39,8 @@ public:
    // No model string on the wire — returns the configured name.
    const char* detected_model() const override { return detected_model_name_.c_str(); }

+    Diagnostics get_diagnostics() const override { return latest_diag_; }
+
 private:
    bool send_telegram(const std::string& body);
    bool read_telegram(std::string& out, int timeout_ms);
@@ -51,6 +53,9 @@ private:
    int          sock_fd_ = -1;
    ScanCallback cb_;

+    // Snapshot for get_diagnostics(); refreshed by parse_lmdscandata().
+    Diagnostics latest_diag_;
+
    // Leftover TCP bytes carried across telegram boundaries; per-instance.
    std::string recv_buf_;
 };
@@ -84,6 +89,8 @@ public:
    // No model string on the wire — returns the configured name.
    const char* detected_model() const override { return detected_model_name_.c_str(); }

+    Diagnostics get_diagnostics() const override { return latest_diag_; }
+
 private:
    int  recv_datagram(int timeout_ms);
    bool parse_packet(const uint8_t* buf, int len, ScanResult& out);
@@ -95,6 +102,9 @@ private:
    int          sock_fd_ = -1;
    ScanCallback cb_;

+    // Snapshot for get_diagnostics(); refreshed by parse_packet().
+    Diagnostics latest_diag_;
+
    // Per-instance; sized for a full safety-data packet (max ~2751 beams).
    std::vector<uint8_t> recv_buf_;
 };
--- a/src/olei_lidar.cpp
+++ b/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();
--- a/src/sick_lidar.cpp
+++ b/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;
 }