Fix Family B angle decode + add LR-1FMI model
parse_family_b() dùng sai hệ số góc 0.25°/LSB; theo spec Olei chính hãng (Olei.LidarSensor/LidarDataBlock.GetAngleDegrees) AngleRaw là 0.01°/LSB. Sai 25× khiến điểm bị gán nhầm góc → một phòng bị bôi thành vòng tròn trên RViz. Đã verify với thiết bị thật OLELR-1FMI: sau khi sửa ra 2400 điểm/vòng, 0–359.9°, đúng hình học môi trường. - Đổi hệ số góc 0.25° → 0.01° trong parse_family_b(). - Bỏ qua block invalid (AngleRaw >= 0xFF00) theo spec. - Dò ranh giới vòng quay PER-POINT thay vì per-packet (một gói có thể chứa >1 vòng), tránh gộp nhiều vòng vào một scan. - Thêm model LR-1FMI (360°, 0.01°/LSB, ~2400 pts/rev) vào bảng model + kModelTable, đặt "1FMI" trước "1F" để khớp đúng chuỗi tên. Co-Authored-By: Claude Opus 4.8 <noreply@anthropic.com>
This commit is contained in:
55
examples/test_dual.cpp
Normal file
55
examples/test_dual.cpp
Normal file
@@ -0,0 +1,55 @@
|
||||
// test_dual.cpp — test 2 Olei lidars (front + rear) concurrently, per appsettings.json
|
||||
// Olei-front: scan_1, DeviceIp 192.168.100.11, LocalIp 192.168.100.100, DevicePort 2368
|
||||
// Olei-rear : scan_2, DeviceIp 192.168.100.12, LocalIp 192.168.100.100, DevicePort 2369
|
||||
#include "lidarlib/lidar.hpp"
|
||||
#include <cstdio>
|
||||
#include <thread>
|
||||
|
||||
static void run_lidar(const char* tag, const lidarlib::ModelConfig& cfg,
|
||||
const std::string& local_ip, uint16_t port, bool inverted, int n_scans) {
|
||||
lidarlib::Driver drv(cfg, local_ip, port, inverted);
|
||||
if (!drv.open()) {
|
||||
fprintf(stderr, "[%s] Khong mo duoc socket tren %s:%u (interface khong ton tai?)\n",
|
||||
tag, local_ip.c_str(), port);
|
||||
return;
|
||||
}
|
||||
printf("[%s] Da bind %s:%u, dang doi scan...\n", tag, local_ip.c_str(), port);
|
||||
|
||||
for (int i = 0; i < n_scans; ++i) {
|
||||
lidarlib::ScanResult result;
|
||||
if (!drv.recv_scan(result, 2000)) {
|
||||
fprintf(stderr, "[%s] Timeout/loi nhan packet (scan #%d)\n", tag, i);
|
||||
continue;
|
||||
}
|
||||
const lidarlib::LaserScan& scan = result.scan;
|
||||
const lidarlib::ExtraInfo& info = result.info;
|
||||
printf("[%s] Scan #%d: %zu diem, ts=%u ms, err=0x%02X, model=%s\n",
|
||||
tag, i, scan.ranges.size(), scan.timestamp_ms, info.error_status,
|
||||
info.detected_model.c_str());
|
||||
for (size_t j = 0; j < 20 && j < scan.ranges.size(); ++j) {
|
||||
float angle_deg = (scan.angle_min + j * scan.angle_increment) * 180.f / 3.14159265f;
|
||||
printf(" [%zu] angle=%.2f dist=%.3fm intensity=%.0f\n",
|
||||
j, angle_deg, scan.ranges[j], scan.intensities[j]);
|
||||
}
|
||||
}
|
||||
drv.close();
|
||||
}
|
||||
|
||||
int main() {
|
||||
// Both front and rear are Family B in practice — front's real header
|
||||
// string is "OLELR-1BS2", rear's is "OLELR-1BS5" (verified via live UDP
|
||||
// sniff), NOT the VB (Family A) model the config name suggested. With
|
||||
// MODEL_AUTO, the driver reads the real model name from the header and
|
||||
// narrows the FOV when it matches a known entry in kModelTable
|
||||
// (olei_lidar.cpp); "1BS5" matches (→ full 360°), but "1BS2" doesn't, so
|
||||
// front currently stays at the unfiltered 360° default. Call
|
||||
// drv.detected_model() to see which name was actually read.
|
||||
std::thread t_front(run_lidar, "front/scan_1", lidarlib::MODEL_AUTO,
|
||||
"192.168.100.100", 2368, false, 5);
|
||||
std::thread t_rear(run_lidar, "rear/scan_2", lidarlib::MODEL_AUTO,
|
||||
"192.168.100.100", 2369, true, 5);
|
||||
|
||||
t_front.join();
|
||||
t_rear.join();
|
||||
return 0;
|
||||
}
|
||||
Reference in New Issue
Block a user