diff --git a/example b/example new file mode 100755 index 0000000..94d5a08 Binary files /dev/null and b/example differ diff --git a/example.cpp b/example.cpp new file mode 100644 index 0000000..a01f857 --- /dev/null +++ b/example.cpp @@ -0,0 +1,45 @@ +// example.cpp — quick try-out of the OLEI LiDAR driver +#include "olei_lidar.hpp" +#include + +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 diff --git a/olei_lidar.cpp b/olei_lidar.cpp new file mode 100644 index 0000000..56906c3 --- /dev/null +++ b/olei_lidar.cpp @@ -0,0 +1,408 @@ +#include "olei_lidar.hpp" + +#include +#include +#include +#include +#include +#include +#include + +namespace olei { + +// ── Little-endian helpers ──────────────────────────────────────────────────── +static inline uint16_t le16(const uint8_t* p) { + return static_cast(p[0]) | (static_cast(p[1]) << 8); +} +static inline uint32_t le32(const uint8_t* p) { + return static_cast(p[0]) + | (static_cast(p[1]) << 8) + | (static_cast(p[2]) << 16) + | (static_cast(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(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(&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(&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(n)); + else if (id_at_0 == FRAME_ID_C) parse_family_c(buf, static_cast(n)); + else if (frame_id_b == FRAME_ID_B) parse_family_b(buf, static_cast(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(buf[6]); + // float ang_end = static_cast(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(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(dist_scale) : 1.f); + const float ang_end = static_cast(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(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(dist_scale) : 1.f); + if (auto_detect_ && !model_locked_) { + std::string raw(reinterpret_cast(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(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(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(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(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(inten_raw > 255 ? 255 : inten_raw) : uint8_t{0} + }); + } + + last_angle_ = raw_first_angle; + return true; +} + +} // namespace olei diff --git a/olei_lidar.hpp b/olei_lidar.hpp new file mode 100644 index 0000000..ae1ee4f --- /dev/null +++ b/olei_lidar.hpp @@ -0,0 +1,131 @@ +#pragma once +#include +#include +#include +#include + +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 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; + + // 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 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 diff --git a/test_dual b/test_dual new file mode 100755 index 0000000..710a676 Binary files /dev/null and b/test_dual differ diff --git a/test_dual.cpp b/test_dual.cpp new file mode 100644 index 0000000..fd78a4a --- /dev/null +++ b/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 +#include + +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; +}