QUYVN/DriverLIdar
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix: driver LGA60

Browse Source
This commit is contained in:
Trương Văn Lộc
2026-08-05 07:58:17 +07:00
parent 145a647d35
commit 2793b33845
3 changed files with 164 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
README.md
View File

@@ -264,6 +264,16 @@ không handshake CoLa2/TCP.
−180°), preset −160…160°, range 0.05…50 m. Frame đo `HISN` (header 16 B
big-endian; điểm = distance mm + intensity, distance 50 000 mm = không có
phản hồi → ∞); frame vùng `WSimu` (nếu thiết bị gửi) được đọc lấy từ lỗi.
Mỗi lượt quét được thiết bị chia thành NHIỀU packet `HISN`: các bộ đếm điểm
trong header là của riêng packet đó, còn vòng quét là cửa sổ cố định
20°→340° = 320°/bước điểm — driver ghép các packet theo `start_angle` và chỉ
phát scan khi packet đóng ở 340°. Điểm không packet nào gửi = NaN. Bước góc
lấy từ packet mở vòng (span/số điểm) rồi giữ nguyên.
Chu kỳ quay đo host-side giữa hai vòng: `scan_time` là thời gian QUÉT
(period × 320/360, đúng như driver ROS của hãng dùng để đóng dấu), nên spin
rate suy ra từ `1/scan_time` cao hơn cơ khí 360/320; vòng đầu chưa đo được
thì để 0. Trường `time` 16-bit trong header là bộ đếm thiết bị đơn vị chưa
xác định (header của hãng ghi "chưa kích hoạt") → `timestamp_ms` = 0.
Độ phân giải (0.025–0.5°), tốc độ quay, mức lọc nhiễu theo cấu hình đã nạp
bằng tool Windows của hãng — driver không tự đổi. Chưa verify phần cứng.
@@ -289,7 +299,7 @@ comment đầu mỗi file plugin và [docs/diagnostics.md](docs/diagnostics.md).
| `intensities` | Cường độ 0–255 |
| `range_min` / `range_max` | Dải đo hợp lệ (m) |
| `timestamp_ms` | Đồng hồ thiết bị (ms); 0 nếu giao thức không có |
| `time_increment` / `scan_time` | Chỉ rplidar đo được (chu kỳ grab thực); driver khác = 0 |
| `time_increment` / `scan_time` | Chỉ rplidar (chu kỳ grab thực) và ESPE (chu kỳ vòng quét, × 320/360) đo được; driver khác = 0 |
`ExtraInfo`: metadata thô tuỳ giao thức — field thiết bị không có giữ
`std::nullopt`:

170
plugins/driver_espe/espe_driver.cpp
View File

@@ -29,6 +29,25 @@ constexpr uint16_t kMaxIntensity = 30000;
constexpr uint32_t kMaxPointsPerRev = 12800; // 320° at the finest 0.025° step
constexpr int kConnectTimeoutMs = 2000;
// The head measures over a fixed window of every turn — from 20° to 340° in
// device angles, 0° at the rear — and is blind over the remaining 40°. Both
// the revolution's size and every packet's position are anchored to that
// window (the vendor ROS driver hard-codes the same two numbers). Consistent
// with MODEL_ESPE_LGA60's -160…160 preset once angle_offset_deg (-180) is
// applied.
constexpr uint16_t kSweepStartDeg = 20;
constexpr uint16_t kSweepEndDeg = 340;
constexpr float kSweepSpanDeg = static_cast<float>(kSweepEndDeg - kSweepStartDeg);
constexpr float kFullTurnDeg = 360.f;
// Rotation periods above this are stalls/reconnects, not a spin rate (the
// device runs at 10 or 20 Hz): don't publish timing derived from them.
constexpr float kMaxRevPeriodS = 1.f;
// A point no packet ever delivered — "invalid", distinct from the infinity
// that means the device looked and got no return.
const float kMissingPoint = std::numeric_limits<float>::quiet_NaN();
uint16_t be16(const uint8_t* p) {
return static_cast<uint16_t>((p[0] << 8) | p[1]);
}
@@ -69,9 +88,12 @@ ErrorCode EspeDriver::open() {
}
recv_buf_.clear();
points_total_ = 0;
pending_time_ = 0;
scan_ready_ = false;
pending_ranges_.clear();
pending_intensities_.clear();
angle_inc_deg_ = 0.f;
points_total_ = 0;
have_last_rev_ = false;
scan_ready_ = false;
espe_error_status_.reset();
latest_diag_ = Diagnostics{};
@@ -147,6 +169,7 @@ bool EspeDriver::parse_buffer() {
recv_buf_.erase(0, sizeof(kRangeMagic)); // bogus header — resync
continue;
}
// Both counters describe this packet; the clamp is the vendor's.
if (data_size > measure_size) data_size = measure_size;
size_t frame_size = kRangeHeaderSize + static_cast<size_t>(data_size) * 4;
@@ -161,39 +184,86 @@ bool EspeDriver::parse_buffer() {
}
}
// Range frame: "HISN", then big-endian u16 start_angle, end_angle (deg),
// data_size (points in this frame), data_position (cumulative points incl.
// this frame), measure_size (points per revolution), time; then data_size ×
// Latch the angular step and size the revolution around it. Kept stable once
// latched: the wire angles are whole degrees, so a step re-derived from a
// mid-sweep packet jitters, and re-sizing would drop the sweep in flight.
void EspeDriver::set_resolution(float inc_deg) {
if (!(inc_deg > 0.f)) return;
const long total = std::lround(kSweepSpanDeg / inc_deg);
if (total < 2 || total > static_cast<long>(kMaxPointsPerRev)) return; // implausible step
if (angle_inc_deg_ > 0.f && static_cast<uint32_t>(total) == points_total_) return;
angle_inc_deg_ = inc_deg;
points_total_ = static_cast<uint32_t>(total);
begin_revolution();
}
void EspeDriver::begin_revolution() {
if (points_total_ == 0) return;
pending_ranges_.assign(points_total_, kMissingPoint);
pending_intensities_.assign(points_total_, 0.f);
}
// Range frame: "HISN", then big-endian u16 start_angle, end_angle (the
// angular window THIS packet covers, whole degrees), data_size (points in
// this packet's payload), data_position and measure_size (the vendor's
// "position"/"count" of the current packet's points), time; then data_size ×
// 4 B little-endian (u16 distance mm, u16 intensity).
//
// A packet is a slice of the sweep, not a revolution: the device splits every
// 20°→340° sweep into several of them, the first opening at 20° and the last
// closing at 340°. So the revolution holds 320°/step points, NOT measure_size
// — reading measure_size as the revolution size (as this driver first did)
// emits one scan per packet, each covering only that packet's few degrees.
void EspeDriver::handle_range_frame(const uint8_t* frame, uint16_t data_size) {
uint16_t start_angle = be16(frame + 4);
uint16_t end_angle = be16(frame + 6);
uint16_t data_position = be16(frame + 10);
uint16_t measure_size = be16(frame + 12);
pending_time_ = be16(frame + 14);
const uint16_t start_angle = be16(frame + 4);
const uint16_t end_angle = be16(frame + 6);
const uint16_t data_position = be16(frame + 10);
const uint16_t measure_size = be16(frame + 12);
// frame + 14 is a 16-bit device counter the vendor header marks as "time
// flag (not enabled)"; see finish_scan() for why it is not a timestamp.
// First frame of a revolution (or geometry changed) → start a new one.
if (points_total_ != measure_size || data_position <= data_size) {
points_total_ = measure_size;
rev_start_deg_ = static_cast<float>(start_angle);
angle_inc_deg_ = static_cast<float>(end_angle - start_angle) / measure_size;
pending_ranges_.assign(points_total_, 0.f);
pending_intensities_.assign(points_total_, 0.f);
// Step = this packet's angular span / its point count. Taken from the
// packet that opens a sweep — the one the vendor driver trusts — or from
// whatever arrives first while nothing is latched yet.
if (start_angle == kSweepStartDeg || angle_inc_deg_ <= 0.f) {
const float span = static_cast<float>(end_angle) - static_cast<float>(start_angle);
if (span > 0.f && measure_size > 0)
set_resolution(span / static_cast<float>(measure_size));
}
if (angle_inc_deg_ <= 0.f) { points_total_ = 0; return; }
if (angle_inc_deg_ <= 0.f) return; // step still unknown — nowhere to put the points
// start_angle is normally constant across the revolution, so this is just
// the cumulative position; the angle term covers firmware that advances it.
int32_t begin = static_cast<int32_t>(std::lround(
(static_cast<float>(start_angle) - rev_start_deg_) / angle_inc_deg_))
+ static_cast<int32_t>(data_position) - static_cast<int32_t>(data_size);
// First packet of a sweep: drop anything a lost closing packet left behind.
if (start_angle == kSweepStartDeg && data_position <= data_size) begin_revolution();
// Index of this packet's first point within the sweep, verbatim from the
// vendor driver: the angular offset from 20°, plus what the header's own
// counters carry. Exactly one of the two terms moves, whichever way the
// firmware numbers its packets — either start_angle walks the sweep while
// data_position stays at this packet's own count, or start_angle stays at
// 20° while data_position accumulates — so the sum is the packet's true
// start index in both cases.
const int32_t begin =
static_cast<int32_t>(std::lround(
(static_cast<float>(start_angle) - static_cast<float>(kSweepStartDeg)) / angle_inc_deg_))
+ static_cast<int32_t>(data_position) - static_cast<int32_t>(data_size);
// Integer wire angles make 320°/step land a point or two short of what the
// device actually streams; grow rather than clip the tail (the vendor
// driver does the same).
const size_t needed = static_cast<size_t>(begin > 0 ? begin : 0) + data_size;
if (needed > pending_ranges_.size() && needed <= kMaxPointsPerRev) {
pending_ranges_.resize(needed, kMissingPoint);
pending_intensities_.resize(needed, 0.f);
points_total_ = static_cast<uint32_t>(needed);
}
const uint8_t* p = frame + kRangeHeaderSize;
for (uint16_t i = 0; i < data_size; ++i, p += 4) {
int32_t idx = begin + i;
if (idx < 0 || idx >= static_cast<int32_t>(points_total_)) continue;
uint16_t dist = le16(p + 0);
uint16_t inten = le16(p + 2);
const int32_t idx = begin + i;
if (idx < 0 || idx >= static_cast<int32_t>(pending_ranges_.size())) continue;
const uint16_t dist = le16(p + 0);
const uint16_t inten = le16(p + 2);
pending_ranges_[idx] = (dist > kMaxDistanceMm)
? std::numeric_limits<float>::infinity()
: static_cast<float>(dist) * 1e-3f; // mm -> m
@@ -203,22 +273,53 @@ void EspeDriver::handle_range_frame(const uint8_t* frame, uint16_t data_size) {
* (255.f / kMaxIntensity);
}
if (data_position >= points_total_) finish_scan();
// The packet that closes the sweep at 340°, with its point counter full,
// ends the revolution — the vendor driver's condition unchanged. It holds
// whichever way the firmware numbers packets: per-packet counters make
// data_position == measure_size true on every packet (so the 340° edge
// decides), cumulative ones make it true only on the sweep's last packet.
if (end_angle == kSweepEndDeg && data_position == measure_size) finish_scan();
}
void EspeDriver::finish_scan() {
if (pending_ranges_.size() < 2 || angle_inc_deg_ <= 0.f) return;
LaserScan& scan = ready_result_.scan;
scan = LaserScan{};
scan.timestamp_ms = pending_time_; // header "time" field, unit unverified
// The header's 16-bit "time" field is a device counter of unverified unit
// (the vendor header calls it "not enabled" and its ROS driver never
// stamps a scan with it), while this field is contracted to be a device
// clock in ms — leave it at 0 and report timing from the rotation below.
scan.timestamp_ms = 0;
scan.ranges = std::move(pending_ranges_);
scan.intensities = std::move(pending_intensities_);
scan.angle_min = (rev_start_deg_ + cfg_.angle_offset_deg) * kDeg2Rad;
scan.angle_min = (static_cast<float>(kSweepStartDeg) + cfg_.angle_offset_deg) * kDeg2Rad;
scan.angle_increment = angle_inc_deg_ * kDeg2Rad;
scan.angle_max = scan.angle_min +
scan.angle_increment * static_cast<float>(scan.ranges.size() - 1);
scan.range_min = cfg_.range_min_m;
scan.range_max = cfg_.range_max_m;
// Timing: revolutions complete one rotation period apart, but the points
// in one only span the 320° the head measures — the remaining 40° is dead
// time before the next sweep starts. scan_time is what a consumer
// subtracts from the arrival time to date the FIRST point, so it must be
// the sweep, not the period (the vendor ROS driver stamps with the same
// 320/360 factor). Consequence: a spin rate read back as 1/scan_time is
// 360/320 higher than the mechanical one. The first revolution has
// nothing to measure against — leave the fields at 0 and let the consumer
// fall back.
const auto now = std::chrono::steady_clock::now();
if (have_last_rev_) {
const float period = std::chrono::duration<float>(now - last_rev_end_).count();
if (period > 0.f && period < kMaxRevPeriodS) {
scan.scan_time = period * (kSweepSpanDeg / kFullTurnDeg);
scan.time_increment = scan.scan_time / static_cast<float>(scan.ranges.size() - 1);
}
}
last_rev_end_ = now;
have_last_rev_ = true;
finalize_scan(scan, cfg_, inverted_);
ExtraInfo& info = ready_result_.info;
@@ -227,13 +328,10 @@ void EspeDriver::finish_scan() {
info.espe_error_status = espe_error_status_;
latest_diag_ = decode_diagnostics(info);
latest_diag_.device_timestamp_ms = scan.timestamp_ms;
mark_scan_decoded();
pending_ranges_.clear();
pending_intensities_.clear();
points_total_ = 0;
scan_ready_ = true;
begin_revolution(); // the vectors above were moved out — restore them
scan_ready_ = true;
}
bool EspeDriver::recv_scan(ScanResult& out, int timeout_ms) {

24
plugins/driver_espe/espe_driver.hpp
View File

@@ -2,6 +2,7 @@
#pragma once
#include "lidar_interface.hpp"
#include <chrono>
#include <cstdint>
#include <optional>
#include <string>
@@ -18,6 +19,11 @@ inline constexpr ModelConfig MODEL_ESPE_LGA60 { "ESPE-LGA60", -160.f, 160.f, 0.0
// ROS driver; NOT verified on real hardware. open() sends the "RAuto" start
// command; device parameters (spin rate, resolution, filters) are whatever
// the vendor Windows config tool programmed — this driver does not set them.
//
// The device streams each 20°→340° sweep as several packets, so a scan is
// assembled across packets and only completes when the sweep closes at 340°
// (see handle_range_frame). Rotation timing is measured host-side: the wire
// carries no usable clock.
class EspeDriver : public LidarDriverInterface {
public:
// ip: device address; use_udp selects the transport the device is
@@ -52,6 +58,8 @@ private:
bool fill_buffer(int timeout_ms); // one recv() into recv_buf_
bool parse_buffer(); // consume frames; true when a scan completed
void handle_range_frame(const uint8_t* frame, uint16_t data_size);
void set_resolution(float inc_deg); // latch the step + (re)size the revolution
void begin_revolution(); // blank the buffer for a fresh sweep
void finish_scan();
ModelConfig cfg_;
@@ -66,13 +74,19 @@ private:
// Stream bytes carried across frame boundaries; per-instance.
std::string recv_buf_;
// Per-revolution accumulation
// Per-revolution accumulation. A revolution is the device's fixed
// 20°→340° sweep, so it holds 320°/angle_inc_deg_ points — several
// packets' worth. The header's point counters describe one PACKET and say
// nothing about the revolution's size.
std::vector<float> pending_ranges_;
std::vector<float> pending_intensities_;
float rev_start_deg_ = 0.f; // device angle of the revolution's first point
float angle_inc_deg_ = 0.f;
uint32_t points_total_ = 0; // measure_size from the header; 0 = no rev open
uint16_t pending_time_ = 0; // header "time" field, unit unverified
float angle_inc_deg_ = 0.f; // angular step; 0 = not latched yet
uint32_t points_total_ = 0; // points per revolution = 320° / step
// Rotation period, measured host-side between completed revolutions: the
// wire carries no usable clock (see finish_scan()).
std::chrono::steady_clock::time_point last_rev_end_{};
bool have_last_rev_ = false;
// Latched from the newest "WSimu" area frame, if the device sends any.
std::optional<uint16_t> espe_error_status_;