SENS:RNG: add device information publishing

src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp

@@ -56,6 +56,8 @@
 #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>
+#include <uORB/Publication.hpp>
 
 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_s> _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;
 	}