fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

refactor(lightware_laser): simplify device_information handling

Browse Source 
- publish every 3 seconds
- adjust to name instead of vendor and model
- buffer size adjusted according to datasheet
- switch to PublicationData
This commit is contained in:
Matthias Grob 2026-03-31 19:31:10 +02:00
parent 5e570f2d5c
commit 71b8aba3e3
No known key found for this signature in database
GPG Key ID: 8924BFB060E86D63
1 changed files with 25 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

85
src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp
View File

@ -54,10 +54,10 @@
#include <drivers/rangefinder/PX4Rangefinder.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/Publication.hpp>
#include <uORB/topics/device_information.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/distance_sensor_mode_change_request.h>
#include <uORB/topics/device_information.h>
using namespace time_literals;
@ -154,17 +154,14 @@ private:
int updateRestriction();
void readDeviceInformation();
void publishDeviceInformation();
PX4Rangefinder _px4_rangefinder;
uORB::Publication<device_information_s> _device_info_pub{ORB_ID(device_information)};
void readDeviceInformation();
void updateDeviceInformation();
uORB::PublicationData<device_information_s> _device_info_pub{ORB_ID(device_information)};
int _conversion_interval{-1};
device_information_s _device_info{};
bool _device_info_valid{false};
perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")};
perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com err")};
@ -185,7 +182,6 @@ private:
bool _restriction{false};
bool _auto_restriction{false};
bool _prev_restriction{false};
hrt_abstime _last_device_info_pub{0};
};
LightwareLaser::LightwareLaser(const I2CSPIDriverConfig &config) :
@ -428,59 +424,34 @@ int LightwareLaser::configure()
void LightwareLaser::readDeviceInformation()
{
_device_info = {};
_device_info.device_type = device_information_s::DEVICE_TYPE_RANGEFINDER;
_device_info.device_id = get_device_id();
_device_info_pub.get().device_type = device_information_s::DEVICE_TYPE_RANGEFINDER;
_device_info_pub.get().device_id = get_device_id();
strlcpy(_device_info.vendor_name, "LightWare", sizeof(_device_info.vendor_name));
uint8_t buffer[32] = {};
if (readRegister(Register::ProductName, buffer, sizeof(buffer) - 1) == PX4_OK) {
strlcpy(_device_info.model_name, (char *)buffer, sizeof(_device_info.model_name));
} else {
strlcpy(_device_info.model_name, "", sizeof(_device_info.model_name));
}
char string_buffer[17] {};
readRegister(Register::ProductName, (uint8_t *)string_buffer, sizeof(string_buffer) - 1);
snprintf(_device_info_pub.get().name, sizeof(_device_info_pub.get().name), "%s %s", "LightWare", string_buffer);
uint32_t hw_version = 0;
readRegister(Register::HardwareVersion, (uint8_t *)&hw_version, sizeof(hw_version));
snprintf(_device_info_pub.get().hardware_version, sizeof(_device_info_pub.get().hardware_version), "%" PRIu32, hw_version);
if (readRegister(Register::HardwareVersion, (uint8_t *)&hw_version, sizeof(hw_version)) == 0) {
snprintf(_device_info.hardware_version, sizeof(_device_info.hardware_version), "%u", (unsigned int)hw_version);
uint8_t fw_bytes[4] {}; // Datasheet: 4 bytes but forth one is "Reserved"
readRegister(Register::FirmwareVersion, fw_bytes, sizeof(fw_bytes));
snprintf(_device_info_pub.get().firmware_version, sizeof(_device_info_pub.get().firmware_version), "%u.%u.%u", fw_bytes[2], fw_bytes[1],
fw_bytes[0]);
} else {
strlcpy(_device_info.hardware_version, "", sizeof(_device_info.hardware_version));
}
uint8_t fw_bytes[4] = {};
if (readRegister(Register::FirmwareVersion, fw_bytes, sizeof(fw_bytes)) == 0) {
snprintf(_device_info.firmware_version, sizeof(_device_info.firmware_version), "%u.%u.%u",
fw_bytes[2], fw_bytes[1], fw_bytes[0]);
} else {
strlcpy(_device_info.firmware_version, "", sizeof(_device_info.firmware_version));
}
if (readRegister(Register::SerialNumber, buffer, sizeof(buffer) - 1) == 0) {
strlcpy(_device_info.serial_number, (char *)buffer, sizeof(_device_info.serial_number));
} else {
strlcpy(_device_info.serial_number, "", sizeof(_device_info.serial_number));
}
_device_info_valid = true;
readRegister(Register::SerialNumber, (uint8_t *)string_buffer, sizeof(string_buffer) - 1);
strlcpy(_device_info_pub.get().serial_number, string_buffer, sizeof(_device_info_pub.get().serial_number));
}
void LightwareLaser::publishDeviceInformation()
void LightwareLaser::updateDeviceInformation()
{
if (!_device_info_valid) {
return;
}
const hrt_abstime now = hrt_absolute_time();
_device_info.timestamp = hrt_absolute_time();
_device_info_pub.publish(_device_info);
if (_device_info_pub.get().name[0] != '\0' && now >= _device_info_pub.get().timestamp + 3_s) {
_device_info_pub.get().timestamp = now;
_device_info_pub.update();
}
}
int LightwareLaser::collect()
@ -657,13 +628,7 @@ void LightwareLaser::RunImpl()
perf_count(_comms_errors);
}
// Publish device information (1 Hz)
const hrt_abstime now = hrt_absolute_time();
if (now - _last_device_info_pub >= 1_s) {
publishDeviceInformation();
_last_device_info_pub = now;
}
updateDeviceInformation();
switch (_state) {
case State::Configuring: {