From 85796fbd84a37f7f7f60da668779aaa6724df513 Mon Sep 17 00:00:00 2001 From: JacobCrabill Date: Tue, 15 Dec 2020 18:28:22 -0800 Subject: [PATCH] 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 | 4 +-- .../distance_sensor/cm8jl65/CM8JL65.cpp | 17 ++++++++++++- .../distance_sensor/leddar_one/LeddarOne.cpp | 16 +++++++++++- .../lightware_laser_i2c.cpp | 3 ++- .../lightware_laser_serial.cpp | 14 ++++++++++- .../lightware_laser_serial.hpp | 1 + .../distance_sensor/ll40ls/LidarLiteI2C.cpp | 2 ++ .../ll40ls_pwm/LidarLitePWM.cpp | 4 ++- .../distance_sensor/mappydot/MappyDot.cpp | 6 +++-- src/drivers/distance_sensor/mb12xx/mb12xx.cpp | 3 ++- src/drivers/distance_sensor/pga460/pga460.cpp | 10 ++++++++ src/drivers/distance_sensor/pga460/pga460.h | 2 ++ src/drivers/distance_sensor/srf02/SRF02.cpp | 2 ++ src/drivers/distance_sensor/srf05/SRF05.cpp | 4 ++- .../distance_sensor/teraranger/TERARANGER.cpp | 3 +++ src/drivers/distance_sensor/tfmini/TFMINI.cpp | 16 +++++++++++- .../ulanding_radar/AerotennaULanding.cpp | 17 ++++++++++++- .../distance_sensor/vl53l0x/VL53L0X.cpp | 4 ++- .../distance_sensor/vl53l1x/vl53l1x.cpp | 4 ++- src/drivers/drv_sensor.h | 7 ++++++ src/drivers/optical_flow/px4flow/px4flow.cpp | 7 ++++-- src/drivers/uavcan/sensors/rangefinder.cpp | 10 ++++---- src/lib/drivers/device/Device.hpp | 1 + .../drivers/rangefinder/PX4Rangefinder.cpp | 17 ++++++------- .../drivers/rangefinder/PX4Rangefinder.hpp | 7 +++--- src/modules/mavlink/mavlink_receiver.cpp | 25 ++++++++++++++++--- 26 files changed, 168 insertions(+), 38 deletions(-) diff --git a/msg/distance_sensor.msg b/msg/distance_sensor.msg index 9cadd5ec7b..dd08e4b589 100644 --- a/msg/distance_sensor.msg +++ b/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 diff --git a/src/drivers/distance_sensor/cm8jl65/CM8JL65.cpp b/src/drivers/distance_sensor/cm8jl65/CM8JL65.cpp index 8f384c1340..7e5894ed82 100644 --- a/src/drivers/distance_sensor/cm8jl65/CM8JL65.cpp +++ b/src/drivers/distance_sensor/cm8jl65/CM8JL65.cpp @@ -35,6 +35,8 @@ #include +#include + 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() diff --git a/src/drivers/distance_sensor/leddar_one/LeddarOne.cpp b/src/drivers/distance_sensor/leddar_one/LeddarOne.cpp index 5b19ce713a..a46d4abeac 100644 --- a/src/drivers/distance_sensor/leddar_one/LeddarOne.cpp +++ b/src/drivers/distance_sensor/leddar_one/LeddarOne.cpp @@ -37,12 +37,26 @@ #include #include +#include + 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); 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 5afbe95b6c..57b720ec6d 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 @@ -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() diff --git a/src/drivers/distance_sensor/lightware_laser_serial/lightware_laser_serial.cpp b/src/drivers/distance_sensor/lightware_laser_serial/lightware_laser_serial.cpp index 6811d26ccb..ec74a38676 100644 --- a/src/drivers/distance_sensor/lightware_laser_serial/lightware_laser_serial.cpp +++ b/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() diff --git a/src/drivers/distance_sensor/lightware_laser_serial/lightware_laser_serial.hpp b/src/drivers/distance_sensor/lightware_laser_serial/lightware_laser_serial.hpp index 2eaa17ba0d..8095256e63 100644 --- a/src/drivers/distance_sensor/lightware_laser_serial/lightware_laser_serial.hpp +++ b/src/drivers/distance_sensor/lightware_laser_serial/lightware_laser_serial.hpp @@ -45,6 +45,7 @@ #include #include #include +#include #include #include diff --git a/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp b/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp index b460313f18..562afbe36d 100644 --- a/src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp +++ b/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() diff --git a/src/drivers/distance_sensor/ll40ls_pwm/LidarLitePWM.cpp b/src/drivers/distance_sensor/ll40ls_pwm/LidarLitePWM.cpp index 9b5148ff05..0d3a4a0754 100644 --- a/src/drivers/distance_sensor/ll40ls_pwm/LidarLitePWM.cpp +++ b/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() diff --git a/src/drivers/distance_sensor/mappydot/MappyDot.cpp b/src/drivers/distance_sensor/mappydot/MappyDot.cpp index efac645092..86a647743a 100644 --- a/src/drivers/distance_sensor/mappydot/MappyDot.cpp +++ b/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]; diff --git a/src/drivers/distance_sensor/mb12xx/mb12xx.cpp b/src/drivers/distance_sensor/mb12xx/mb12xx.cpp index a1805fc678..a94b954d59 100644 --- a/src/drivers/distance_sensor/mb12xx/mb12xx.cpp +++ b/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]; diff --git a/src/drivers/distance_sensor/pga460/pga460.cpp b/src/drivers/distance_sensor/pga460/pga460.cpp index 3d54d74926..e479fc7d50 100644 --- a/src/drivers/distance_sensor/pga460/pga460.cpp +++ b/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; diff --git a/src/drivers/distance_sensor/pga460/pga460.h b/src/drivers/distance_sensor/pga460/pga460.h index 9e3e262c82..c7fa0f23c8 100644 --- a/src/drivers/distance_sensor/pga460/pga460.h +++ b/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 diff --git a/src/drivers/distance_sensor/srf02/SRF02.cpp b/src/drivers/distance_sensor/srf02/SRF02.cpp index 3c8d81fbe8..6b223072c6 100644 --- a/src/drivers/distance_sensor/srf02/SRF02.cpp +++ b/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); } diff --git a/src/drivers/distance_sensor/srf05/SRF05.cpp b/src/drivers/distance_sensor/srf05/SRF05.cpp index 36d3ed66fd..76ba2eb502 100644 --- a/src/drivers/distance_sensor/srf05/SRF05.cpp +++ b/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 diff --git a/src/drivers/distance_sensor/teraranger/TERARANGER.cpp b/src/drivers/distance_sensor/teraranger/TERARANGER.cpp index 538011f62a..507a8460f4 100644 --- a/src/drivers/distance_sensor/teraranger/TERARANGER.cpp +++ b/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() diff --git a/src/drivers/distance_sensor/tfmini/TFMINI.cpp b/src/drivers/distance_sensor/tfmini/TFMINI.cpp index 4832a4b96c..f4b3156245 100644 --- a/src/drivers/distance_sensor/tfmini/TFMINI.cpp +++ b/src/drivers/distance_sensor/tfmini/TFMINI.cpp @@ -33,17 +33,31 @@ #include "TFMINI.hpp" +#include #include 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() diff --git a/src/drivers/distance_sensor/ulanding_radar/AerotennaULanding.cpp b/src/drivers/distance_sensor/ulanding_radar/AerotennaULanding.cpp index b0704083ff..d959f71eb1 100644 --- a/src/drivers/distance_sensor/ulanding_radar/AerotennaULanding.cpp +++ b/src/drivers/distance_sensor/ulanding_radar/AerotennaULanding.cpp @@ -33,9 +33,11 @@ #include "AerotennaULanding.hpp" +#include + 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); } diff --git a/src/drivers/distance_sensor/vl53l0x/VL53L0X.cpp b/src/drivers/distance_sensor/vl53l0x/VL53L0X.cpp index e5890925c8..f15b2dc629 100644 --- a/src/drivers/distance_sensor/vl53l0x/VL53L0X.cpp +++ b/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() diff --git a/src/drivers/distance_sensor/vl53l1x/vl53l1x.cpp b/src/drivers/distance_sensor/vl53l1x/vl53l1x.cpp index ed68d0987c..28418a0ee2 100644 --- a/src/drivers/distance_sensor/vl53l1x/vl53l1x.cpp +++ b/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() diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 2dfbd2ef65..3d9ff54a3c 100644 --- a/src/drivers/drv_sensor.h +++ b/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 diff --git a/src/drivers/optical_flow/px4flow/px4flow.cpp b/src/drivers/optical_flow/px4flow/px4flow.cpp index 4224875c76..c1f9758edf 100644 --- a/src/drivers/optical_flow/px4flow/px4flow.cpp +++ b/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.id = 0; + distance_report.device_id = device_id.devid; distance_report.orientation = _sonar_rotation; _distance_sensor_topic.publish(distance_report); diff --git a/src/drivers/uavcan/sensors/rangefinder.cpp b/src/drivers/uavcan/sensors/rangefinder.cpp index 82387e922b..cff084292c 100644 --- a/src/drivers/uavcan/sensors/rangefinder.cpp +++ b/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); diff --git a/src/lib/drivers/device/Device.hpp b/src/lib/drivers/device/Device.hpp index 45a314d070..a76255f6d0 100644 --- a/src/lib/drivers/device/Device.hpp +++ b/src/lib/drivers/device/Device.hpp @@ -147,6 +147,7 @@ public: DeviceBusType_SPI = 2, DeviceBusType_UAVCAN = 3, DeviceBusType_SIMULATION = 4, + DeviceBusType_SERIAL = 5, }; /* diff --git a/src/lib/drivers/rangefinder/PX4Rangefinder.cpp b/src/lib/drivers/rangefinder/PX4Rangefinder.cpp index 7191be4b0c..1c4e9f37b9 100644 --- a/src/lib/drivers/rangefinder/PX4Rangefinder.cpp +++ b/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 - // union device::Device::DeviceId device_id; - // device_id.devid = _distance_sensor_pub.get().device_id; + // update to new device type + device_id.devid_s.devtype = device_type; - // // update to new device type - // device_id.devid_s.devtype = devtype; - - // // copy back to report - // _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) diff --git a/src/lib/drivers/rangefinder/PX4Rangefinder.hpp b/src/lib/drivers/rangefinder/PX4Rangefinder.hpp index c5cfa522b5..f666594a6e 100644 --- a/src/lib/drivers/rangefinder/PX4Rangefinder.hpp +++ b/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); - //void set_error_count(uint64_t error_count) { _distance_sensor_pub.get().error_count = error_count; } + // Set the MAV_DISTANCE_SENSOR type (LASER, ULTRASOUND, INFRARED, RADAR) + 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; } diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index fc57b9b529..624906c253 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -64,6 +64,8 @@ #include "mavlink_main.h" #include "mavlink_receiver.h" +#include // 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.id = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND; + d.type = 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(dist_sensor.min_distance) * 1e-2f; /* cm to m */ ds.max_distance = static_cast(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