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 825cbdf97f..5a377e2ab5 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 @@ -56,6 +56,8 @@ #include #include #include +#include +#include using namespace time_literals; @@ -85,6 +87,9 @@ private: enum class Register : uint8_t { // see http://support.lightware.co.za/sf20/#/commands ProductName = 0, + HardwareVersion = 1, + FirmwareVersion = 2, + SerialNumber = 3, DistanceOutput = 27, DistanceData = 44, LaserFiring = 50, @@ -125,6 +130,7 @@ private: int collect(); int updateRestriction(); + void publishDeviceInformation(); PX4Rangefinder _px4_rangefinder; @@ -149,6 +155,9 @@ private: bool _restriction{false}; bool _auto_restriction{false}; bool _prev_restriction{false}; + + uORB::Publication _device_information_pub{ORB_ID(device_information)}; + hrt_abstime _last_timestamp{0}; }; LightwareLaser::LightwareLaser(const I2CSPIDriverConfig &config) : @@ -383,6 +392,9 @@ int LightwareLaser::collect() perf_begin(_sample_perf); OutputData data; const hrt_abstime timestamp_sample = hrt_absolute_time(); + // Publish device information on first successful detection + publishDeviceInformation(); + _last_timestamp = hrt_absolute_time(); if (readRegister(Register::DistanceData, (uint8_t *)&data, sizeof(data)) < 0) { perf_count(_comms_errors); @@ -489,6 +501,73 @@ int LightwareLaser::updateRestriction() return 0; } +void LightwareLaser::publishDeviceInformation() +{ + device_information_s device_info{}; + device_info.timestamp = hrt_absolute_time(); + device_info.device_type = device_information_s::DEVICE_TYPE_RANGEFINDER; + + // Set vendor name to "Lightware" + strlcpy(device_info.vendor_name, "Lightware", sizeof(device_info.vendor_name)); + + // Read product name + uint8_t product_name[16]; + + if (readRegister(Register::ProductName, product_name, sizeof(product_name)) == 0) { + product_name[sizeof(product_name) - 1] = '\0'; + strlcpy(device_info.model_name, (const char *)product_name, sizeof(device_info.model_name)); + } + + // Read hardware version + uint8_t hw_version[4]; + + if (readRegister(Register::HardwareVersion, hw_version, sizeof(hw_version)) == 0) { + uint32_t hw_ver_num = (hw_version[3] << 24) | (hw_version[2] << 16) | + (hw_version[1] << 8) | hw_version[0]; + + snprintf(device_info.hardware_version, sizeof(device_info.hardware_version), + "%lu", (unsigned long)hw_ver_num); + + } else { + strlcpy(device_info.hardware_version, "-1", sizeof(device_info.hardware_version)); + } + + // Read firmware version + uint8_t fw_version[4]; + + if (readRegister(Register::FirmwareVersion, fw_version, sizeof(fw_version)) == 0) { + // According to Lightware spec: + uint8_t patch = fw_version[0]; // Byte 1 = Patch + uint8_t minor = fw_version[1]; // Byte 2 = Minor + uint8_t major = fw_version[2]; // Byte 3 = Major + // fw_version[3] is Byte 4 = Reserved + + snprintf(device_info.firmware_version, sizeof(device_info.firmware_version), + "%u.%u.%u", major, minor, patch); + + } else { + strlcpy(device_info.firmware_version, "-1", sizeof(device_info.firmware_version)); + } + + // Read serial number + uint8_t serial_number[32]; + + if (readRegister(Register::SerialNumber, serial_number, sizeof(serial_number)) == 0) { + serial_number[sizeof(serial_number) - 1] = '\0'; + strncpy(device_info.serial_number, (const char *)serial_number, sizeof(device_info.serial_number)); + + } else { + strncpy(device_info.serial_number, "-1", sizeof(device_info.serial_number)); + } + + _device_information_pub.publish(device_info); + + PX4_DEBUG("Published device information: %s %s, HW: %s, FW: %s, SN: %s", + device_info.vendor_name, device_info.model_name, + device_info.hardware_version, device_info.firmware_version, + device_info.serial_number); +} + void LightwareLaser::RunImpl() { if (PX4_OK != updateRestriction()) { @@ -533,6 +612,14 @@ void LightwareLaser::RunImpl() } + // Publish device information once per second + const hrt_abstime now = hrt_absolute_time(); + + if (now - _last_timestamp >= 1_s) { + publishDeviceInformation(); + _last_timestamp = now; + } + ScheduleDelayed(_conversion_interval); break; }