dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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
This commit is contained in:
JacobCrabill 2020-12-15 18:28:22 -08:00 committed by Daniel Agar
parent adf2d2f73a
commit 85796fbd84
26 changed files with 168 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
msg/distance_sensor.msg
View File

@ -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

17
src/drivers/distance_sensor/cm8jl65/CM8JL65.cpp
View File

@ -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()

16
src/drivers/distance_sensor/leddar_one/LeddarOne.cpp
View File

@ -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);

3
src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp
View File

@ -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()

14
src/drivers/distance_sensor/lightware_laser_serial/lightware_laser_serial.cpp
View File

@ -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()

1
src/drivers/distance_sensor/lightware_laser_serial/lightware_laser_serial.hpp
View File

@ -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>

2
src/drivers/distance_sensor/ll40ls/LidarLiteI2C.cpp
View File

@ -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()

4
src/drivers/distance_sensor/ll40ls_pwm/LidarLitePWM.cpp
View File

@ -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()

6
src/drivers/distance_sensor/mappydot/MappyDot.cpp
View File

@ -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];

3
src/drivers/distance_sensor/mb12xx/mb12xx.cpp
View File

@ -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];

10
src/drivers/distance_sensor/pga460/pga460.cpp
View File

@ -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;

2
src/drivers/distance_sensor/pga460/pga460.h
View File

@ -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

2
src/drivers/distance_sensor/srf02/SRF02.cpp
View File

@ -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);
}

4
src/drivers/distance_sensor/srf05/SRF05.cpp
View File

@ -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

3
src/drivers/distance_sensor/teraranger/TERARANGER.cpp
View File

@ -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()

16
src/drivers/distance_sensor/tfmini/TFMINI.cpp
View File

@ -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()

17
src/drivers/distance_sensor/ulanding_radar/AerotennaULanding.cpp
View File

@ -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);
}

4
src/drivers/distance_sensor/vl53l0x/VL53L0X.cpp
View File

@ -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()

4
src/drivers/distance_sensor/vl53l1x/vl53l1x.cpp
View File

@ -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()

7
src/drivers/drv_sensor.h
View File

@ -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

7
src/drivers/optical_flow/px4flow/px4flow.cpp
View File

@ -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);

10
src/drivers/uavcan/sensors/rangefinder.cpp
View File

@ -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);

1
src/lib/drivers/device/Device.hpp
View File

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

17
src/lib/drivers/rangefinder/PX4Rangefinder.cpp
View File

@ -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)

7
src/lib/drivers/rangefinder/PX4Rangefinder.hpp
View File

@ -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; }

25
src/modules/mavlink/mavlink_receiver.cpp
View File

@ -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.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<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