409 lines
17 KiB
C++
409 lines
17 KiB
C++
#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
|