refactor: restructure lidarlib into xlidar-driver plugin SDK
- LidarManager facade (liblidar_manager.so): dlopen plugin discovery, available_drivers map<driver_id, PluginRegistry>, create_lidar_device, config.json load/save with legacy lidarlib migration - Common LidarDriverInterface + DriverInfo/DeviceConfig plugin ABI (extern C get_driver_info / create_driver_instance) - Plugins: driver_rplidar (ported from xlocd, Slamtec SDK), driver_olei, driver_sick_code (TiM CoLa-A), driver_sick_safety (nanoScan3), driver_espe - Diagnostics extended with rplidar health + firmware; FOV filter window, range override and legacy remap window unified in DeviceConfig - Rewritten README, diagnostics doc and examples (list_drivers, example, lidar_app) Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
This commit is contained in:
1
plugins/driver_olei/CMakeLists.txt
Normal file
1
plugins/driver_olei/CMakeLists.txt
Normal file
@@ -0,0 +1 @@
|
||||
xlidar_add_plugin(driver_olei olei_driver.cpp)
|
||||
457
plugins/driver_olei/olei_driver.cpp
Normal file
457
plugins/driver_olei/olei_driver.cpp
Normal file
@@ -0,0 +1,457 @@
|
||||
// OLEI 2D lidars over UDP — Family A (0xFAF0), Family B (0xFEF0) and
|
||||
// Family C / Protocol V3 (0xFEAC, GS1-5) packet parsing.
|
||||
#include "olei_driver.hpp"
|
||||
#include "plugin_helpers.hpp"
|
||||
|
||||
#include <cerrno>
|
||||
#include <cstring>
|
||||
#include <cmath>
|
||||
#include <arpa/inet.h>
|
||||
#include <sys/select.h>
|
||||
#include <sys/socket.h>
|
||||
#include <unistd.h>
|
||||
|
||||
namespace xlidar {
|
||||
|
||||
// Normalize into (-180, 180]: 0 = ahead, + = left, - = right.
|
||||
static inline float to_signed_deg(float deg) {
|
||||
deg = std::fmod(deg, 360.f);
|
||||
if (deg < 0.f) deg += 360.f;
|
||||
if (deg > 180.f) deg -= 360.f;
|
||||
return deg;
|
||||
}
|
||||
|
||||
static inline float maybe_invert(float signed_deg, bool inverted) {
|
||||
return inverted ? to_signed_deg(-signed_deg) : signed_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;
|
||||
}
|
||||
|
||||
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)
|
||||
|
||||
OleiDriver::OleiDriver(const ModelConfig& cfg, const std::string& ip, uint16_t port,
|
||||
bool inverted)
|
||||
: cfg_(cfg), ip_(ip), port_(port), inverted_(inverted)
|
||||
{
|
||||
auto_detect_ = (std::strcmp(cfg.name, "AUTO") == 0);
|
||||
}
|
||||
|
||||
OleiDriver::~OleiDriver() { close(); }
|
||||
|
||||
ErrorCode OleiDriver::open() {
|
||||
if (is_open()) return set_error(ErrorCode::AlreadyOpen);
|
||||
|
||||
sockaddr_in addr{};
|
||||
addr.sin_family = AF_INET;
|
||||
addr.sin_port = htons(port_);
|
||||
if (::inet_pton(AF_INET, ip_.c_str(), &addr.sin_addr) != 1)
|
||||
return set_error(ErrorCode::InvalidAddress);
|
||||
|
||||
sock_fd_ = ::socket(AF_INET, SOCK_DGRAM, 0);
|
||||
if (sock_fd_ < 0) return set_error(ErrorCode::SocketError);
|
||||
|
||||
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
|
||||
|
||||
if (::bind(sock_fd_, reinterpret_cast<sockaddr*>(&addr), sizeof(addr)) < 0) {
|
||||
int err = errno;
|
||||
::close(sock_fd_);
|
||||
sock_fd_ = -1;
|
||||
return set_error((err == EADDRINUSE || err == EACCES) ? ErrorCode::PortInUse
|
||||
: ErrorCode::BindFailed);
|
||||
}
|
||||
|
||||
// Reset per-revolution state so a close()/open() cycle starts clean.
|
||||
pending_angle_deg_.clear();
|
||||
pending_dist_m_.clear();
|
||||
pending_intensity_.clear();
|
||||
pending_info_ = ExtraInfo{};
|
||||
latest_diag_ = Diagnostics{};
|
||||
last_angle_ = -1.f;
|
||||
scan_ready_ = false;
|
||||
|
||||
pending_angle_deg_.reserve(2048);
|
||||
pending_dist_m_.reserve(2048);
|
||||
pending_intensity_.reserve(2048);
|
||||
return set_error(ErrorCode::Ok);
|
||||
}
|
||||
|
||||
void OleiDriver::close() {
|
||||
if (sock_fd_ >= 0) {
|
||||
::close(sock_fd_);
|
||||
sock_fd_ = -1;
|
||||
}
|
||||
}
|
||||
|
||||
bool OleiDriver::recv_scan(ScanResult& out, int timeout_ms) {
|
||||
if (!is_open()) { set_error(ErrorCode::NotOpen); return false; }
|
||||
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) {
|
||||
set_error(r == 0 ? ErrorCode::Timeout : ErrorCode::DeviceDisconnected);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
if (!poll_packet()) return false;
|
||||
}
|
||||
out = std::move(ready_result_);
|
||||
set_error(ErrorCode::Ok);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool OleiDriver::spin_once() {
|
||||
if (!poll_packet()) return false;
|
||||
if (scan_ready_) {
|
||||
scan_ready_ = false;
|
||||
if (cb_) cb_(ready_result_);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool OleiDriver::poll_packet() {
|
||||
if (!is_open()) { set_error(ErrorCode::NotOpen); return false; }
|
||||
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) { set_error(ErrorCode::DeviceDisconnected); return false; }
|
||||
|
||||
// A/C carry the frame id at [0-1]; B has a 0x010F preamble, real id at [2-3].
|
||||
if (n < 4) return true;
|
||||
uint16_t id_at_0 = le16(buf);
|
||||
uint16_t frame_id_b = le16(buf + 2);
|
||||
|
||||
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));
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// Append with angle-unwrapping so the ±180° seam stays a continuous ramp.
|
||||
void OleiDriver::push_point(float signed_angle_deg, float dist_m, uint8_t intensity) {
|
||||
float angle = signed_angle_deg;
|
||||
if (!pending_angle_deg_.empty()) {
|
||||
float prev = pending_angle_deg_.back();
|
||||
while (angle - prev > 180.f) angle -= 360.f;
|
||||
while (angle - prev < -180.f) angle += 360.f;
|
||||
}
|
||||
pending_angle_deg_.push_back(angle);
|
||||
pending_dist_m_.push_back(dist_m);
|
||||
pending_intensity_.push_back(intensity);
|
||||
}
|
||||
|
||||
void OleiDriver::flush_scan() {
|
||||
if (pending_angle_deg_.empty()) return;
|
||||
|
||||
const size_t n = pending_angle_deg_.size();
|
||||
|
||||
LaserScan& scan = ready_result_.scan;
|
||||
scan.timestamp_ms = pending_ts_;
|
||||
scan.angle_min = pending_angle_deg_.front() * kDeg2Rad;
|
||||
scan.angle_max = pending_angle_deg_.back() * kDeg2Rad;
|
||||
scan.angle_increment = (n > 1)
|
||||
? (scan.angle_max - scan.angle_min) / static_cast<float>(n - 1) : 0.f;
|
||||
scan.time_increment = 0.f;
|
||||
scan.scan_time = 0.f;
|
||||
scan.range_min = cfg_.range_min_m;
|
||||
scan.range_max = cfg_.range_max_m;
|
||||
scan.ranges.assign(pending_dist_m_.begin(), pending_dist_m_.end());
|
||||
scan.intensities.assign(pending_intensity_.begin(), pending_intensity_.end());
|
||||
|
||||
// Inversion already happened per point (maybe_invert), so inverted=false.
|
||||
finalize_scan(scan, cfg_, /*inverted=*/false);
|
||||
|
||||
ExtraInfo& info = ready_result_.info;
|
||||
info = pending_info_;
|
||||
info.detected_model = detected_model_name_;
|
||||
info.error_status = pending_err_;
|
||||
|
||||
latest_diag_ = decode_diagnostics(info);
|
||||
latest_diag_.device_timestamp_ms = scan.timestamp_ms;
|
||||
mark_scan_decoded();
|
||||
|
||||
pending_angle_deg_.clear();
|
||||
pending_dist_m_.clear();
|
||||
pending_intensity_.clear();
|
||||
pending_info_ = ExtraInfo{};
|
||||
scan_ready_ = true;
|
||||
}
|
||||
|
||||
// Family A (0xFAF0): 20B header + 3B blocks (u16 dist, u8 intensity).
|
||||
bool OleiDriver::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;
|
||||
|
||||
uint8_t dist_scale = buf[4]; // mm per count
|
||||
uint8_t err_status = buf[5];
|
||||
float ang_start = static_cast<float>(buf[6]);
|
||||
uint16_t num_pts = le16(buf + 8);
|
||||
uint16_t rotation_raw = le16(buf + 10);
|
||||
uint32_t timestamp = le32(buf + 12);
|
||||
uint32_t crc_packet = le32(buf + 16);
|
||||
|
||||
int block_bytes = len - HEADER_LEN;
|
||||
if (block_bytes < num_pts * BLOCK_LEN) return false;
|
||||
|
||||
uint32_t crc_calc = crc32_olei(buf + HEADER_LEN, static_cast<size_t>(num_pts * BLOCK_LEN));
|
||||
if (crc_calc != crc_packet) return false;
|
||||
|
||||
if (last_angle_ >= 0.f && ang_start < last_angle_ - 90.f) {
|
||||
flush_scan();
|
||||
}
|
||||
|
||||
pending_ts_ = timestamp;
|
||||
pending_err_ = err_status;
|
||||
pending_info_.distance_scale_mm = dist_scale;
|
||||
pending_info_.rotation_raw = rotation_raw;
|
||||
|
||||
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];
|
||||
|
||||
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) + cfg_.angle_offset_deg);
|
||||
angle = maybe_invert(angle, inverted_);
|
||||
|
||||
if (angle < cfg_.scan_angle_min || angle > cfg_.scan_angle_max) continue;
|
||||
|
||||
push_point(angle, dist_raw * scale_mm * 0.001f, intensity);
|
||||
}
|
||||
|
||||
last_angle_ = ang_start;
|
||||
return true;
|
||||
}
|
||||
|
||||
// Family B (0xFEF0): 40B header (model string at [7-16]) + 8B blocks
|
||||
// (u16 angle ×0.01°, u16 dist, u16 signal). No timestamp/error on the wire.
|
||||
bool OleiDriver::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];
|
||||
const float scale_mm = (dist_scale ? static_cast<float>(dist_scale) : 1.f);
|
||||
pending_info_.distance_scale_mm = dist_scale;
|
||||
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 },
|
||||
{ "1FMI", &MODEL_LR1FMI }, // must precede "1F": "OLELR-1FMI" also contains "1F"
|
||||
{ "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;
|
||||
cfg_.range_min_m = entry.cfg->range_min_m;
|
||||
cfg_.range_max_m = entry.cfg->range_max_m;
|
||||
cfg_.angle_offset_deg = entry.cfg->angle_offset_deg;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int num_pts = (len - HEADER_LEN) / BLOCK_LEN;
|
||||
if (num_pts <= 0) return false;
|
||||
|
||||
const uint8_t* blk = buf + HEADER_LEN;
|
||||
// A packet is only a ~22° arc and may span >1 rev, so the revolution
|
||||
// boundary is detected per point: a >90° drop between consecutive angles.
|
||||
static constexpr uint16_t INVALID_ANGLE = 0xFF00;
|
||||
for (int i = 0; i < num_pts; ++i, blk += BLOCK_LEN) {
|
||||
uint16_t angle_raw = le16(blk);
|
||||
if (angle_raw >= INVALID_ANGLE) continue;
|
||||
|
||||
float dev_deg = std::fmod(angle_raw * 0.01f, 360.f);
|
||||
if (last_angle_ >= 0.f && dev_deg < last_angle_ - 90.f) {
|
||||
flush_scan();
|
||||
}
|
||||
last_angle_ = dev_deg;
|
||||
|
||||
float angle = maybe_invert(to_signed_deg(angle_raw * 0.01f + cfg_.angle_offset_deg), inverted_);
|
||||
float dist_m = le16(blk + 2) * scale_mm * 0.001f;
|
||||
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;
|
||||
|
||||
push_point(angle, dist_m, intensity);
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// Family C / Protocol V3 (0xFEAC, GS1-5): 48B header + 2 or 4B points depending
|
||||
// on Types. Ported from the C# driver OleiGS15Driver.cs; NOT verified on real
|
||||
// hardware. Angle = (FirstIndex + i) * (360 / NumPointsScan) - 180.
|
||||
bool OleiDriver::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 distance_ratio_raw = buf[10];
|
||||
uint8_t types = buf[11];
|
||||
uint16_t scan_frequency_raw = le16(buf + 24);
|
||||
uint16_t num_pts_scan = le16(buf + 26);
|
||||
uint16_t input_status = le16(buf + 28);
|
||||
uint16_t output_status = le16(buf + 30);
|
||||
uint32_t field_status = le32(buf + 32);
|
||||
uint16_t first_index = le16(buf + 40);
|
||||
uint16_t num_pts_packet = le16(buf + 42);
|
||||
uint32_t status_flags = le32(buf + 44);
|
||||
|
||||
if (num_pts_scan == 0) return false;
|
||||
|
||||
int header_size = (header_size_field == 0) ? HEADER_LEN : header_size_field;
|
||||
if (header_size < HEADER_LEN || header_size > len) return false;
|
||||
|
||||
// Types: 0x00 = 2B/point (range only), 0x01 = 4B (range+intensity),
|
||||
// 0x10 = 4B (range at [+2,+4)).
|
||||
int bytes_per_point = (types == 0x00) ? 2 : (types == 0x01 || types == 0x10) ? 4 : 0;
|
||||
if (bytes_per_point == 0) return false;
|
||||
|
||||
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;
|
||||
|
||||
pending_info_.distance_ratio_raw = distance_ratio_raw;
|
||||
pending_info_.scan_frequency_raw = scan_frequency_raw;
|
||||
pending_info_.input_status = input_status;
|
||||
pending_info_.output_status = output_status;
|
||||
pending_info_.field_status = field_status;
|
||||
pending_info_.status_flags = status_flags;
|
||||
|
||||
// Magic 0xFEAC == exactly one model (GS1-5).
|
||||
if (auto_detect_ && !model_locked_) {
|
||||
cfg_.scan_angle_min = MODEL_GS15.scan_angle_min;
|
||||
cfg_.scan_angle_max = MODEL_GS15.scan_angle_max;
|
||||
cfg_.range_min_m = MODEL_GS15.range_min_m;
|
||||
cfg_.range_max_m = MODEL_GS15.range_max_m;
|
||||
cfg_.angle_offset_deg = MODEL_GS15.angle_offset_deg;
|
||||
detected_model_name_ = MODEL_GS15.name;
|
||||
model_locked_ = true;
|
||||
}
|
||||
|
||||
const float angle_inc = 360.f / static_cast<float>(num_pts_scan);
|
||||
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 + cfg_.angle_offset_deg);
|
||||
angle = maybe_invert(angle, inverted_);
|
||||
if (angle < cfg_.scan_angle_min || angle > cfg_.scan_angle_max) continue;
|
||||
|
||||
push_point(angle, range_mm * 0.001f,
|
||||
has_inten ? static_cast<uint8_t>(inten_raw > 255 ? 255 : inten_raw) : uint8_t{0});
|
||||
}
|
||||
|
||||
last_angle_ = raw_first_angle;
|
||||
return true;
|
||||
}
|
||||
|
||||
// ── plugin registration ─────────────────────────────────────────────────────
|
||||
|
||||
namespace {
|
||||
|
||||
const DriverInfo kDriverInfo = [] {
|
||||
DriverInfo info;
|
||||
info.vendor = "OLEI";
|
||||
info.model = "2D series (VB/VF/LR-1x/GS1-5)";
|
||||
info.driver_id = "olei_lidar_driver";
|
||||
info.description = "OLEI 2D lidars over UDP — auto-detects the Family A/B/C "
|
||||
"protocol per packet; model AUTO self-detects from the "
|
||||
"stream (Family B/C). Default port 2368.";
|
||||
info.transport = Transport::Udp;
|
||||
info.supported_models = {"AUTO", "VB", "VF", "LR-1F", "LR-1FMI", "LR-1BS5",
|
||||
"LR-16F", "GS1-5"};
|
||||
return info;
|
||||
}();
|
||||
|
||||
const ModelConfig* model_by_name(const std::string& name) {
|
||||
static constexpr const ModelConfig* kModels[] = {
|
||||
&MODEL_AUTO, &MODEL_VB, &MODEL_VF, &MODEL_LR1F, &MODEL_LR1FMI,
|
||||
&MODEL_LR1BS5, &MODEL_LR16F, &MODEL_GS15,
|
||||
};
|
||||
for (const ModelConfig* m : kModels)
|
||||
if (name == m->name) return m;
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
DriverInfo OleiDriver::get_driver_info() const { return kDriverInfo; }
|
||||
|
||||
} // namespace xlidar
|
||||
|
||||
XLIDAR_PLUGIN_EXPORT void get_driver_info(xlidar::DriverInfo* out) {
|
||||
*out = xlidar::kDriverInfo;
|
||||
}
|
||||
|
||||
XLIDAR_PLUGIN_EXPORT xlidar::LidarDriverInterface*
|
||||
create_driver_instance(const xlidar::DeviceConfig* cfg) {
|
||||
using namespace xlidar;
|
||||
const ModelConfig* preset = model_by_name(cfg->model);
|
||||
if (!preset) preset = &MODEL_AUTO; // unknown model → auto-detect
|
||||
const uint16_t port = cfg->port ? cfg->port : 2368;
|
||||
return new OleiDriver(apply_device_config(*preset, *cfg), cfg->ip, port, cfg->inverted);
|
||||
}
|
||||
89
plugins/driver_olei/olei_driver.hpp
Normal file
89
plugins/driver_olei/olei_driver.hpp
Normal file
@@ -0,0 +1,89 @@
|
||||
// OLEI 2D lidars over UDP (Family A / B / C protocols) — plugin-private header.
|
||||
#pragma once
|
||||
#include "lidar_interface.hpp"
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
namespace xlidar {
|
||||
|
||||
// Model presets. Family B/C packets carry enough to auto-detect the model;
|
||||
// Family A doesn't, so MODEL_AUTO keeps the wide default FOV.
|
||||
inline constexpr ModelConfig MODEL_VB { "VB", -135.f, 135.f, 0.05f, 30.f }; // 2D 270°
|
||||
inline constexpr ModelConfig MODEL_VF { "VF", -180.f, 180.f, 0.05f, 30.f }; // 2D 360°
|
||||
inline constexpr ModelConfig MODEL_LR1F { "LR-1F", -180.f, 180.f, 0.05f, 50.f, 180.f }; // 2D 360° 50m; device 0° = rear
|
||||
inline constexpr ModelConfig MODEL_LR1FMI { "LR-1FMI", -180.f, 180.f, 0.05f, 30.f, 180.f }; // 2D 360° (Family B); device 0° = rear
|
||||
inline constexpr ModelConfig MODEL_LR1BS5 { "LR-1BS5", -180.f, 180.f, 0.05f, 30.f }; // 2D 360° (Family B)
|
||||
inline constexpr ModelConfig MODEL_LR16F { "LR-16F", -135.f, 135.f, 0.05f, 30.f }; // 3D 16 line
|
||||
inline constexpr ModelConfig MODEL_GS15 { "GS1-5", -180.f, 180.f, 0.05f, 30.f }; // 2D 360° (Family C/V3)
|
||||
inline constexpr ModelConfig MODEL_AUTO { "AUTO", -180.f, 180.f, 0.05f, 30.f };
|
||||
|
||||
// OLEI UDP driver.
|
||||
class OleiDriver : public LidarDriverInterface {
|
||||
public:
|
||||
// ip: local bind address; port: UDP port the lidar sends to;
|
||||
// inverted: unit mounted upside-down → mirror every angle.
|
||||
explicit OleiDriver(const ModelConfig& cfg,
|
||||
const std::string& ip = "0.0.0.0",
|
||||
uint16_t port = 2368,
|
||||
bool inverted = false);
|
||||
~OleiDriver();
|
||||
|
||||
OleiDriver(const OleiDriver&) = delete;
|
||||
OleiDriver& operator=(const OleiDriver&) = delete;
|
||||
|
||||
DriverInfo get_driver_info() const override;
|
||||
|
||||
ErrorCode open() override;
|
||||
void close() override;
|
||||
bool recv_scan(ScanResult& out, int timeout_ms = 1000) override;
|
||||
void set_scan_callback(ScanCallback cb) override { cb_ = std::move(cb); }
|
||||
bool spin_once() override;
|
||||
bool is_open() const override { return sock_fd_ >= 0; }
|
||||
|
||||
// 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 poll_packet(); // one recvfrom() + dispatch to the family parser
|
||||
bool parse_family_a(const uint8_t* buf, int len); // ID=0xFAF0
|
||||
bool parse_family_b(const uint8_t* buf, int len); // ID=0xFEF0
|
||||
bool parse_family_c(const uint8_t* buf, int len); // Magic=0xFEAC (GS1-5)
|
||||
|
||||
void push_point(float signed_angle_deg, float dist_m, uint8_t intensity);
|
||||
void flush_scan();
|
||||
|
||||
ModelConfig cfg_;
|
||||
std::string ip_;
|
||||
uint16_t port_;
|
||||
bool inverted_ = false;
|
||||
int sock_fd_ = -1;
|
||||
ScanCallback cb_;
|
||||
|
||||
// Per-revolution accumulation buffers (index-aligned)
|
||||
std::vector<float> pending_angle_deg_;
|
||||
std::vector<float> pending_dist_m_;
|
||||
std::vector<uint8_t> pending_intensity_;
|
||||
uint32_t pending_ts_ = 0;
|
||||
uint8_t pending_err_ = 0;
|
||||
float last_angle_ = -1.f; // wrap detection, device space [0,360)
|
||||
|
||||
ExtraInfo pending_info_;
|
||||
|
||||
// Snapshot for get_diagnostics(); refreshed by flush_scan().
|
||||
Diagnostics latest_diag_;
|
||||
|
||||
ScanResult ready_result_;
|
||||
bool scan_ready_ = false;
|
||||
|
||||
// Per-instance so two drivers on two threads don't race.
|
||||
uint8_t recv_buf_[4096];
|
||||
|
||||
bool auto_detect_ = false;
|
||||
bool model_locked_ = false;
|
||||
std::string detected_model_name_ = "AUTO";
|
||||
};
|
||||
|
||||
} // namespace xlidar
|
||||
Reference in New Issue
Block a user