init

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