init

example.cpp

@@ -0,0 +1,45 @@
+// example.cpp — quick try-out of the OLEI LiDAR driver
+#include "olei_lidar.hpp"
+#include <cstdio>
+
+int main() {
+    // ── pick a model ────────────────────────────────────────────────────────
+    // olei::Driver drv(olei::MODEL_VF);        // 2D 360°
+    // olei::Driver drv(olei::MODEL_LR1F);      // 2D 360°, 50m
+    olei::Driver drv(olei::MODEL_VB);           // 2D 270°
+
+    if (!drv.open()) {
+        fprintf(stderr, "Không mở được socket\n");
+        return 1;
+    }
+
+    // ── option 1: blocking recv ─────────────────────────────────────────────
+    for (int i = 0; i < 10; ++i) {
+        olei::Scan scan;
+        if (!drv.recv_scan(scan, 2000)) {
+            fprintf(stderr, "Timeout hoặc lỗi nhận packet\n");
+            break;
+        }
+        printf("Scan #%d: %zu điểm, ts=%u ms, err=0x%02X\n",
+               i, scan.points.size(), scan.timestamp_ms, scan.error_status);
+
+        // Print the first few points
+        for (size_t j = 0; j < 5 && j < scan.points.size(); ++j) {
+            const auto& p = scan.points[j];
+            printf("  [%zu] angle=%.2f° dist=%.3fm intensity=%u\n",
+                   j, p.angle_deg, p.distance_m, p.intensity);
+        }
+    }
+
+    // ── option 2: callback (your own loop) ──────────────────────────────────
+    // drv.set_scan_callback([](const olei::Scan& scan) {
+    //     printf("Got scan: %zu pts\n", scan.points.size());
+    // });
+    // while (true) drv.spin_once();
+
+    drv.close();
+    return 0;
+}
+
+// Build:
+//   g++ -std=c++17 -O2 -o example example.cpp olei_lidar.cpp

olei_lidar.cpp

@@ -0,0 +1,408 @@
+#include "olei_lidar.hpp"
+
+#include <cstring>
+#include <cmath>
+#include <stdexcept>
+#include <sys/socket.h>
+#include <arpa/inet.h>
+#include <unistd.h>
+#include <sys/select.h>
+
+namespace olei {
+
+// ── Little-endian helpers ────────────────────────────────────────────────────
+static inline uint16_t le16(const uint8_t* p) {
+    return static_cast<uint16_t>(p[0]) | (static_cast<uint16_t>(p[1]) << 8);
+}
+static inline uint32_t le32(const uint8_t* p) {
+    return static_cast<uint32_t>(p[0])
+         | (static_cast<uint32_t>(p[1]) <<  8)
+         | (static_cast<uint32_t>(p[2]) << 16)
+         | (static_cast<uint32_t>(p[3]) << 24);
+}
+
+// Normalize any angle into the SIGNED system (-180, 180]: 0 = straight ahead,
+// + = left, - = right. This lets a model's FOV (e.g. VB -135…135) correctly
+// filter lidars that report angles in 0–360 too.
+static inline float to_signed_deg(float deg) {
+    deg = std::fmod(deg, 360.f);
+    if (deg < 0.f)    deg += 360.f;   // → [0,360)
+    if (deg > 180.f)  deg -= 360.f;   // → (-180,180]
+    return deg;
+}
+
+// ── CRC32 (poly 0x04C11DB7, MSB-first) ──────────────────────────────────────
+static uint32_t crc32_olei(const uint8_t* data, size_t len) {
+    uint32_t crc = 0xFFFFFFFF;
+    for (size_t i = 0; i < len; ++i) {
+        crc ^= static_cast<uint32_t>(data[i]) << 24;
+        for (int b = 0; b < 8; ++b)
+            crc = (crc & 0x80000000u) ? (crc << 1) ^ 0x04C11DB7u : (crc << 1);
+    }
+    return crc;
+}
+
+// ── Frame IDs ────────────────────────────────────────────────────────────────
+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)
+
+// ─── Constructor / Destructor ────────────────────────────────────────────────
+Driver::Driver(const ModelConfig& cfg, const std::string& ip, uint16_t port)
+    : cfg_(cfg), ip_(ip), port_(port)
+{
+    auto_detect_ = (std::strcmp(cfg.name, "AUTO") == 0);
+}
+
+Driver::~Driver() { close(); }
+
+// ─── open() ─────────────────────────────────────────────────────────────────
+bool Driver::open() {
+    sock_fd_ = ::socket(AF_INET, SOCK_DGRAM, 0);
+    if (sock_fd_ < 0) return false;
+
+    // Allow multiple sockets to bind the same port (run alongside another
+    // app / debugging). SO_REUSEPORT lets several listeners receive the same
+    // UDP stream — only works if EVERY socket on that port sets this flag.
+    int reuse = 1;
+    ::setsockopt(sock_fd_, SOL_SOCKET, SO_REUSEADDR, &reuse, sizeof(reuse));
+#ifdef SO_REUSEPORT
+    ::setsockopt(sock_fd_, SOL_SOCKET, SO_REUSEPORT, &reuse, sizeof(reuse));
+#endif
+
+    sockaddr_in addr{};
+    addr.sin_family      = AF_INET;
+    addr.sin_port        = htons(port_);
+    addr.sin_addr.s_addr = inet_addr(ip_.c_str());
+
+    if (::bind(sock_fd_, reinterpret_cast<sockaddr*>(&addr), sizeof(addr)) < 0) {
+        ::close(sock_fd_);
+        sock_fd_ = -1;
+        return false;
+    }
+    pending_.reserve(2048);
+    return true;
+}
+
+// ─── close() ────────────────────────────────────────────────────────────────
+void Driver::close() {
+    if (sock_fd_ >= 0) {
+        ::close(sock_fd_);
+        sock_fd_ = -1;
+    }
+}
+
+// ─── recv_scan() — blocks until one full revolution is available ──────────
+bool Driver::recv_scan(Scan& out, int timeout_ms) {
+    scan_ready_ = false;
+
+    while (!scan_ready_) {
+        if (timeout_ms > 0) {
+            fd_set fds; FD_ZERO(&fds); FD_SET(sock_fd_, &fds);
+            timeval tv{ timeout_ms / 1000, (timeout_ms % 1000) * 1000 };
+            int r = ::select(sock_fd_ + 1, &fds, nullptr, nullptr, &tv);
+            if (r <= 0) return false;  // timeout or error
+        }
+        if (!spin_once()) return false;
+    }
+    out = std::move(ready_scan_);
+    return true;
+}
+
+// ─── spin_once() ────────────────────────────────────────────────────────────
+bool Driver::spin_once() {
+    // buf is the recv_buf_ member, NOT static → each Driver has its own
+    // memory, safe when 2 lidars receive concurrently on 2 threads.
+    uint8_t* buf = recv_buf_;
+    sockaddr_in from{};
+    socklen_t   fromlen = sizeof(from);
+
+    ssize_t n = ::recvfrom(sock_fd_, buf, sizeof(recv_buf_), 0,
+                            reinterpret_cast<sockaddr*>(&from), &fromlen);
+    if (n < 0) return false;
+
+    // Distinguish protocol family by Frame ID (little-endian)
+    // Family A / C: Frame ID / magic sits right at bytes [0-1]
+    // Family B: has a 0x010F preamble at bytes [0-1], real Frame ID at bytes [2-3]
+    if (n < 4) return true;  // too short, skip
+    uint16_t id_at_0 = le16(buf);        // Family A (0xFAF0) or Family C (0xFEAC)
+    uint16_t frame_id_b = le16(buf + 2); // Family B: preamble 0x010F + real id at [2-3]
+
+    if      (id_at_0 == FRAME_ID_A)    parse_family_a(buf, static_cast<int>(n));
+    else if (id_at_0 == FRAME_ID_C)    parse_family_c(buf, static_cast<int>(n));
+    else if (frame_id_b == FRAME_ID_B) parse_family_b(buf, static_cast<int>(n));
+    // else: unknown family (3D LR-16F uses a different format, extend later)
+
+    return true;
+}
+
+// ─── flush_scan() — a revolution is complete ───────────────────────────────
+void Driver::flush_scan() {
+    if (pending_.empty()) return;
+
+    ready_scan_.points      = std::move(pending_);
+    ready_scan_.timestamp_ms = pending_ts_;
+    ready_scan_.error_status = pending_err_;
+    pending_.clear();
+    scan_ready_ = true;
+
+    if (cb_) cb_(ready_scan_);
+}
+
+// ─── parse_family_a() ───────────────────────────────────────────────────────
+// 20-byte header:
+//   [0-1]  Frame ID   = 0xFAF0
+//   [2-3]  Protocol   = 0x0200
+//   [4]    Distance scale (mm/count)
+//   [5]    Error status
+//   [6]    Start angle (deg, uint8)
+//   [7]    End angle   (deg, uint8, exclusive)
+//   [8-9]  Num points  (uint16 LE)
+//   [10-11] Rotation info
+//   [12-15] Timestamp  (uint32 LE, ms)
+//   [16-19] CRC32 of the block data
+// 3-byte block × N:
+//   [0-1]  Distance readout (uint16 LE)
+//   [2]    Intensity (uint8)
+bool Driver::parse_family_a(const uint8_t* buf, int len) {
+    static constexpr int HEADER_LEN = 20;
+    static constexpr int BLOCK_LEN  =  3;
+
+    if (len < HEADER_LEN) return false;
+
+    // ── read header ──
+    // uint16_t protocol  = le16(buf + 2);   // 0x0200
+    uint8_t  dist_scale = buf[4];           // mm per count
+    uint8_t  err_status = buf[5];
+    float    ang_start  = static_cast<float>(buf[6]);
+    // float ang_end    = static_cast<float>(buf[7]);  // exclusive
+    uint16_t num_pts    = le16(buf + 8);
+    uint32_t timestamp  = le32(buf + 12);
+    uint32_t crc_packet = le32(buf + 16);
+
+    // ── verify CRC (optional but recommended) ──
+    int block_bytes = len - HEADER_LEN;
+    if (block_bytes < num_pts * BLOCK_LEN) return false;  // truncated packet
+
+    uint32_t crc_calc = crc32_olei(buf + HEADER_LEN, static_cast<size_t>(num_pts * BLOCK_LEN));
+    if (crc_calc != crc_packet) return false;  // CRC mismatch
+
+    // ── detect wrap-around → flush the previous revolution ──
+    if (last_angle_ >= 0.f && ang_start < last_angle_ - 90.f) {
+        flush_scan();
+    }
+
+    // ── decode points ──
+    pending_ts_  = timestamp;
+    pending_err_ = err_status;
+
+    // scale=0 means the firmware didn't report it → default to 1 mm/count to avoid dist=0.
+    const float scale_mm = (dist_scale ? static_cast<float>(dist_scale) : 1.f);
+    const float ang_end  = static_cast<float>(buf[7]);
+
+    const uint8_t* blk = buf + HEADER_LEN;
+    for (uint16_t i = 0; i < num_pts; ++i, blk += BLOCK_LEN) {
+        uint16_t dist_raw = le16(blk);
+        uint8_t  intensity = blk[2];
+
+        // Compute angle: linear interpolation within the packet's range (device-space)
+        float frac  = (num_pts > 1) ? static_cast<float>(i) / (num_pts - 1) : 0.f;
+        float angle = to_signed_deg(ang_start + frac * (ang_end - ang_start));
+
+        // Filter out anything outside the model's FOV (already in the signed -180…180 system)
+        if (angle < cfg_.scan_angle_min || angle > cfg_.scan_angle_max) continue;
+
+        pending_.push_back(Point{
+            angle,
+            dist_raw * scale_mm * 0.001f,  // mm → m
+            intensity
+        });
+    }
+
+    last_angle_ = ang_start;
+    return true;
+}
+
+// ─── parse_family_b() ───────────────────────────────────────────────────────
+// 40-byte header:
+//   [0-1]  0x010F
+//   [2-3]  0xFEF0  (Frame ID)
+//   [4-5]  0x0200  (Protocol)
+//   [6]    Distance scale
+//   [7-16] Model identifier string (e.g. "OLELR-1BS5")
+//   [17-39] Reserved
+// 8-byte block × N:
+//   [0-1]  Angle (uint16 LE, × 0.25° → deg, 0–360)
+//   [2-3]  Distance mm (uint16 LE)
+//   [4-5]  Signal strength (uint16 LE)
+//   [6-7]  Unused (0x0000)
+bool Driver::parse_family_b(const uint8_t* buf, int len) {
+    static constexpr int HEADER_LEN = 40;
+    static constexpr int BLOCK_LEN  =  8;
+
+    if (len < HEADER_LEN) return false;
+
+    uint8_t dist_scale = buf[6];
+    // scale=0 → default to 1 mm/count so distances don't collapse to zero.
+    const float scale_mm = (dist_scale ? static_cast<float>(dist_scale) : 1.f);
+    if (auto_detect_ && !model_locked_) {
+        std::string raw(reinterpret_cast<const char*>(buf + 7), 10);
+        size_t z = raw.find('\0');
+        if (z != std::string::npos) raw.resize(z);
+
+        if (!raw.empty()) {
+            detected_model_name_ = raw;
+            model_locked_        = true;
+
+            static constexpr struct { const char* key; const ModelConfig* cfg; } kModelTable[] = {
+                { "1BS5", &MODEL_LR1BS5 },
+                { "16F",  &MODEL_LR16F  },
+                { "1F",   &MODEL_LR1F   },
+                { "VF",   &MODEL_VF     },
+                { "VB",   &MODEL_VB     },
+            };
+            for (const auto& entry : kModelTable) {
+                if (raw.find(entry.key) != std::string::npos) {
+                    cfg_.scan_angle_min = entry.cfg->scan_angle_min;
+                    cfg_.scan_angle_max = entry.cfg->scan_angle_max;
+                    break;
+                }
+            }
+        }
+    }
+
+    int num_pts = (len - HEADER_LEN) / BLOCK_LEN;
+    if (num_pts <= 0) return false;
+
+    const uint8_t* blk = buf + HEADER_LEN;
+    // The device's angle counter runs continuously across revolutions
+    // (no per-revolution reset) → mod 360 is needed to get the real angle
+    // in device-space [0, 360). Wrap-around is detected on the [0,360) space
+    // (monotonically increasing, then resets), NOT on the signed space, since
+    // the signed space jumps by ±360 right in front of the device.
+    float first_angle = std::fmod(le16(blk) * 0.25f, 360.f);
+
+    // ── detect wrap-around ──
+    if (last_angle_ >= 0.f && first_angle < last_angle_ - 90.f) {
+        flush_scan();
+    }
+
+    for (int i = 0; i < num_pts; ++i, blk += BLOCK_LEN) {
+        float    angle     = to_signed_deg(le16(blk) * 0.25f);          // -180…180
+        float    dist_m    = le16(blk + 2) * scale_mm * 0.001f;         // mm → m
+        uint8_t  intensity = static_cast<uint8_t>(le16(blk + 4) >> 2);  // 10-bit → 8-bit
+
+        if (angle < cfg_.scan_angle_min || angle > cfg_.scan_angle_max) continue;
+
+        pending_.push_back(Point{ angle, dist_m, intensity });
+    }
+
+    last_angle_ = first_angle;
+    return true;
+}
+
+// ─── parse_family_c() ───────────────────────────────────────────────────────
+// Protocol V3 (Olei GS1-5, magic 0xFEAC) — ported from the existing C#
+// production driver OleiGS15Driver.cs (RobotNet10.RobotApp); NOT independently
+// sniffed/verified against real GS1-5 hardware (no device was available to
+// test this while writing the code).
+// 48-byte header:
+//   [0-1]   Magic        = 0xFEAC
+//   [2-3]   Version
+//   [4-7]   PacketSize   (uint32 LE)
+//   [8-9]   HeaderSize   (uint16 LE, usually = 48)
+//   [10]    Distance ratio — read by the original C# driver but NOT applied
+//           (distance is always raw mm / 1000); same behavior kept here.
+//   [11]    Types: 0x00=2B/point (range only), 0x01=4B/point (range+intensity),
+//           0x10=4B/point (first 2 bytes unused, range at [+2,+4))
+//   [12-13] Scan number    [14-15] Packet number
+//   [16-19] Timestamp decimal   [20-23] Timestamp integer
+//   [24-25] Scan frequency raw  [26-27] NumPointsScan (total points per revolution)
+//   [28-29] Input status        [30-31] Output status
+//   [32-35] Field status
+//   [36-37] StartIndex   [38-39] EndIndex
+//   [40-41] FirstIndex — index of this packet's first point within the full revolution
+//   [42-43] NumPointsPacket — number of points in this packet
+//   [44-47] Status flags
+// Angle: angle = (FirstIndex + i) * (360 / NumPointsScan) - 180 → already in
+// the signed system (-180..180); no fmod needed like Family B since the
+// index always stays within [0, NumPointsScan).
+bool Driver::parse_family_c(const uint8_t* buf, int len) {
+    static constexpr int HEADER_LEN = 48;
+    if (len < HEADER_LEN) return false;
+
+    uint16_t header_size_field = le16(buf + 8);
+    uint8_t  types             = buf[11];
+    uint16_t num_pts_scan      = le16(buf + 26);
+    uint16_t first_index       = le16(buf + 40);
+    uint16_t num_pts_packet    = le16(buf + 42);
+
+    if (num_pts_scan == 0) return false;  // avoid divide-by-zero
+
+    int header_size = (header_size_field == 0) ? HEADER_LEN : header_size_field;
+    if (header_size < HEADER_LEN || header_size > len) return false;
+
+    int bytes_per_point = (types == 0x00) ? 2 : (types == 0x01 || types == 0x10) ? 4 : 0;
+    if (bytes_per_point == 0) return false;  // unknown Types, layout unclear
+
+    int payload_bytes = len - header_size;
+    int num_pts        = num_pts_packet;
+    if (num_pts == 0 || num_pts * bytes_per_point > payload_bytes) {
+        num_pts = payload_bytes / bytes_per_point;
+    }
+    if (num_pts <= 0) return false;
+
+    // Magic 0xFEAC corresponds to exactly one model (GS1-5) — no model name
+    // string in the header like Family B, but recognizing this family is
+    // already enough to know the model, so auto-detect resolves immediately
+    // without reading any extra field.
+    if (auto_detect_ && !model_locked_) {
+        cfg_.scan_angle_min   = MODEL_GS15.scan_angle_min;
+        cfg_.scan_angle_max   = MODEL_GS15.scan_angle_max;
+        detected_model_name_  = MODEL_GS15.name;
+        model_locked_         = true;
+    }
+
+    const float angle_inc = 360.f / static_cast<float>(num_pts_scan);
+    // raw_angle is used for wrap-around detection: it does NOT have the -180
+    // offset that the externally-exposed angle gets, and stays in [0,360),
+    // monotonically increasing — matching the same convention used by
+    // Family A/B (last_angle_ >= 0 means "we already have a previous value");
+    // subtracting 180 here could go negative and break that sentinel check.
+    float raw_first_angle = static_cast<float>(first_index) * angle_inc;
+
+    if (last_angle_ >= 0.f && raw_first_angle < last_angle_ - 90.f) {
+        flush_scan();
+    }
+
+    const uint8_t* blk = buf + header_size;
+    for (int i = 0; i < num_pts; ++i, blk += bytes_per_point) {
+        uint16_t range_mm;
+        uint16_t inten_raw = 0;
+        bool     has_inten = false;
+
+        if (types == 0x00) {
+            range_mm = le16(blk);
+        } else if (types == 0x01) {
+            range_mm  = le16(blk);
+            inten_raw = le16(blk + 2);
+            has_inten = true;
+        } else {  // 0x10
+            range_mm = le16(blk + 2);
+        }
+
+        float angle = to_signed_deg(static_cast<float>(first_index + i) * angle_inc - 180.f);
+        if (angle < cfg_.scan_angle_min || angle > cfg_.scan_angle_max) continue;
+
+        pending_.push_back(Point{
+            angle,
+            range_mm * 0.001f,  // mm → m
+            has_inten ? static_cast<uint8_t>(inten_raw > 255 ? 255 : inten_raw) : uint8_t{0}
+        });
+    }
+
+    last_angle_ = raw_first_angle;
+    return true;
+}
+
+} // namespace olei

olei_lidar.hpp

@@ -0,0 +1,131 @@
+#pragma once
+#include <cstdint>
+#include <vector>
+#include <string>
+#include <functional>
+
+namespace olei {
+
+// ─── A single measured point ───────────────────────────────────────────────
+struct Point {
+    float angle_deg;      // -180…180, signed; + = left, 0 = straight ahead
+    float distance_m;     // meters
+    uint8_t intensity;    // 0–255
+};
+
+// ─── One complete revolution ───────────────────────────────────────────────
+struct Scan {
+    std::vector<Point> points;
+    uint32_t timestamp_ms;  // ms since power-on
+    uint8_t  error_status;  // 0 = OK; BIT0=Monitor, BIT1=Voltage, BIT2=Temp
+};
+
+// ─── Per-model configuration ───────────────────────────────────────────────
+// scan_angle_* use the SIGNED system [-180,180]: 0 = straight ahead, + = left, - = right.
+// 360° lidars keep the full circle [-180,180]; narrow-FOV lidars (VB 270°) shrink it.
+struct ModelConfig {
+    const char* name;
+    float       scan_angle_min;   // deg — VB/LR-16F: -135, 360° models: -180
+    float       scan_angle_max;   // deg — VB/LR-16F:  135, 360° models:  180
+    // Remaining fields are read from the packet header (distance_scale, rotation_rate…)
+};
+
+// Table of known models — the driver auto-detects the packet family (A=0xFAF0 /
+// B=0xFEF0 / C=0xFEAC) per packet, so this config mainly decides the angular
+// window (FOV) that gets kept.
+inline constexpr ModelConfig MODEL_VB     { "VB",      -135.f, 135.f };  // 2D 270°
+inline constexpr ModelConfig MODEL_VF     { "VF",      -180.f, 180.f };  // 2D 360°
+inline constexpr ModelConfig MODEL_LR1F   { "LR-1F",   -180.f, 180.f };  // 2D 360° 50m
+inline constexpr ModelConfig MODEL_LR1BS5 { "LR-1BS5", -180.f, 180.f };  // 2D 360° (Family B)
+inline constexpr ModelConfig MODEL_LR16F  { "LR-16F",  -135.f, 135.f };  // 3D 16 line
+inline constexpr ModelConfig MODEL_GS15   { "GS1-5",   -180.f, 180.f };  // 2D 360°
+
+// Sentinel: model unknown ahead of time. Family B (0xFEF0) carries an ASCII
+// model name string in its header (e.g. "OLELR-1BS5", verified via live UDP
+// sniff) → the driver auto-detects it and narrows the FOV per the table
+// above. Family C (0xFEAC, GS1-5) is identified by magic alone. Family A has
+// no such string, so on a Family-A device MODEL_AUTO keeps the wide default
+// FOV (-180..180, no points dropped) until the user specifies a concrete model.
+inline constexpr ModelConfig MODEL_AUTO   { "AUTO",    -180.f, 180.f };
+
+// ─── Driver ─────────────────────────────────────────────────────────────────
+class Driver {
+public:
+    // callback invoked whenever a complete scan is ready
+    using ScanCallback = std::function<void(const Scan&)>;
+
+    // ip   : receiving host's bind address, usually "0.0.0.0"
+    // port : UDP port the lidar sends to (default 2368)
+    // cfg  : model config
+    explicit Driver(const ModelConfig& cfg,
+                    const std::string& ip   = "0.0.0.0",
+                    uint16_t           port = 2368);
+    ~Driver();
+
+    // Non-copyable
+    Driver(const Driver&)            = delete;
+    Driver& operator=(const Driver&) = delete;
+
+    // Open the socket and start receiving
+    bool open();
+
+    // Close the socket
+    void close();
+
+    // Blocks until a full revolution has been received; returns false on error/timeout
+    // timeout_ms = 0 → block indefinitely
+    bool recv_scan(Scan& out, int timeout_ms = 1000);
+
+    // Or use the callback (drive it from your own non-blocking loop)
+    void set_scan_callback(ScanCallback cb) { cb_ = std::move(cb); }
+
+    // Receive + dispatch callback (call from your own loop)
+    bool spin_once();
+
+    // The REAL model name read from the Family B/C header (only meaningful
+    // when the Driver was constructed with MODEL_AUTO). Always the actual
+    // string found in the packet (e.g. "OLELR-1BS2"), even when that model
+    // has no specific FOV entry in the table (FOV then stays at the 360°
+    // default). Returns "AUTO" if no Family B/C packet has been seen yet.
+    const char* detected_model() const { return detected_model_name_.c_str(); }
+
+private:
+    // ── parse Family A packet (ID=0xFAF0): 20B header, 3B block ──
+    bool parse_family_a(const uint8_t* buf, int len);
+
+    // ── parse Family B packet (ID=0xFEF0): 40B header, 8B block ──
+    bool parse_family_b(const uint8_t* buf, int len);
+
+    // ── parse Family C / protocol V3 packet (Magic=0xFEAC, GS1-5): 48B header ──
+    bool parse_family_c(const uint8_t* buf, int len);
+
+    // Once a full revolution is ready → flush into ready_scan_ and fire the callback
+    void flush_scan();
+
+    ModelConfig  cfg_;
+    std::string  ip_;
+    uint16_t     port_;
+    int          sock_fd_   = -1;
+    ScanCallback cb_;
+
+    // Buffer accumulating points for the scan currently in progress
+    std::vector<Point> pending_;
+    uint32_t           pending_ts_   = 0;
+    uint8_t            pending_err_  = 0;
+    float              last_angle_   = -1.f;  // wrap-around detection
+
+    // recv_scan()'s output, gated by a simple ready flag
+    Scan     ready_scan_;
+    bool     scan_ready_ = false;
+
+    // Per-instance receive buffer — NOT static, so that 2 lidars running on 2
+    // threads don't overwrite each other's data (data race).
+    uint8_t  recv_buf_[4096];
+
+    // Model auto-detection from the Family B/C header (see MODEL_AUTO)
+    bool        auto_detect_        = false;
+    bool        model_locked_       = false;
+    std::string detected_model_name_ = "AUTO";
+};
+
+} // namespace olei

test_dual.cpp

@@ -0,0 +1,53 @@
+// test_dual.cpp — test 2 Olei lidars (front + rear) concurrently, per appsettings.json
+//   Olei-front: scan_1, DeviceIp 192.168.100.11, LocalIp 192.168.100.100, DevicePort 2368
+//   Olei-rear : scan_2, DeviceIp 192.168.100.12, LocalIp 192.168.100.100, DevicePort 2369
+#include "olei_lidar.hpp"
+#include <cstdio>
+#include <thread>
+
+static void run_lidar(const char* tag, const olei::ModelConfig& cfg,
+                       const std::string& local_ip, uint16_t port, int n_scans) {
+    olei::Driver drv(cfg, local_ip, port);
+    if (!drv.open()) {
+        fprintf(stderr, "[%s] Khong mo duoc socket tren %s:%u (interface khong ton tai?)\n",
+                tag, local_ip.c_str(), port);
+        return;
+    }
+    printf("[%s] Da bind %s:%u, dang doi scan...\n", tag, local_ip.c_str(), port);
+
+    for (int i = 0; i < n_scans; ++i) {
+        olei::Scan scan;
+        if (!drv.recv_scan(scan, 2000)) {
+            fprintf(stderr, "[%s] Timeout/loi nhan packet (scan #%d)\n", tag, i);
+            continue;
+        }
+        printf("[%s] Scan #%d: %zu diem, ts=%u ms, err=0x%02X, model=%s\n",
+               tag, i, scan.points.size(), scan.timestamp_ms, scan.error_status,
+               drv.detected_model());
+        for (size_t j = 0; j < 3 && j < scan.points.size(); ++j) {
+            const auto& p = scan.points[j];
+            printf("       [%zu] angle=%.2f dist=%.3fm intensity=%u\n",
+                   j, p.angle_deg, p.distance_m, p.intensity);
+        }
+    }
+    drv.close();
+}
+
+int main() {
+    // Both front and rear are Family B in practice — front's real header
+    // string is "OLELR-1BS2", rear's is "OLELR-1BS5" (verified via live UDP
+    // sniff), NOT the VB (Family A) model the config name suggested. With
+    // MODEL_AUTO, the driver reads the real model name from the header and
+    // narrows the FOV when it matches a known entry in kModelTable
+    // (olei_lidar.cpp); "1BS5" matches (→ full 360°), but "1BS2" doesn't, so
+    // front currently stays at the unfiltered 360° default. Call
+    // drv.detected_model() to see which name was actually read.
+    std::thread t_front(run_lidar, "front/scan_1", olei::MODEL_AUTO,
+                         "192.168.100.100", 2368, 5);
+    std::thread t_rear(run_lidar, "rear/scan_2", olei::MODEL_AUTO,
+                        "192.168.100.100", 2369, 5);
+
+    t_front.join();
+    t_rear.join();
+    return 0;
+}