Skip to content

Commit 7438fcb

Browse files
ibrahimhroobibrahimhroob
committed
feat: enhance device configuration reporting and add firmware query callbacks
1 parent e662c7b commit 7438fcb

1 file changed

Lines changed: 175 additions & 16 deletions

File tree

‎src/tools/livox_scan.cpp‎

Lines changed: 175 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -42,11 +42,26 @@
4242
// Per-device information collected during the scan
4343
// --------------------------------------------------------------------------
4444

45+
struct ConfigInfo {
46+
std::string firmware_version;
47+
std::string work_mode;
48+
int32_t pcl_data_type;
49+
int32_t scan_pattern;
50+
uint32_t blind_spot;
51+
bool dual_emit;
52+
bool imu_enabled;
53+
bool config_queried;
54+
55+
ConfigInfo() : pcl_data_type(-1), scan_pattern(-1), blind_spot(0),
56+
dual_emit(false), imu_enabled(false), config_queried(false) {}
57+
};
58+
4559
struct DeviceEntry {
4660
uint32_t handle;
4761
std::string lidar_ip;
4862
std::string sn;
4963
uint8_t dev_type;
64+
ConfigInfo config;
5065
};
5166

5267
// --------------------------------------------------------------------------
@@ -79,6 +94,101 @@ static const char* DevTypeName(uint8_t dev_type) {
7994
}
8095
}
8196

97+
// --------------------------------------------------------------------------
98+
// Helper: convert point data type enum to string
99+
// --------------------------------------------------------------------------
100+
static const char* DataTypeName(int32_t type) {
101+
switch (type) {
102+
case 0x01: return "Cartesian High";
103+
case 0x02: return "Cartesian Low";
104+
case 0x03: return "Spherical";
105+
default: return "Unknown";
106+
}
107+
}
108+
109+
// --------------------------------------------------------------------------
110+
// Helper: convert scan pattern enum to string
111+
// --------------------------------------------------------------------------
112+
static const char* ScanPatternName(int32_t pattern) {
113+
switch (pattern) {
114+
case 0x00: return "Non-Repetitive";
115+
case 0x01: return "Repetitive";
116+
case 0x02: return "Repetitive Low FPS";
117+
default: return "Unknown";
118+
}
119+
}
120+
121+
// --------------------------------------------------------------------------
122+
// Helper: find device entry by handle
123+
// --------------------------------------------------------------------------
124+
static DeviceEntry* FindDeviceByHandle(uint32_t handle) {
125+
std::lock_guard<std::mutex> lock(g_scan.mtx);
126+
for (auto& d : g_scan.devices) {
127+
if (d.handle == handle) return &d;
128+
}
129+
return nullptr;
130+
}
131+
132+
// --------------------------------------------------------------------------
133+
// Callback: firmware version query response
134+
// --------------------------------------------------------------------------
135+
static void OnFirmwareVersionQuery(livox_status status,
136+
uint32_t handle,
137+
LivoxLidarDiagInternalInfoResponse* response,
138+
void* /*client_data*/) {
139+
DeviceEntry* dev = FindDeviceByHandle(handle);
140+
if (!dev) return;
141+
142+
if (status == kLivoxLidarStatusSuccess && response && response->ret_code == 0) {
143+
// Parse firmware version from response data
144+
// The format is typically a param_num followed by key-value pairs
145+
if (response->param_num > 0 && response->data[0]) {
146+
// Simple extraction - firmware version is typically a string
147+
dev->config.firmware_version = std::string(reinterpret_cast<const char*>(response->data));
148+
}
149+
}
150+
dev->config.config_queried = true;
151+
}
152+
153+
// --------------------------------------------------------------------------
154+
// Callback: internal info query response
155+
// --------------------------------------------------------------------------
156+
static void OnInternalInfoQuery(livox_status status,
157+
uint32_t handle,
158+
LivoxLidarDiagInternalInfoResponse* response,
159+
void* /*client_data*/) {
160+
DeviceEntry* dev = FindDeviceByHandle(handle);
161+
if (!dev) return;
162+
163+
if (status == kLivoxLidarStatusSuccess && response && response->ret_code == 0) {
164+
std::cout << "[info] Received internal config for " << dev->lidar_ip
165+
<< " (param_num=" << response->param_num << ")\n";
166+
167+
// Parse key-value parameters from the response
168+
// The data format is: [key(2B)][length(2B)][value(length B)] repeated
169+
uint8_t* ptr = response->data;
170+
size_t offset = 0;
171+
172+
for (int i = 0; i < response->param_num && offset < 1024; ++i) {
173+
if (offset + 4 > 1024) break;
174+
175+
uint16_t key = *reinterpret_cast<uint16_t*>(ptr + offset);
176+
offset += 2;
177+
uint16_t length = *reinterpret_cast<uint16_t*>(ptr + offset);
178+
offset += 2;
179+
180+
if (offset + length > 1024) break;
181+
182+
// Parse based on key - common keys include work mode, data type, etc.
183+
// Key values would need to be determined from SDK documentation
184+
std::cout << " [param] key=0x" << std::hex << key
185+
<< " length=" << std::dec << length << "\n";
186+
187+
offset += length;
188+
}
189+
}
190+
}
191+
82192
// --------------------------------------------------------------------------
83193
// Callback: called by the SDK whenever a lidar is discovered or its info
84194
// changes (multiple calls for the same device are de-duplicated)
@@ -88,25 +198,38 @@ static void OnLidarInfoChange(const uint32_t handle,
88198
void* /*client_data*/) {
89199
if (!info) return;
90200

91-
std::lock_guard<std::mutex> lock(g_scan.mtx);
201+
bool should_query = false;
202+
{
203+
std::lock_guard<std::mutex> lock(g_scan.mtx);
92204

93-
// De-duplicate by handle
94-
for (const auto& d : g_scan.devices) {
95-
if (d.handle == handle) return;
96-
}
205+
// De-duplicate by handle
206+
for (const auto& d : g_scan.devices) {
207+
if (d.handle == handle) return;
208+
}
97209

98-
DeviceEntry entry;
99-
entry.handle = handle;
100-
entry.lidar_ip = info->lidar_ip;
101-
entry.sn = info->sn;
102-
entry.dev_type = info->dev_type;
210+
DeviceEntry entry;
211+
entry.handle = handle;
212+
entry.lidar_ip = info->lidar_ip;
213+
entry.sn = info->sn;
214+
entry.dev_type = info->dev_type;
103215

104-
std::cout << "[found] " << entry.lidar_ip
105-
<< " type=" << DevTypeName(entry.dev_type)
106-
<< " sn=" << entry.sn
107-
<< "\n";
216+
std::cout << "[found] " << entry.lidar_ip
217+
<< " type=" << DevTypeName(entry.dev_type)
218+
<< " sn=" << entry.sn
219+
<< "\n";
108220

109-
g_scan.devices.push_back(std::move(entry));
221+
g_scan.devices.push_back(std::move(entry));
222+
should_query = true;
223+
}
224+
225+
// Query device configuration outside the lock
226+
if (should_query) {
227+
// Query firmware version
228+
QueryLivoxLidarFirmwareVer(handle, OnFirmwareVersionQuery, nullptr);
229+
230+
// Query internal info (includes various config parameters)
231+
QueryLivoxLidarInternalInfo(handle, OnInternalInfoQuery, nullptr);
232+
}
110233
}
111234

112235
// --------------------------------------------------------------------------
@@ -267,7 +390,7 @@ int main(int argc, char** argv) {
267390
std::cout << "[result] Found " << devs.size()
268391
<< " device(s):\n\n";
269392

270-
// Column widths
393+
// Print basic info table
271394
const int w_ip = 16;
272395
const int w_type = 16;
273396
const int w_sn = 18;
@@ -296,5 +419,41 @@ int main(int argc, char** argv) {
296419
hline();
297420
std::cout << "\n";
298421

422+
// Print detailed configuration for each device
423+
std::cout << "=== Device Configuration Details ===\n\n";
424+
425+
for (const auto& d : devs) {
426+
std::cout << "Device: " << d.lidar_ip << " (" << d.sn << ")\n";
427+
std::cout << " Type: " << DevTypeName(d.dev_type) << "\n";
428+
429+
if (!d.config.firmware_version.empty()) {
430+
std::cout << " Firmware Version: " << d.config.firmware_version << "\n";
431+
}
432+
433+
if (d.config.pcl_data_type >= 0) {
434+
std::cout << " Point Data Type: " << DataTypeName(d.config.pcl_data_type)
435+
<< " (" << d.config.pcl_data_type << ")\n";
436+
}
437+
438+
if (d.config.scan_pattern >= 0) {
439+
std::cout << " Scan Pattern: " << ScanPatternName(d.config.scan_pattern)
440+
<< " (" << d.config.scan_pattern << ")\n";
441+
}
442+
443+
if (d.config.blind_spot > 0) {
444+
std::cout << " Blind Spot: " << d.config.blind_spot << " cm\n";
445+
}
446+
447+
std::cout << " Dual Emit: " << (d.config.dual_emit ? "Enabled" : "Disabled") << "\n";
448+
std::cout << " IMU Data: " << (d.config.imu_enabled ? "Enabled" : "Disabled") << "\n";
449+
450+
if (!d.config.work_mode.empty()) {
451+
std::cout << " Work Mode: " << d.config.work_mode << "\n";
452+
}
453+
454+
std::cout << " Config Queried: " << (d.config.config_queried ? "Yes" : "No") << "\n";
455+
std::cout << "\n";
456+
}
457+
299458
return 0;
300459
}

0 commit comments

Comments
 (0)