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

update brand ESPE

Browse Source
This commit is contained in:
QUYVN
2026-07-07 10:38:42 +07:00
parent 1c347a4918
commit 917b4fe4c5
15 changed files with 741 additions and 263 deletions
Download Patch File Download Diff File Expand all files Collapse all files

265
src/espe_lidar.cpp Normal file
View File

@@ -0,0 +1,265 @@
#include "lidarlib/espe_lidar.hpp"
#include "lidar_bytes.hpp"
#include "lidar_net.hpp"
#include <algorithm>
#include <cerrno>
#include <cmath>
#include <cstring>
#include <limits>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <sys/select.h>
#include <netinet/in.h>
namespace lidarlib {
namespace {
// "RAuto" + fixed tail — puts the device into continuous measurement output.
constexpr uint8_t kStartCapture[8] = {0x52, 0x41, 0x75, 0x74, 0x6F, 0x01, 0x87, 0x80};
constexpr char kRangeMagic[4] = {'H', 'I', 'S', 'N'};
constexpr char kAreaMagic[5] = {'W', 'S', 'i', 'm', 'u'};
constexpr size_t kRangeHeaderSize = 16; // magic + 6 big-endian u16 fields
constexpr size_t kAreaFrameSize = 13; // magic + 4 status bytes + err u16 + crc u16
constexpr uint16_t kMaxDistanceMm = 50000; // wire sentinel: beyond = no return
constexpr uint16_t kMaxIntensity = 30000;
constexpr uint32_t kMaxPointsPerRev = 12800; // 320° at the finest 0.025° step
constexpr int kConnectTimeoutMs = 2000;
uint16_t be16(const uint8_t* p) {
return static_cast<uint16_t>((p[0] << 8) | p[1]);
}
} // namespace
EspeDriver::EspeDriver(const ModelConfig& cfg, const std::string& ip,
uint16_t port, bool use_udp, bool inverted)
: cfg_(cfg), detected_model_name_(cfg.name ? cfg.name : ""), ip_(ip),
port_(port), use_udp_(use_udp), inverted_(inverted) {}
EspeDriver::~EspeDriver() { close(); }
ErrorCode EspeDriver::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, use_udp_ ? SOCK_DGRAM : SOCK_STREAM, 0);
if (sock_fd_ < 0) return set_error(ErrorCode::SocketError);
ErrorCode conn_err = ErrorCode::Ok;
if (use_udp_) {
// connect() on UDP just fixes the peer; replies come to our port.
if (::connect(sock_fd_, reinterpret_cast<sockaddr*>(&addr), sizeof(addr)) < 0)
conn_err = ErrorCode::ConnectionFailed;
} else {
conn_err = connect_tcp_with_timeout(sock_fd_, addr, kConnectTimeoutMs);
}
if (conn_err != ErrorCode::Ok) {
::close(sock_fd_);
sock_fd_ = -1;
return set_error(conn_err);
}
recv_buf_.clear();
points_total_ = 0;
pending_time_ = 0;
scan_ready_ = false;
espe_error_status_.reset();
latest_diag_ = Diagnostics{};
// Device is passive until told to stream.
ssize_t n = ::send(sock_fd_, kStartCapture, sizeof(kStartCapture), 0);
if (n != static_cast<ssize_t>(sizeof(kStartCapture))) {
close();
return set_error(ErrorCode::HandshakeFailed);
}
return set_error(ErrorCode::Ok);
}
void EspeDriver::close() {
if (sock_fd_ >= 0) {
::close(sock_fd_);
sock_fd_ = -1;
}
}
bool EspeDriver::fill_buffer(int timeout_ms) {
if (!is_open()) { set_error(ErrorCode::NotOpen); return false; }
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;
}
}
char buf[4096];
ssize_t n = ::recv(sock_fd_, buf, sizeof(buf), 0);
if (n <= 0) { set_error(ErrorCode::DeviceDisconnected); return false; }
recv_buf_.append(buf, static_cast<size_t>(n));
return true;
}
// Consume complete frames from recv_buf_; returns true once a full revolution
// has been assembled (ready_result_/scan_ready_ set by finish_scan()).
bool EspeDriver::parse_buffer() {
for (;;) {
size_t range_pos = recv_buf_.find(kRangeMagic, 0, sizeof(kRangeMagic));
size_t area_pos = recv_buf_.find(kAreaMagic, 0, sizeof(kAreaMagic));
size_t pos = std::min(range_pos, area_pos);
if (pos == std::string::npos) {
// No magic in sight: keep only a possible partial magic at the tail.
if (recv_buf_.size() > sizeof(kAreaMagic) - 1)
recv_buf_.erase(0, recv_buf_.size() - (sizeof(kAreaMagic) - 1));
return scan_ready_;
}
if (pos > 0) recv_buf_.erase(0, pos);
const uint8_t* d = reinterpret_cast<const uint8_t*>(recv_buf_.data());
if (area_pos < range_pos) {
if (recv_buf_.size() < kAreaFrameSize) return scan_ready_;
// Zone/obstacle frame — only sent when the host polls areas, but
// it carries the device fault word, so latch it if it appears.
// Byte order unverified on hardware: the protocol is mixed-endian
// (header fields big-endian, point payload little-endian) and no
// spec covers this field; little-endian assumed like the payload.
espe_error_status_ = le16(d + 9);
recv_buf_.erase(0, kAreaFrameSize);
continue;
}
if (recv_buf_.size() < kRangeHeaderSize) return scan_ready_;
uint16_t data_size = be16(d + 8);
uint16_t measure_size = be16(d + 12);
if (measure_size == 0 || measure_size > kMaxPointsPerRev) {
recv_buf_.erase(0, sizeof(kRangeMagic)); // bogus header — resync
continue;
}
if (data_size > measure_size) data_size = measure_size;
size_t frame_size = kRangeHeaderSize + static_cast<size_t>(data_size) * 4;
if (recv_buf_.size() < frame_size) return scan_ready_;
handle_range_frame(d, data_size);
recv_buf_.erase(0, frame_size);
// Stop as soon as a revolution completes — draining further frames
// could finish a second revolution and overwrite ready_result_ before
// the caller consumes it. Leftover bytes wait for the next call.
if (scan_ready_) return true;
}
}
// 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 ×
// 4 B little-endian (u16 distance mm, u16 intensity).
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);
// 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);
}
if (angle_inc_deg_ <= 0.f) { points_total_ = 0; return; }
// 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);
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);
pending_ranges_[idx] = (dist > kMaxDistanceMm)
? std::numeric_limits<float>::infinity()
: static_cast<float>(dist) * 1e-3f; // mm -> m
// Wire intensity is 0..30000 — rescale to the 0-255 LaserScan contract.
pending_intensities_[idx] =
static_cast<float>(inten > kMaxIntensity ? kMaxIntensity : inten)
* (255.f / kMaxIntensity);
}
if (data_position >= points_total_) finish_scan();
}
void EspeDriver::finish_scan() {
LaserScan& scan = ready_result_.scan;
scan = LaserScan{};
scan.timestamp_ms = pending_time_; // header "time" field, unit unverified
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_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;
if (inverted_)
invert_scan(scan);
if (cfg_.remap_angles)
remap_scan_window(scan, cfg_.out_angle_min, cfg_.out_angle_max);
ExtraInfo& info = ready_result_.info;
info = ExtraInfo{};
info.detected_model = cfg_.name;
info.espe_error_status = espe_error_status_;
latest_diag_ = decode_diagnostics(info);
latest_diag_.device_timestamp_ms = scan.timestamp_ms;
pending_ranges_.clear();
pending_intensities_.clear();
points_total_ = 0;
scan_ready_ = true;
}
bool EspeDriver::recv_scan(ScanResult& out, int timeout_ms) {
for (;;) {
if (parse_buffer()) {
scan_ready_ = false;
out = std::move(ready_result_);
set_error(ErrorCode::Ok);
return true;
}
if (!fill_buffer(timeout_ms)) return false;
}
}
bool EspeDriver::spin_once() {
if (!parse_buffer()) {
if (!fill_buffer(0)) return false;
parse_buffer();
}
if (scan_ready_) {
scan_ready_ = false;
if (cb_) cb_(ready_result_);
}
return true;
}
} // namespace lidarlib

11
src/lidar_bytes.hpp
View File

@@ -1,6 +1,7 @@
// Internal helpers shared by the driver TUs — not part of the public API.
#pragma once
#include "lidarlib/lidar.hpp"
#include <algorithm>
#include <cstdint>
#include <cstring>
@@ -20,6 +21,16 @@ inline void remap_scan_window(LaserScan& scan, float min_deg, float max_deg) {
scan.angle_max = new_max;
}
// Mirror a finished scan for a unit mounted upside-down: reverse the point
// order and negate the angular window. Apply before remap_scan_window().
inline void invert_scan(LaserScan& scan) {
std::reverse(scan.ranges.begin(), scan.ranges.end());
std::reverse(scan.intensities.begin(), scan.intensities.end());
const float new_min = -scan.angle_max;
scan.angle_max = -scan.angle_min;
scan.angle_min = new_min;
}
// Little-endian readers (bounds are the caller's responsibility).
inline uint8_t le_u8 (const uint8_t* p) { return p[0]; }
inline uint16_t le16(const uint8_t* p) {

27
src/lidar_config.cpp
View File

@@ -1,5 +1,6 @@
#include "lidarlib/config.hpp"
#include "lidarlib/sick_lidar.hpp"
#include "lidarlib/espe_lidar.hpp"
#include "json_mini.hpp"
#include <algorithm>
#include <fstream>
@@ -26,6 +27,7 @@ constexpr ModelEntry kModels[] = {
{ "SICK-TIM571", &MODEL_SICK_TIM571, "SICK" },
{ "SICK-TIM7xx", &MODEL_SICK_TIM7XX, "SICK" },
{ "SICK-nanoScan3", &MODEL_SICK_NANOSCAN3, "SICK" },
{ "ESPE-LGA60", &MODEL_ESPE_LGA60, "ESPE" },
};
json::Value to_json(const LidarConfig& c) {
@@ -36,6 +38,7 @@ json::Value to_json(const LidarConfig& c) {
v.set("brand", json::Value::make_string(c.brand));
v.set("model", json::Value::make_string(c.model));
v.set("inverted", json::Value::make_bool(c.inverted));
v.set("use_udp", json::Value::make_bool(c.use_udp));
v.set("angle_min_deg", json::Value::make_number(c.angle_min_deg));
v.set("angle_max_deg", json::Value::make_number(c.angle_max_deg));
return v;
@@ -49,6 +52,7 @@ LidarConfig lidar_from_json(const json::Value& v, const LidarConfig& def) {
c.brand = v.get_string("brand", def.brand);
c.model = v.get_string("model", def.model);
c.inverted = v.get_bool("inverted", def.inverted);
c.use_udp = v.get_bool("use_udp", def.use_udp);
c.angle_min_deg = static_cast<float>(v.get_number("angle_min_deg", def.angle_min_deg));
c.angle_max_deg = static_cast<float>(v.get_number("angle_max_deg", def.angle_max_deg));
return c;
@@ -80,7 +84,7 @@ const std::vector<std::string>& model_names() {
}
const std::vector<std::string>& brand_names() {
static const std::vector<std::string> names = {"OLEI", "SICK"};
static const std::vector<std::string> names = {"OLEI", "SICK", "ESPE"};
return names;
}
@@ -137,11 +141,20 @@ void save_config(const std::string& path, const Config& cfg) {
}
std::unique_ptr<Lidar> make_lidar(const LidarConfig& cfg) {
// Anything but the exact string "SICK" is OLEI (keeps brand-less configs working).
// Anything but the exact strings "SICK"/"ESPE" is OLEI (keeps brand-less
// configs working). Brand name and fallback model are decided together so
// a new brand adds exactly one branch here plus one construction case.
const bool is_sick = (cfg.brand == "SICK");
const bool is_espe = (cfg.brand == "ESPE");
const ModelConfig* model = model_by_name_for_brand(cfg.model, is_sick ? "SICK" : "OLEI");
if (!model) model = is_sick ? &MODEL_SICK_TIM571 : &MODEL_AUTO;
const char* brand;
const ModelConfig* fallback;
if (is_sick) { brand = "SICK"; fallback = &MODEL_SICK_TIM571; }
else if (is_espe) { brand = "ESPE"; fallback = &MODEL_ESPE_LGA60; }
else { brand = "OLEI"; fallback = &MODEL_AUTO; }
const ModelConfig* model = model_by_name_for_brand(cfg.model, brand);
if (!model) model = fallback;
ModelConfig mc = *model;
if (cfg.angle_min_deg > -360.f || cfg.angle_max_deg < 360.f) {
@@ -152,9 +165,11 @@ std::unique_ptr<Lidar> make_lidar(const LidarConfig& cfg) {
if (is_sick) {
if (model == &MODEL_SICK_NANOSCAN3)
return std::make_unique<NanoScanDriver>(mc, cfg.ip, cfg.port);
return std::make_unique<SickDriver>(mc, cfg.ip, cfg.port);
return std::make_unique<NanoScanDriver>(mc, cfg.ip, cfg.port, cfg.inverted);
return std::make_unique<SickDriver>(mc, cfg.ip, cfg.port, cfg.inverted);
}
if (is_espe)
return std::make_unique<EspeDriver>(mc, cfg.ip, cfg.port, cfg.use_udp, cfg.inverted);
return std::make_unique<Driver>(mc, cfg.ip, cfg.port, cfg.inverted);
}

51
src/lidar_net.hpp Normal file
View File

@@ -0,0 +1,51 @@
// Internal socket helpers shared by the TCP driver TUs — not part of the public API.
#pragma once
#include "lidarlib/error.hpp"
#include <cerrno>
#include <fcntl.h>
#include <netinet/in.h>
#include <netinet/tcp.h>
#include <sys/select.h>
#include <sys/socket.h>
namespace lidarlib {
// Non-blocking connect with a bounded timeout — a blocking connect() to an
// unreachable device would stall for the OS default (~2 min on Linux).
// Enables TCP_NODELAY on success; the fd is returned to blocking mode either
// way. The caller owns the fd and closes it on failure.
inline ErrorCode connect_tcp_with_timeout(int fd, const sockaddr_in& addr, int timeout_ms) {
int flags = ::fcntl(fd, F_GETFL, 0);
::fcntl(fd, F_SETFL, flags | O_NONBLOCK);
ErrorCode conn_err = ErrorCode::Ok;
int rc = ::connect(fd, reinterpret_cast<const sockaddr*>(&addr), sizeof(addr));
if (rc < 0 && errno != EINPROGRESS) {
conn_err = (errno == ECONNREFUSED) ? ErrorCode::ConnectionRefused
: ErrorCode::ConnectionFailed;
} else if (rc < 0) {
fd_set wfds; FD_ZERO(&wfds); FD_SET(fd, &wfds);
timeval tv{ timeout_ms / 1000, (timeout_ms % 1000) * 1000 };
rc = ::select(fd + 1, nullptr, &wfds, nullptr, &tv);
if (rc == 0) {
conn_err = ErrorCode::Timeout;
} else if (rc < 0) {
conn_err = ErrorCode::ConnectionFailed;
} else {
int err = 0; socklen_t errlen = sizeof(err);
::getsockopt(fd, SOL_SOCKET, SO_ERROR, &err, &errlen);
if (err != 0)
conn_err = (err == ECONNREFUSED) ? ErrorCode::ConnectionRefused
: ErrorCode::ConnectionFailed;
}
}
::fcntl(fd, F_SETFL, flags);
if (conn_err == ErrorCode::Ok) {
int nodelay = 1;
::setsockopt(fd, IPPROTO_TCP, TCP_NODELAY, &nodelay, sizeof(nodelay));
}
return conn_err;
}
} // namespace lidarlib

14
src/olei_lidar.cpp
View File

@@ -52,6 +52,7 @@ Diagnostics decode_diagnostics(const ExtraInfo& info) {
d.status_flags = info.status_flags;
d.sick_device_status = info.sick_device_status;
d.nano_general_state = info.nano_general_state;
d.espe_error_status = info.espe_error_status;
return d;
}
@@ -125,7 +126,7 @@ bool Driver::recv_scan(ScanResult& out, int timeout_ms) {
return false;
}
}
if (!spin_once()) return false;
if (!poll_packet()) return false;
}
out = std::move(ready_result_);
set_error(ErrorCode::Ok);
@@ -133,6 +134,15 @@ bool Driver::recv_scan(ScanResult& out, int timeout_ms) {
}
bool Driver::spin_once() {
if (!poll_packet()) return false;
if (scan_ready_) {
scan_ready_ = false;
if (cb_) cb_(ready_result_);
}
return true;
}
bool Driver::poll_packet() {
if (!is_open()) { set_error(ErrorCode::NotOpen); return false; }
uint8_t* buf = recv_buf_;
sockaddr_in from{};
@@ -201,8 +211,6 @@ void Driver::flush_scan() {
pending_intensity_.clear();
pending_info_ = ExtraInfo{};
scan_ready_ = true;
if (cb_) cb_(ready_result_);
}
// Family A (0xFAF0): 20B header + 3B blocks (u16 dist, u8 intensity).

59
src/sick_lidar.cpp
View File

@@ -1,5 +1,6 @@
#include "lidarlib/sick_lidar.hpp"
#include "lidar_bytes.hpp"
#include "lidar_net.hpp"
#include <cctype>
#include <cerrno>
@@ -8,13 +9,11 @@
#include <cstring>
#include <limits>
#include <vector>
#include <fcntl.h>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <unistd.h>
#include <sys/select.h>
#include <netinet/in.h>
#include <netinet/tcp.h>
namespace lidarlib {
@@ -48,8 +47,10 @@ constexpr size_t kNanoRecvBufSize = 65536;
constexpr double kNanoAngleResolution = 4194304.0;
} // namespace
SickDriver::SickDriver(const ModelConfig& cfg, const std::string& ip, uint16_t port)
: cfg_(cfg), detected_model_name_(cfg.name ? cfg.name : ""), ip_(ip), port_(port) {}
SickDriver::SickDriver(const ModelConfig& cfg, const std::string& ip, uint16_t port,
bool inverted)
: cfg_(cfg), detected_model_name_(cfg.name ? cfg.name : ""), ip_(ip), port_(port),
inverted_(inverted) {}
SickDriver::~SickDriver() { close(); }
@@ -65,43 +66,13 @@ ErrorCode SickDriver::open() {
sock_fd_ = ::socket(AF_INET, SOCK_STREAM, 0);
if (sock_fd_ < 0) return set_error(ErrorCode::SocketError);
// Non-blocking connect with a bounded timeout — a blocking connect() to an
// unreachable device would stall for the OS default (~2 min on Linux).
int flags = ::fcntl(sock_fd_, F_GETFL, 0);
::fcntl(sock_fd_, F_SETFL, flags | O_NONBLOCK);
ErrorCode conn_err = ErrorCode::Ok;
int rc = ::connect(sock_fd_, reinterpret_cast<sockaddr*>(&addr), sizeof(addr));
if (rc < 0 && errno != EINPROGRESS) {
conn_err = (errno == ECONNREFUSED) ? ErrorCode::ConnectionRefused
: ErrorCode::ConnectionFailed;
} else if (rc < 0) {
fd_set wfds; FD_ZERO(&wfds); FD_SET(sock_fd_, &wfds);
timeval tv{ kConnectTimeoutMs / 1000, (kConnectTimeoutMs % 1000) * 1000 };
rc = ::select(sock_fd_ + 1, nullptr, &wfds, nullptr, &tv);
if (rc == 0) {
conn_err = ErrorCode::Timeout;
} else if (rc < 0) {
conn_err = ErrorCode::ConnectionFailed;
} else {
int err = 0; socklen_t errlen = sizeof(err);
::getsockopt(sock_fd_, SOL_SOCKET, SO_ERROR, &err, &errlen);
if (err != 0)
conn_err = (err == ECONNREFUSED) ? ErrorCode::ConnectionRefused
: ErrorCode::ConnectionFailed;
}
}
::fcntl(sock_fd_, F_SETFL, flags);
ErrorCode conn_err = connect_tcp_with_timeout(sock_fd_, addr, kConnectTimeoutMs);
if (conn_err != ErrorCode::Ok) {
::close(sock_fd_);
sock_fd_ = -1;
return set_error(conn_err);
}
int nodelay = 1;
::setsockopt(sock_fd_, IPPROTO_TCP, TCP_NODELAY, &nodelay, sizeof(nodelay));
recv_buf_.clear();
latest_diag_ = Diagnostics{};
@@ -261,7 +232,9 @@ bool SickDriver::parse_lmdscandata(const std::string& telegram, ScanResult& out)
scan.ranges[d] = static_cast<float>(raw) * scale * 0.001f; // mm -> m
got_dist = true;
} else if (is_rssi && d < scan.intensities.size()) {
scan.intensities[d] = static_cast<float>(raw) * scale;
// Clamp to the 0-255 LaserScan contract (16-bit RSSI can exceed it).
float v = static_cast<float>(raw) * scale;
scan.intensities[d] = v > 255.f ? 255.f : v;
}
}
};
@@ -286,6 +259,8 @@ bool SickDriver::parse_lmdscandata(const std::string& telegram, ScanResult& out)
if (scan.intensities.size() != scan.ranges.size())
scan.intensities.assign(scan.ranges.size(), 0.f);
if (inverted_)
invert_scan(scan);
if (cfg_.remap_angles)
remap_scan_window(scan, cfg_.out_angle_min, cfg_.out_angle_max);
@@ -306,9 +281,10 @@ bool SickDriver::parse_lmdscandata(const std::string& telegram, ScanResult& out)
// ── NanoScanDriver — SICK nanoScan3/microScan3 safety-scanner UDP output ────
NanoScanDriver::NanoScanDriver(const ModelConfig& cfg, const std::string& ip, uint16_t port)
NanoScanDriver::NanoScanDriver(const ModelConfig& cfg, const std::string& ip, uint16_t port,
bool inverted)
: cfg_(cfg), detected_model_name_(cfg.name ? cfg.name : ""), ip_(ip), port_(port),
recv_buf_(kNanoRecvBufSize) {}
inverted_(inverted), recv_buf_(kNanoRecvBufSize) {}
NanoScanDriver::~NanoScanDriver() { close(); }
@@ -371,6 +347,7 @@ int NanoScanDriver::recv_datagram(int timeout_ms) {
// drops that scan and we resync on the next scanNumber.
bool NanoScanDriver::recv_scan(ScanResult& out, int timeout_ms) {
std::vector<uint8_t> tele;
std::vector<uint8_t> have; // per-byte coverage so duplicate fragments don't count twice
uint32_t cur_scan = 0, total = 0, got = 0;
bool assembling = false;
@@ -394,12 +371,14 @@ bool NanoScanDriver::recv_scan(ScanResult& out, int timeout_ms) {
if (!assembling || scan != cur_scan || tl != total) {
cur_scan = scan; total = tl; got = 0;
tele.assign(total, 0);
have.assign(total, 0);
assembling = true;
}
if (static_cast<uint64_t>(foff) + pl_len <= total) {
std::memcpy(tele.data() + foff, pl, pl_len);
got += pl_len;
for (uint32_t b = 0; b < pl_len; ++b)
if (!have[foff + b]) { have[foff + b] = 1; ++got; }
}
if (got >= total) {
@@ -487,6 +466,8 @@ bool NanoScanDriver::parse_packet(const uint8_t* buf, int len, ScanResult& out)
// Raw device time from the DataHeader — an opaque tag, not ms since power-on.
scan.timestamp_ms = le32(buf + 28);
if (inverted_)
invert_scan(scan);
if (cfg_.remap_angles)
remap_scan_window(scan, cfg_.out_angle_min, cfg_.out_angle_max);