From 71b8aba3e3986a346d76c54ec3dd0d2eba56d538 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Tue, 31 Mar 2026 19:31:10 +0200 Subject: [PATCH] refactor(lightware_laser): simplify device_information handling - publish every 3 seconds - adjust to name instead of vendor and model - buffer size adjusted according to datasheet - switch to PublicationData --- .../lightware_laser_i2c.cpp | 85 ++++++------------- 1 file changed, 25 insertions(+), 60 deletions(-) diff --git a/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp b/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp index 3c737a4907..78217b745f 100644 --- a/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp +++ b/src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp @@ -54,10 +54,10 @@ #include #include #include +#include #include #include #include -#include using namespace time_literals; @@ -154,17 +154,14 @@ private: int updateRestriction(); - void readDeviceInformation(); - void publishDeviceInformation(); - PX4Rangefinder _px4_rangefinder; - uORB::Publication _device_info_pub{ORB_ID(device_information)}; + + void readDeviceInformation(); + void updateDeviceInformation(); + uORB::PublicationData _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: {