SENS: RNG: Add Device Information publishing to Lightware Rangefinder

src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp

@@ -53,9 +53,11 @@
 #include <drivers/drv_hrt.h>
 #include <drivers/rangefinder/PX4Rangefinder.hpp>
 #include <uORB/Subscription.hpp>
+#include <uORB/Publication.hpp>
 #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;
 
@@ -85,6 +87,9 @@ private:
 	enum class Register : uint8_t {
 		// Common registers
 		ProductName = 0,
+		HardwareVersion = 1,
+		FirmwareVersion = 2,
+		SerialNumber = 3,
 		DistanceData = 44,
 		LaserFiring = 50,
 		Protocol = 120,
@@ -149,10 +154,17 @@ private:
 
 	int updateRestriction();
 
+	void readDeviceInformation();
+	void publishDeviceInformation();
+
 	PX4Rangefinder _px4_rangefinder;
+	uORB::Publication<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")};
 
@@ -173,6 +185,7 @@ private:
 	bool _restriction{false};
 	bool _auto_restriction{false};
 	bool _prev_restriction{false};
+	hrt_abstime _last_device_info_pub{0};
 };
 
 LightwareLaser::LightwareLaser(const I2CSPIDriverConfig &config) :
@@ -368,6 +381,10 @@ int LightwareLaser::configure()
 			const uint8_t cmd6[] = {(uint8_t)Register::LaserFiring, (uint8_t)(_restriction ? 0 : 1)};
 			ret |= transfer(cmd6, sizeof(cmd6), nullptr, 0);
 
+			if (ret == PX4_OK) {
+				readDeviceInformation();
+			}
+
 			return ret;
 		}
 		break;
@@ -386,6 +403,10 @@ int LightwareLaser::configure()
 			const uint8_t cmd6[] = {(uint8_t)Register::LaserFiring, (uint8_t)(_restriction ? 0 : 1)};
 			ret |= transfer(cmd6, sizeof(cmd6), nullptr, 0);
 
+			if (ret == PX4_OK) {
+				readDeviceInformation();
+			}
+
 			return ret;
 		}
 		break;
@@ -405,6 +426,63 @@ int LightwareLaser::configure()
 	return -1;
 }
 
+void LightwareLaser::readDeviceInformation()
+{
+	_device_info = {};
+	_device_info.device_type = device_information_s::DEVICE_TYPE_RANGEFINDER;
+	_device_info.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));
+	}
+
+	uint32_t hw_version = 0;
+
+	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);
+
+	} 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;
+}
+
+void LightwareLaser::publishDeviceInformation()
+{
+	if (!_device_info_valid) {
+		return;
+	}
+
+	_device_info.timestamp = hrt_absolute_time();
+	_device_info_pub.publish(_device_info);
+}
+
 int LightwareLaser::collect()
 {
 	switch (_type) {
@@ -579,6 +657,14 @@ 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;
+	}
+
 	switch (_state) {
 	case State::Configuring: {
 			if (configure() == 0) {