diff --git a/README.md b/README.md index 9ffdb99..f47da80 100644 --- 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`, diff --git a/docs/diagnostics.md b/docs/diagnostics.md new file mode 100644 index 0000000..46235aa --- /dev/null +++ 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. diff --git a/examples/example.cpp b/examples/example.cpp index 7ebda13..ca7af2c 100644 --- 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", diff --git a/include/lidarlib/diagnostics.hpp b/include/lidarlib/diagnostics.hpp new file mode 100644 index 0000000..84b294a --- /dev/null +++ b/include/lidarlib/diagnostics.hpp @@ -0,0 +1,108 @@ +#pragma once +#include +#include +#include +#include + +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 rotation_raw; + + // Family C / V3 (GS1-5) only — raw passthroughs, bit meanings unverified + std::optional scan_frequency_raw; + std::optional input_status; + std::optional output_status; + std::optional field_status; + std::optional status_flags; + + // SICK TiM — LMDscandata status pair (word0<<8)|word1 + std::optional 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 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 diff --git a/include/lidarlib/lidar.hpp b/include/lidarlib/lidar.hpp index 21eb16a..ea56aab 100644 --- 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 #include @@ -39,6 +40,13 @@ struct ExtraInfo { std::optional output_status; std::optional field_status; std::optional status_flags; + + // SICK TiM only — LMDscandata status pair (word0<<8)|word1: + // 0 ok, 1 error, 2 pollution warning, 4 pollution error. + std::optional sick_device_status; + + // SICK nanoScan3 only — General System State byte 0 (see kNanoState* bits). + std::optional 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; diff --git a/include/lidarlib/sick_lidar.hpp b/include/lidarlib/sick_lidar.hpp index 1f99629..dc9ca21 100644 --- 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 recv_buf_; }; diff --git a/src/olei_lidar.cpp b/src/olei_lidar.cpp index 4c639b6..ceaa512 100644 --- 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(); diff --git a/src/sick_lidar.cpp b/src/sick_lidar.cpp index 7755369..6f0a3a6 100644 --- 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(status0 & 0xFF); + info.detected_model = cfg_.name; + info.sick_device_status = static_cast(((status0 & 0xFF) << 8) | (status1 & 0xFF)); info.status_flags = (status0 << 8) | status1; info.scan_frequency_raw = static_cast(scanning_frequency); info.input_status = static_cast((in0 << 8) | in1); info.output_status = static_cast((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(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; }