Drivers: Distance Sensors: Add proper device IDs

Add new DeviceBusType_SERIAL to Device::DeviceId union
Add DRV_DIST_DEVTYPE's for all distance sensors
Change distance_sensor_s.id to distance_sensor_s.device_id
Modify all distance_sensor drivers to apply 'proper' device_id

msg/distance_sensor.msg

@@ -2,6 +2,8 @@
 
 uint64 timestamp		# time since system start (microseconds)
 
+uint32 device_id		# unique device ID for the sensor that does not change between power cycles
+
 float32 min_distance		# Minimum distance the sensor can measure (in m)
 float32 max_distance		# Maximum distance the sensor can measure (in m)
 float32 current_distance	# Current distance reading (in m)
@@ -14,8 +16,6 @@ uint8 MAV_DISTANCE_SENSOR_ULTRASOUND = 1
 uint8 MAV_DISTANCE_SENSOR_INFRARED = 2
 uint8 MAV_DISTANCE_SENSOR_RADAR = 3
 
-uint8 id			# Onboard ID of the sensor
-
 float32 h_fov # Sensor horizontal field of view (rad)
 float32 v_fov # Sensor vertical field of view (rad)
 float32[4] q # Quaterion sensor orientation with respect to the vehicle body frame to specify the orientation ROTATION_CUSTOM

src/drivers/distance_sensor/cm8jl65/CM8JL65.cpp

@@ -35,6 +35,8 @@
 
 #include <fcntl.h>
 
+#include <lib/drivers/device/Device.hpp>
+
 static constexpr unsigned char crc_msb_vector[] = {
 	0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
 	0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
@@ -87,7 +89,7 @@ static constexpr unsigned char crc_lsb_vector[] = {
 
 CM8JL65::CM8JL65(const char *port, uint8_t rotation) :
 	ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)),
-	_px4_rangefinder(0 /* TODO: device ids */, rotation)
+	_px4_rangefinder(0, rotation)
 {
 	// Store the port name.
 	strncpy(_port, port, sizeof(_port) - 1);
@@ -95,10 +97,23 @@ CM8JL65::CM8JL65(const char *port, uint8_t rotation) :
 	// Enforce null termination.
 	_port[sizeof(_port) - 1] = '\0';
 
+	device::Device::DeviceId device_id;
+	device_id.devid_s.bus_type = device::Device::DeviceBusType_SERIAL;
+
+	uint8_t bus_num = atoi(&_port[sizeof(_port) - 2]);
+
+	if (bus_num < 10) {
+		device_id.devid_s.bus = bus_num;
+	}
+
+	_px4_rangefinder.set_device_id(device_id.devid);
+
 	// Use conservative distance bounds, to make sure we don't fuse garbage data
 	_px4_rangefinder.set_min_distance(0.2f);	// Datasheet: 0.17m
 	_px4_rangefinder.set_max_distance(7.9f);	// Datasheet: 8.0m
 	_px4_rangefinder.set_fov(0.0488692f);
+	_px4_rangefinder.set_device_type(DRV_DIST_DEVTYPE_CM8JL65);
+	_px4_rangefinder.set_rangefinder_type(distance_sensor_s::MAV_DISTANCE_SENSOR_LASER);
 }
 
 CM8JL65::~CM8JL65()

src/drivers/distance_sensor/leddar_one/LeddarOne.cpp

@@ -37,12 +37,26 @@
 #include <stdlib.h>
 #include <string.h>
 
+#include <lib/drivers/device/Device.hpp>
+
 LeddarOne::LeddarOne(const char *serial_port, uint8_t device_orientation):
 	ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(serial_port)),
-	_px4_rangefinder(0 /* device id not yet used */, device_orientation)
+	_px4_rangefinder(0, device_orientation)
 {
 	_serial_port = strdup(serial_port);
 
+	device::Device::DeviceId device_id;
+	device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SERIAL;
+
+	uint8_t bus_num = atoi(&_serial_port[sizeof(_serial_port) - 2]);
+
+	if (bus_num < 10) {
+		device_id.devid_s.bus = bus_num;
+	}
+
+	_px4_rangefinder.set_device_id(device_id.devid);
+	_px4_rangefinder.set_device_type(DRV_DIST_DEVTYPE_LEDDARONE);
+
 	_px4_rangefinder.set_max_distance(LEDDAR_ONE_MAX_DISTANCE);
 	_px4_rangefinder.set_min_distance(LEDDAR_ONE_MIN_DISTANCE);
 	_px4_rangefinder.set_fov(LEDDAR_ONE_FIELD_OF_VIEW);

src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp

@@ -136,8 +136,9 @@ LightwareLaser::LightwareLaser(I2CSPIBusOption bus_option, const int bus, const
 			       int address) :
 	I2C(DRV_DIST_DEVTYPE_LIGHTWARE_LASER, MODULE_NAME, bus, address, bus_frequency),
 	I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus),
-	_px4_rangefinder(DRV_DIST_DEVTYPE_LIGHTWARE_LASER, rotation)
+	_px4_rangefinder(get_device_id(), rotation)
 {
+	_px4_rangefinder.set_device_type(DRV_DIST_DEVTYPE_LIGHTWARE_LASER);
 }
 
 LightwareLaser::~LightwareLaser()

src/drivers/distance_sensor/lightware_laser_serial/lightware_laser_serial.cpp

@@ -41,7 +41,7 @@
 
 LightwareLaserSerial::LightwareLaserSerial(const char *port, uint8_t rotation) :
 	ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)),
-	_px4_rangefinder(0 /* device id not yet used */, rotation),
+	_px4_rangefinder(0, rotation),
 	_sample_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": read")),
 	_comms_errors(perf_alloc(PC_COUNT, MODULE_NAME": com_err"))
 {
@@ -50,6 +50,18 @@ LightwareLaserSerial::LightwareLaserSerial(const char *port, uint8_t rotation) :
 
 	/* enforce null termination */
 	_port[sizeof(_port) - 1] = '\0';
+
+	device::Device::DeviceId device_id;
+	device_id.devid_s.bus_type = device::Device::DeviceBusType_SERIAL;
+
+	uint8_t bus_num = atoi(&_port[sizeof(_port) - 2]);
+
+	if (bus_num < 10) {
+		device_id.devid_s.bus = bus_num;
+	}
+
+	_px4_rangefinder.set_device_id(device_id.devid);
+	_px4_rangefinder.set_device_type(DRV_DIST_DEVTYPE_LIGHTWARE_LASER);
 }
 
 LightwareLaserSerial::~LightwareLaserSerial()

src/drivers/distance_sensor/lightware_laser_serial/lightware_laser_serial.hpp

@@ -45,6 +45,7 @@
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
 #include <lib/drivers/rangefinder/PX4Rangefinder.hpp>
 #include <drivers/drv_hrt.h>
+#include <drivers/device/device.h>
 #include <lib/parameters/param.h>
 #include <lib/perf/perf_counter.h>
 

src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp

@@ -52,6 +52,8 @@ LidarLiteI2C::LidarLiteI2C(I2CSPIBusOption bus_option, const int bus, const uint
 	_px4_rangefinder.set_fov(0.008); // Divergence 8 mRadian
 	// up the retries since the device misses the first measure attempts
 	_retries = 3;
+
+	_px4_rangefinder.set_device_type(DRV_DIST_DEVTYPE_LL40LS); /// TODO
 }
 
 LidarLiteI2C::~LidarLiteI2C()

src/drivers/distance_sensor/ll40ls_pwm/LidarLitePWM.cpp

@@ -50,12 +50,14 @@
 
 LidarLitePWM::LidarLitePWM(const uint8_t rotation) :
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default),
-	_px4_rangefinder(0 /* device id not yet used */, rotation)
+	_px4_rangefinder(0 /* no device type for PWM input */, rotation)
 {
 	_px4_rangefinder.set_min_distance(LL40LS_MIN_DISTANCE);
 	_px4_rangefinder.set_max_distance(LL40LS_MAX_DISTANCE);
 	_px4_rangefinder.set_fov(0.008); // Divergence 8 mRadian
 	px4_arch_configgpio(io_timer_channel_get_gpio_output(GPIO_VDD_RANGEFINDER_EN_CHAN));
+
+	_px4_rangefinder.set_device_type(DRV_DIST_DEVTYPE_LL40LS);
 }
 
 LidarLitePWM::~LidarLitePWM()

src/drivers/distance_sensor/mappydot/MappyDot.cpp

@@ -225,7 +225,9 @@ MappyDot::MappyDot(I2CSPIBusOption bus_option, const int bus, int bus_frequency)
 	I2C(DRV_DIST_DEVTYPE_MAPPYDOT, MODULE_NAME, bus, MAPPYDOT_BASE_ADDR, bus_frequency),
 	ModuleParams(nullptr),
 	I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus)
-{}
+{
+	set_device_type(DRV_DIST_DEVTYPE_MAPPYDOT);
+}
 
 MappyDot::~MappyDot()
 {
@@ -266,7 +268,7 @@ MappyDot::collect()
 
 		distance_sensor_s report {};
 		report.current_distance = distance_m;
-		report.id               = _sensor_addresses[index];
+		report.device_id        = get_device_id();
 		report.max_distance     = MAPPYDOT_MAX_DISTANCE;
 		report.min_distance     = MAPPYDOT_MIN_DISTANCE;
 		report.orientation      = _sensor_rotations[index];

src/drivers/distance_sensor/mb12xx/mb12xx.cpp

@@ -180,6 +180,7 @@ MB12XX::MB12XX(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int
 	ModuleParams(nullptr),
 	I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, address)
 {
+	set_device_type(DRV_DIST_DEVTYPE_MB12XX);
 }
 
 MB12XX::~MB12XX()
@@ -222,7 +223,7 @@ MB12XX::collect()
 
 	distance_sensor_s report;
 	report.current_distance = distance_m;
-	report.id               = _sensor_addresses[_sensor_index];
+	report.device_id        = get_device_id();
 	report.max_distance     = MB12XX_MAX_DISTANCE;
 	report.min_distance     = MB12XX_MIN_DISTANCE;
 	report.orientation      = _sensor_rotations[_sensor_index];

src/drivers/distance_sensor/pga460/pga460.cpp

@@ -47,6 +47,15 @@ PGA460::PGA460(const char *port)
 	strncpy(_port, port, sizeof(_port) - 1);
 	// Enforce null termination.
 	_port[sizeof(_port) - 1] = '\0';
+
+	_device_id.devid_s.devtype = DRV_DIST_DEVTYPE_PGA460;
+	_device_id.devid_s.bus_type = device::Device::DeviceBusType_SERIAL;
+
+	uint8_t bus_num = atoi(&_port[sizeof(_port) - 2]);
+
+	if (bus_num < 10) {
+		_device_id.devid_s.bus = bus_num;
+	}
 }
 
 PGA460::~PGA460()
@@ -763,6 +772,7 @@ void PGA460::uORB_publish_results(const float object_distance)
 {
 	struct distance_sensor_s report = {};
 	report.timestamp = hrt_absolute_time();
+	report.device_id = _device_id.devid;
 	report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND;
 	report.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
 	report.current_distance = object_distance;

src/drivers/distance_sensor/pga460/pga460.h

@@ -402,6 +402,8 @@ private:
 
 	/** @param _start_loop The starting value for the loop time of the main loop. */
 	uint64_t _start_loop{0};
+
+	device::Device::DeviceId _device_id;
 };
 
 #endif

src/drivers/distance_sensor/srf02/SRF02.cpp

@@ -38,6 +38,8 @@ SRF02::SRF02(I2CSPIBusOption bus_option, const int bus, const uint8_t rotation,
 	I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus),
 	_px4_rangefinder(get_device_id(), rotation)
 {
+	_px4_rangefinder.set_device_type(DRV_DIST_DEVTYPE_SRF02);
+	_px4_rangefinder.set_rangefinder_type(distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND);
 	_px4_rangefinder.set_max_distance(SRF02_MAX_DISTANCE);
 	_px4_rangefinder.set_min_distance(SRF02_MIN_DISTANCE);
 }

src/drivers/distance_sensor/srf05/SRF05.cpp

@@ -48,8 +48,10 @@
 
 SRF05::SRF05(const uint8_t rotation) :
 	ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::hp_default),
-	_px4_rangefinder(0 /* device id not yet used */, rotation)
+	_px4_rangefinder(0 /* no device type for GPIO input */, rotation)
 {
+	_px4_rangefinder.set_device_type(DRV_DIST_DEVTYPE_SRF05);
+	_px4_rangefinder.set_rangefinder_type(distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND);
 	_px4_rangefinder.set_min_distance(HXSRX0X_MIN_DISTANCE);
 	_px4_rangefinder.set_max_distance(HXSRX0X_MAX_DISTANCE);
 	_px4_rangefinder.set_fov(0.261799); // 15 degree FOV

src/drivers/distance_sensor/teraranger/TERARANGER.cpp

@@ -80,6 +80,9 @@ TERARANGER::TERARANGER(I2CSPIBusOption bus_option, const int bus, const uint8_t
 {
 	// up the retries since the device misses the first measure attempts
 	I2C::_retries = 3;
+
+	_px4_rangefinder.set_device_type(DRV_DIST_DEVTYPE_TERARANGER);
+	_px4_rangefinder.set_rangefinder_type(distance_sensor_s::MAV_DISTANCE_SENSOR_LASER);
 }
 
 TERARANGER::~TERARANGER()

src/drivers/distance_sensor/tfmini/TFMINI.cpp

@@ -33,17 +33,31 @@
 
 #include "TFMINI.hpp"
 
+#include <lib/drivers/device/Device.hpp>
 #include <fcntl.h>
 
 TFMINI::TFMINI(const char *port, uint8_t rotation) :
 	ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)),
-	_px4_rangefinder(0 /* TODO: device id */, rotation)
+	_px4_rangefinder(0, rotation)
 {
 	// store port name
 	strncpy(_port, port, sizeof(_port) - 1);
 
 	// enforce null termination
 	_port[sizeof(_port) - 1] = '\0';
+
+	device::Device::DeviceId device_id;
+	device_id.devid_s.devtype = DRV_DIST_DEVTYPE_TFMINI;
+	device_id.devid_s.bus_type = device::Device::DeviceBusType_SERIAL;
+
+	uint8_t bus_num = atoi(&_port[sizeof(_port) - 2]);
+
+	if (bus_num < 10) {
+		device_id.devid_s.bus = bus_num;
+	}
+
+	_px4_rangefinder.set_device_id(device_id.devid);
+	_px4_rangefinder.set_rangefinder_type(distance_sensor_s::MAV_DISTANCE_SENSOR_LASER);
 }
 
 TFMINI::~TFMINI()

src/drivers/distance_sensor/ulanding_radar/AerotennaULanding.cpp

@@ -33,9 +33,11 @@
 
 #include "AerotennaULanding.hpp"
 
+#include <lib/drivers/device/Device.hpp>
+
 AerotennaULanding::AerotennaULanding(const char *port, uint8_t rotation) :
 	ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(port)),
-	_px4_rangefinder(0 /* TODO: device ids */, rotation)
+	_px4_rangefinder(0, rotation)
 {
 	/* store port name */
 	strncpy(_port, port, sizeof(_port) - 1);
@@ -43,6 +45,19 @@ AerotennaULanding::AerotennaULanding(const char *port, uint8_t rotation) :
 	/* enforce null termination */
 	_port[sizeof(_port) - 1] = '\0';
 
+	device::Device::DeviceId device_id;
+	device_id.devid_s.bus_type = device::Device::DeviceBusType_SERIAL;
+
+	uint8_t bus_num = atoi(&_port[sizeof(_port) - 2]);
+
+	if (bus_num < 10) {
+		device_id.devid_s.bus = bus_num;
+	}
+
+	_px4_rangefinder.set_device_id(device_id.devid);
+	_px4_rangefinder.set_device_type(DRV_DIST_DEVTYPE_ULANDING);
+	_px4_rangefinder.set_rangefinder_type(distance_sensor_s::MAV_DISTANCE_SENSOR_RADAR);
+
 	_px4_rangefinder.set_min_distance(ULANDING_MIN_DISTANCE);
 	_px4_rangefinder.set_max_distance(ULANDING_MAX_DISTANCE);
 }

src/drivers/distance_sensor/vl53l0x/VL53L0X.cpp

@@ -62,7 +62,7 @@
 VL53L0X::VL53L0X(I2CSPIBusOption bus_option, const int bus, const uint8_t rotation, int bus_frequency, int address) :
 	I2C(DRV_DIST_DEVTYPE_VL53L0X, MODULE_NAME, bus, address, bus_frequency),
 	I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus),
-	_px4_rangefinder(0 /* device id not yet used */, rotation)
+	_px4_rangefinder(get_device_id(), rotation)
 {
 	// VL53L0X typical range 0-2 meters with 25 degree field of view
 	_px4_rangefinder.set_min_distance(0.f);
@@ -71,6 +71,8 @@ VL53L0X::VL53L0X(I2CSPIBusOption bus_option, const int bus, const uint8_t rotati
 
 	// Allow 3 retries as the device typically misses the first measure attempts.
 	I2C::_retries = 3;
+
+	_px4_rangefinder.set_device_type(DRV_DIST_DEVTYPE_VL53L0X);
 }
 
 VL53L0X::~VL53L0X()

src/drivers/distance_sensor/vl53l1x/vl53l1x.cpp

@@ -143,7 +143,7 @@ static const uint8_t status_rtn[24] = { 255, 255, 255, 5, 2, 4, 1, 7, 3, 0,
 VL53L1X::VL53L1X(I2CSPIBusOption bus_option, const int bus, const uint8_t rotation, int bus_frequency, int address) :
 	I2C(DRV_DIST_DEVTYPE_VL53L1X, MODULE_NAME, bus, address, bus_frequency),
 	I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus),
-	_px4_rangefinder(0 /* device id not yet used */, rotation)
+	_px4_rangefinder(get_device_id(), rotation)
 {
 	// VL53L1X typical range 0-2 meters with 25 degree field of view
 	_px4_rangefinder.set_min_distance(0.f);
@@ -152,6 +152,8 @@ VL53L1X::VL53L1X(I2CSPIBusOption bus_option, const int bus, const uint8_t rotati
 
 	// Allow 3 retries as the device typically misses the first measure attempts.
 	I2C::_retries = 3;
+
+	_px4_rangefinder.set_device_type(DRV_DIST_DEVTYPE_VL53L1X);
 }
 
 VL53L1X::~VL53L1X()

src/drivers/drv_sensor.h

@@ -162,6 +162,13 @@
 
 #define DRV_ADC_DEVTYPE_ADS1115	0x90
 #define DRV_DIST_DEVTYPE_VL53L1X 0x91
+#define DRV_DIST_DEVTYPE_CM8JL65 0x92
+#define DRV_DIST_DEVTYPE_LEDDARONE 0x93
+#define DRV_DIST_DEVTYPE_MAVLINK 0x94
+#define DRV_DIST_DEVTYPE_PGA460 0x95
+#define DRV_DIST_DEVTYPE_PX4FLOW 0x96
+#define DRV_DIST_DEVTYPE_TFMINI 0x97
+#define DRV_DIST_DEVTYPE_ULANDING 0x98
 
 #define DRV_GPIO_DEVTYPE_MCP23009	0x99
 

src/drivers/optical_flow/px4flow/px4flow.cpp

@@ -313,6 +313,10 @@ PX4FLOW::collect()
 	/* publish to the distance_sensor topic as well */
 	if (_distance_sensor_topic.get_instance() == 0) {
 		distance_sensor_s distance_report{};
+		DeviceId device_id;
+		device_id.devid = get_device_id();
+		device_id.devid_s.devtype = DRV_DIST_DEVTYPE_PX4FLOW;
+
 		distance_report.timestamp = report.timestamp;
 		distance_report.min_distance = PX4FLOW_MIN_DISTANCE;
 		distance_report.max_distance = PX4FLOW_MAX_DISTANCE;
@@ -320,8 +324,7 @@ PX4FLOW::collect()
 		distance_report.variance = 0.0f;
 		distance_report.signal_quality = -1;
 		distance_report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND;
-		/* TODO: the ID needs to be properly set */
+		distance_report.device_id = device_id.devid;
-		distance_report.id = 0;
 		distance_report.orientation = _sonar_rotation;
 
 		_distance_sensor_topic.publish(distance_report);

src/drivers/uavcan/sensors/rangefinder.cpp

@@ -82,25 +82,25 @@ void UavcanRangefinderBridge::range_sub_cb(const
 
 	if (!_inited) {
 
-		uint8_t device_type = 0;
+		uint8_t rangefinder_type = 0;
 
 		switch (msg.sensor_type) {
 		case uavcan::equipment::range_sensor::Measurement::SENSOR_TYPE_SONAR:
-			device_type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND;
+			rangefinder_type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND;
 			break;
 
 		case uavcan::equipment::range_sensor::Measurement::SENSOR_TYPE_RADAR:
-			device_type = distance_sensor_s::MAV_DISTANCE_SENSOR_RADAR;
+			rangefinder_type = distance_sensor_s::MAV_DISTANCE_SENSOR_RADAR;
 			break;
 
 		case uavcan::equipment::range_sensor::Measurement::SENSOR_TYPE_LIDAR:
 		case uavcan::equipment::range_sensor::Measurement::SENSOR_TYPE_UNDEFINED:
 		default:
-			device_type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
+			rangefinder_type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
 			break;
 		}
 
-		rangefinder->set_device_type(device_type);
+		rangefinder->set_rangefinder_type(rangefinder_type);
 		rangefinder->set_fov(msg.field_of_view);
 		rangefinder->set_min_distance(_range_min_m);
 		rangefinder->set_max_distance(_range_max_m);

src/lib/drivers/device/Device.hpp

@@ -147,6 +147,7 @@ public:
 		DeviceBusType_SPI     = 2,
 		DeviceBusType_UAVCAN  = 3,
 		DeviceBusType_SIMULATION = 4,
+		DeviceBusType_SERIAL = 5,
 	};
 
 	/*

src/lib/drivers/rangefinder/PX4Rangefinder.cpp

@@ -41,6 +41,7 @@ PX4Rangefinder::PX4Rangefinder(const uint32_t device_id, const uint8_t device_or
 
 	set_device_id(device_id);
 	set_orientation(device_orientation);
+	set_rangefinder_type(distance_sensor_s::MAV_DISTANCE_SENSOR_LASER); // Default to type LASER
 }
 
 PX4Rangefinder::~PX4Rangefinder()
@@ -50,17 +51,15 @@ PX4Rangefinder::~PX4Rangefinder()
 
 void PX4Rangefinder::set_device_type(uint8_t device_type)
 {
-	// TODO: range finders should have device ids
+	// current DeviceStructure
+	union device::Device::DeviceId device_id;
+	device_id.devid = _distance_sensor_pub.get().device_id;
 
-	// // current DeviceStructure
+	// update to new device type
-	// union device::Device::DeviceId device_id;
+	device_id.devid_s.devtype = device_type;
-	// device_id.devid = _distance_sensor_pub.get().device_id;
 
-	// // update to new device type
+	// copy back to report
-	// device_id.devid_s.devtype = devtype;
+	_distance_sensor_pub.get().device_id = device_id.devid;
-
-	// // copy back to report
-	// _distance_sensor_pub.get().device_id = device_id.devid;
 }
 
 void PX4Rangefinder::set_orientation(const uint8_t device_orientation)

src/lib/drivers/rangefinder/PX4Rangefinder.hpp

@@ -45,10 +45,11 @@ public:
 		       const uint8_t device_orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
 	~PX4Rangefinder();
 
-	void set_device_type(uint8_t device_type);
+	// Set the MAV_DISTANCE_SENSOR type (LASER, ULTRASOUND, INFRARED, RADAR)
-	//void set_error_count(uint64_t error_count) { _distance_sensor_pub.get().error_count = error_count; }
+	void set_rangefinder_type(uint8_t rangefinder_type) { _distance_sensor_pub.get().type = rangefinder_type; };
 
-	void set_device_id(const uint8_t device_id) { _distance_sensor_pub.get().id = device_id; };
+	void set_device_id(const uint8_t device_id) { _distance_sensor_pub.get().device_id = device_id; };
+	void set_device_type(const uint8_t device_type);
 
 	void set_fov(const float fov) { set_hfov(fov); set_vfov(fov); }
 	void set_hfov(const float fov) { _distance_sensor_pub.get().h_fov = fov; }

src/modules/mavlink/mavlink_receiver.cpp

@@ -64,6 +64,8 @@
 #include "mavlink_main.h"
 #include "mavlink_receiver.h"
 
+#include <lib/drivers/device/Device.hpp> // For DeviceId union
+
 #ifdef CONFIG_NET
 #define MAVLINK_RECEIVER_NET_ADDED_STACK 1360
 #else
@@ -640,12 +642,17 @@ MavlinkReceiver::handle_message_optical_flow_rad(mavlink_message_t *msg)
 
 		distance_sensor_s d{};
 
+		device::Device::DeviceId device_id;
+		device_id.devid_s.bus = device::Device::DeviceBusType::DeviceBusType_SERIAL;
+		device_id.devid_s.devtype = DRV_DIST_DEVTYPE_MAVLINK;
+		device_id.devid_s.address = msg->sysid;
+
 		d.timestamp = f.timestamp;
 		d.min_distance = 0.3f;
 		d.max_distance = 5.0f;
 		d.current_distance = flow.distance; /* both are in m */
-		d.type = 1;
+		d.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND;
-		d.id = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND;
+		d.device_id = device_id.devid;
 		d.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
 		d.variance = 0.0;
 
@@ -680,12 +687,17 @@ MavlinkReceiver::handle_message_hil_optical_flow(mavlink_message_t *msg)
 	/* Use distance value for distance sensor topic */
 	distance_sensor_s d{};
 
+	device::Device::DeviceId device_id;
+	device_id.devid_s.bus = device::Device::DeviceBusType::DeviceBusType_SERIAL;
+	device_id.devid_s.devtype = DRV_DIST_DEVTYPE_MAVLINK;
+	device_id.devid_s.address = msg->sysid;
+
 	d.timestamp = hrt_absolute_time();
 	d.min_distance = 0.3f;
 	d.max_distance = 5.0f;
 	d.current_distance = flow.distance; /* both are in m */
 	d.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
-	d.id = 0;
+	d.device_id = device_id.devid;
 	d.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
 	d.variance = 0.0;
 
@@ -729,6 +741,11 @@ MavlinkReceiver::handle_message_distance_sensor(mavlink_message_t *msg)
 
 	distance_sensor_s ds{};
 
+	device::Device::DeviceId device_id;
+	device_id.devid_s.bus = device::Device::DeviceBusType::DeviceBusType_SERIAL;
+	device_id.devid_s.devtype = DRV_DIST_DEVTYPE_MAVLINK;
+	device_id.devid_s.address = dist_sensor.id;
+
 	ds.timestamp        = hrt_absolute_time(); /* Use system time for now, don't trust sender to attach correct timestamp */
 	ds.min_distance     = static_cast<float>(dist_sensor.min_distance) * 1e-2f;     /* cm to m */
 	ds.max_distance     = static_cast<float>(dist_sensor.max_distance) * 1e-2f;     /* cm to m */
@@ -741,7 +758,7 @@ MavlinkReceiver::handle_message_distance_sensor(mavlink_message_t *msg)
 	ds.q[2]             = dist_sensor.quaternion[2];
 	ds.q[3]             = dist_sensor.quaternion[3];
 	ds.type             = dist_sensor.type;
-	ds.id               = dist_sensor.id;
+	ds.device_id        = device_id.devid;
 	ds.orientation      = dist_sensor.orientation;
 
 	// MAVLink DISTANCE_SENSOR signal_quality value of 0 means unset/unknown