132 lines
5.8 KiB
C++
132 lines
5.8 KiB
C++
#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
|