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 93c4b94b82..3c737a4907 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 @@ -53,9 +53,11 @@ #include #include #include +#include #include #include #include +#include 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_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) {