diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 61dbb2f35c4d..32ec379ac92c 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -184,6 +184,7 @@ #define DRV_IMU_DEVTYPE_UAVCAN 0x87 #define DRV_MAG_DEVTYPE_UAVCAN 0x88 #define DRV_DIST_DEVTYPE_UAVCAN 0x89 +#define DRV_HYGRO_DEVTYPE_UAVCAN 0x8A #define DRV_ADC_DEVTYPE_ADS1115 0x90 diff --git a/src/drivers/uavcan/sensors/accel.cpp b/src/drivers/uavcan/sensors/accel.cpp index b725b978bd1d..0efa5c5e6ed0 100644 --- a/src/drivers/uavcan/sensors/accel.cpp +++ b/src/drivers/uavcan/sensors/accel.cpp @@ -43,7 +43,9 @@ const char *const UavcanAccelBridge::NAME = "accel"; UavcanAccelBridge::UavcanAccelBridge(uavcan::INode &node, NodeInfoPublisher *node_info_publisher) : UavcanSensorBridgeBase("uavcan_accel", ORB_ID(sensor_accel), node_info_publisher), _sub_imu_data(node) -{ } +{ + set_device_type(DRV_ACC_DEVTYPE_UAVCAN); +} int UavcanAccelBridge::init() { @@ -59,7 +61,7 @@ int UavcanAccelBridge::init() void UavcanAccelBridge::imu_sub_cb(const uavcan::ReceivedDataStructure &msg) { - uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get()); + uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get(), msg.getIfaceIndex()); const hrt_abstime timestamp_sample = hrt_absolute_time(); @@ -87,13 +89,10 @@ void UavcanAccelBridge::imu_sub_cb(const uavcan::ReceivedDataStructure(channel->node_id); + // Build device ID using node_id and interface index + uint32_t device_id = make_uavcan_device_id(static_cast(channel->node_id), channel->iface_index); - channel->h_driver = new PX4Accelerometer(device_id.devid); + channel->h_driver = new PX4Accelerometer(device_id); if (channel->h_driver == nullptr) { return PX4_ERROR; diff --git a/src/drivers/uavcan/sensors/baro.cpp b/src/drivers/uavcan/sensors/baro.cpp index 2ef69eb6b40e..51c8a4b89966 100644 --- a/src/drivers/uavcan/sensors/baro.cpp +++ b/src/drivers/uavcan/sensors/baro.cpp @@ -49,7 +49,9 @@ UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode &node, NodeInfoPublis UavcanSensorBridgeBase("uavcan_baro", ORB_ID(sensor_baro), node_info_publisher), _sub_air_pressure_data(node), _sub_air_temperature_data(node) -{ } +{ + set_device_type(DRV_BARO_DEVTYPE_UAVCAN); +} int UavcanBarometerBridge::init() { @@ -91,7 +93,7 @@ void UavcanBarometerBridge::air_pressure_sub_cb(const { const hrt_abstime timestamp_sample = hrt_absolute_time(); - uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get()); + uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get(), msg.getIfaceIndex()); if (channel == nullptr) { // Something went wrong - no channel to publish on; return @@ -105,23 +107,18 @@ void UavcanBarometerBridge::air_pressure_sub_cb(const return; } - DeviceId device_id{}; - device_id.devid_s.bus = 0; - device_id.devid_s.bus_type = DeviceBusType_UAVCAN; - - device_id.devid_s.devtype = DRV_BARO_DEVTYPE_UAVCAN; - device_id.devid_s.address = static_cast(channel->node_id); + uint32_t device_id = make_uavcan_device_id(msg); // Register barometer capability with NodeInfoPublisher after first successful message if (_node_info_publisher != nullptr) { - _node_info_publisher->registerDeviceCapability(msg.getSrcNodeID().get(), device_id.devid, + _node_info_publisher->registerDeviceCapability(msg.getSrcNodeID().get(), device_id, NodeInfoPublisher::DeviceCapability::BAROMETER); } // publish sensor_baro_s sensor_baro{}; sensor_baro.timestamp_sample = timestamp_sample; - sensor_baro.device_id = device_id.devid; + sensor_baro.device_id = device_id; sensor_baro.pressure = msg.static_pressure; if (PX4_ISFINITE(_last_temperature_kelvin) && (_last_temperature_kelvin >= 0.f)) { diff --git a/src/drivers/uavcan/sensors/differential_pressure.cpp b/src/drivers/uavcan/sensors/differential_pressure.cpp index 7099a55dc4a4..107547a121ee 100644 --- a/src/drivers/uavcan/sensors/differential_pressure.cpp +++ b/src/drivers/uavcan/sensors/differential_pressure.cpp @@ -49,6 +49,7 @@ UavcanDifferentialPressureBridge::UavcanDifferentialPressureBridge(uavcan::INode UavcanSensorBridgeBase("uavcan_differential_pressure", ORB_ID(differential_pressure), node_info_publisher), _sub_air(node) { + set_device_type(DRV_DIFF_PRESS_DEVTYPE_UAVCAN); } int UavcanDifferentialPressureBridge::init() @@ -68,8 +69,6 @@ void UavcanDifferentialPressureBridge::air_sub_cb(const { const hrt_abstime timestamp_sample = hrt_absolute_time(); - _device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_UAVCAN; - _device_id.devid_s.address = msg.getSrcNodeID().get() & 0xFF; float diff_press_pa = msg.differential_pressure; int32_t differential_press_rev = 0; param_get(param_find("SENS_DPRES_REV"), &differential_press_rev); @@ -83,7 +82,7 @@ void UavcanDifferentialPressureBridge::air_sub_cb(const differential_pressure_s report{}; report.timestamp_sample = timestamp_sample; - report.device_id = _device_id.devid; + report.device_id = make_uavcan_device_id(msg); report.differential_pressure_pa = diff_press_pa; report.temperature = temperature_c; report.timestamp = hrt_absolute_time(); diff --git a/src/drivers/uavcan/sensors/flow.cpp b/src/drivers/uavcan/sensors/flow.cpp index cda437970eac..001b78566d71 100644 --- a/src/drivers/uavcan/sensors/flow.cpp +++ b/src/drivers/uavcan/sensors/flow.cpp @@ -41,6 +41,7 @@ UavcanFlowBridge::UavcanFlowBridge(uavcan::INode &node, NodeInfoPublisher *node_ UavcanSensorBridgeBase("uavcan_flow", ORB_ID(sensor_optical_flow), node_info_publisher), _sub_flow(node) { + set_device_type(DRV_FLOW_DEVTYPE_UAVCAN); } int @@ -61,13 +62,7 @@ void UavcanFlowBridge::flow_sub_cb(const uavcan::ReceivedDataStructure const uint8_t spoofing_state) { sensor_gps_s sensor_gps{}; - sensor_gps.device_id = get_device_id(); + + sensor_gps.device_id = make_uavcan_device_id(msg); // Register GPS capability with NodeInfoPublisher after first successful message if (_node_info_publisher != nullptr) { diff --git a/src/drivers/uavcan/sensors/gnss_relative.cpp b/src/drivers/uavcan/sensors/gnss_relative.cpp index b2d3960404ad..8cb5bf344ebc 100644 --- a/src/drivers/uavcan/sensors/gnss_relative.cpp +++ b/src/drivers/uavcan/sensors/gnss_relative.cpp @@ -42,6 +42,7 @@ UavcanGnssRelativeBridge::UavcanGnssRelativeBridge(uavcan::INode &node, NodeInfo UavcanSensorBridgeBase("uavcan_gnss_relative", ORB_ID(sensor_gnss_relative), node_info_publisher), _sub_rel_pos_heading(node) { + set_device_type(DRV_GPS_DEVTYPE_UAVCAN); } int @@ -70,7 +71,7 @@ void UavcanGnssRelativeBridge::rel_pos_heading_sub_cb(const sensor_gnss_relative.position_length = msg.relative_distance_m; sensor_gnss_relative.position[2] = msg.relative_down_pos_m; - sensor_gnss_relative.device_id = get_device_id(); + sensor_gnss_relative.device_id = make_uavcan_device_id(msg); // Register GPS capability with NodeInfoPublisher after first successful message if (_node_info_publisher != nullptr) { diff --git a/src/drivers/uavcan/sensors/gyro.cpp b/src/drivers/uavcan/sensors/gyro.cpp index 2f9ab4a9f5d7..ab7269386653 100644 --- a/src/drivers/uavcan/sensors/gyro.cpp +++ b/src/drivers/uavcan/sensors/gyro.cpp @@ -43,7 +43,9 @@ const char *const UavcanGyroBridge::NAME = "gyro"; UavcanGyroBridge::UavcanGyroBridge(uavcan::INode &node, NodeInfoPublisher *node_info_publisher) : UavcanSensorBridgeBase("uavcan_gyro", ORB_ID(sensor_gyro), node_info_publisher), _sub_imu_data(node) -{ } +{ + set_device_type(DRV_GYR_DEVTYPE_UAVCAN); +} int UavcanGyroBridge::init() { @@ -59,7 +61,7 @@ int UavcanGyroBridge::init() void UavcanGyroBridge::imu_sub_cb(const uavcan::ReceivedDataStructure &msg) { - uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get()); + uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get(), msg.getIfaceIndex()); if (channel == nullptr) { // Something went wrong - no channel to publish on; return @@ -87,13 +89,10 @@ void UavcanGyroBridge::imu_sub_cb(const uavcan::ReceivedDataStructure(channel->node_id); + // Build device ID using node_id and interface index + uint32_t device_id = make_uavcan_device_id(static_cast(channel->node_id), channel->iface_index); - channel->h_driver = new PX4Gyroscope(device_id.devid); + channel->h_driver = new PX4Gyroscope(device_id); if (channel->h_driver == nullptr) { return PX4_ERROR; diff --git a/src/drivers/uavcan/sensors/hygrometer.cpp b/src/drivers/uavcan/sensors/hygrometer.cpp index d19b286e9463..d9975b226e5d 100755 --- a/src/drivers/uavcan/sensors/hygrometer.cpp +++ b/src/drivers/uavcan/sensors/hygrometer.cpp @@ -42,6 +42,7 @@ UavcanHygrometerBridge::UavcanHygrometerBridge(uavcan::INode &node, NodeInfoPubl UavcanSensorBridgeBase("uavcan_hygrometer_sensor", ORB_ID(sensor_hygrometer), node_info_publisher), _sub_hygro(node) { + set_device_type(DRV_HYGRO_DEVTYPE_UAVCAN); } int UavcanHygrometerBridge::init() @@ -64,7 +65,7 @@ void UavcanHygrometerBridge::hygro_sub_cb(const uavcan::ReceivedDataStructure &msg) { - uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get()); + uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get(), msg.getIfaceIndex()); if (channel == nullptr) { // Something went wrong - no channel to publish on; return @@ -104,7 +105,7 @@ void UavcanMagnetometerBridge::mag2_sub_cb(const uavcan::ReceivedDataStructure &msg) { - uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get()); + uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get(), msg.getIfaceIndex()); if (channel == nullptr || channel->instance < 0) { // Something went wrong - no channel to publish on; return @@ -134,13 +135,10 @@ UavcanMagnetometerBridge::mag2_sub_cb(const int UavcanMagnetometerBridge::init_driver(uavcan_bridge::Channel *channel) { - // update device id as we now know our device node_id - DeviceId device_id{_device_id}; + // Build device ID using node_id and interface index + uint32_t device_id = make_uavcan_device_id(static_cast(channel->node_id), channel->iface_index); - device_id.devid_s.devtype = DRV_MAG_DEVTYPE_UAVCAN; - device_id.devid_s.address = static_cast(channel->node_id); - - channel->h_driver = new PX4Magnetometer(device_id.devid, ROTATION_NONE); + channel->h_driver = new PX4Magnetometer(device_id, ROTATION_NONE); if (channel->h_driver == nullptr) { return PX4_ERROR; diff --git a/src/drivers/uavcan/sensors/rangefinder.cpp b/src/drivers/uavcan/sensors/rangefinder.cpp index 1222aad1ea7e..b476c6694770 100644 --- a/src/drivers/uavcan/sensors/rangefinder.cpp +++ b/src/drivers/uavcan/sensors/rangefinder.cpp @@ -45,7 +45,9 @@ const char *const UavcanRangefinderBridge::NAME = "rangefinder"; UavcanRangefinderBridge::UavcanRangefinderBridge(uavcan::INode &node, NodeInfoPublisher *node_info_publisher) : UavcanSensorBridgeBase("uavcan_rangefinder", ORB_ID(distance_sensor), node_info_publisher), _sub_range_data(node) -{ } +{ + set_device_type(DRV_DIST_DEVTYPE_UAVCAN); +} int UavcanRangefinderBridge::init() { @@ -66,7 +68,7 @@ int UavcanRangefinderBridge::init() void UavcanRangefinderBridge::range_sub_cb(const uavcan::ReceivedDataStructure &msg) { - uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get()); + uavcan_bridge::Channel *channel = get_channel_for_node(msg.getSrcNodeID().get(), msg.getIfaceIndex()); if (channel == nullptr || channel->instance < 0) { // Something went wrong - no channel to publish on; return @@ -125,13 +127,10 @@ void UavcanRangefinderBridge::range_sub_cb(const int UavcanRangefinderBridge::init_driver(uavcan_bridge::Channel *channel) { - // update device id as we now know our device node_id - DeviceId device_id{_device_id}; - - device_id.devid_s.devtype = DRV_DIST_DEVTYPE_UAVCAN; - device_id.devid_s.address = static_cast(channel->node_id); + // Build device ID using node_id and interface index + uint32_t device_id = make_uavcan_device_id(static_cast(channel->node_id), channel->iface_index); - channel->h_driver = new PX4Rangefinder(device_id.devid, distance_sensor_s::ROTATION_DOWNWARD_FACING); + channel->h_driver = new PX4Rangefinder(device_id, distance_sensor_s::ROTATION_DOWNWARD_FACING); if (channel->h_driver == nullptr) { return PX4_ERROR; diff --git a/src/drivers/uavcan/sensors/sensor_bridge.cpp b/src/drivers/uavcan/sensors/sensor_bridge.cpp index ec9f7c913180..350dd9a625c0 100644 --- a/src/drivers/uavcan/sensors/sensor_bridge.cpp +++ b/src/drivers/uavcan/sensors/sensor_bridge.cpp @@ -316,7 +316,7 @@ UavcanSensorBridgeBase::publish(const int node_id, const void *report) (void)orb_publish(_orb_topic, channel->orb_advert, report); } -uavcan_bridge::Channel *UavcanSensorBridgeBase::get_channel_for_node(int node_id) +uavcan_bridge::Channel *UavcanSensorBridgeBase::get_channel_for_node(int node_id, uint8_t iface_index) { uavcan_bridge::Channel *channel = nullptr; @@ -354,6 +354,7 @@ uavcan_bridge::Channel *UavcanSensorBridgeBase::get_channel_for_node(int node_id // initialize the driver, which registers the class device name and uORB publisher channel->node_id = node_id; + channel->iface_index = iface_index; int ret = init_driver(channel); if (ret != PX4_OK) { diff --git a/src/drivers/uavcan/sensors/sensor_bridge.hpp b/src/drivers/uavcan/sensors/sensor_bridge.hpp index 9c59b816dcb5..e8fb1a124b40 100644 --- a/src/drivers/uavcan/sensors/sensor_bridge.hpp +++ b/src/drivers/uavcan/sensors/sensor_bridge.hpp @@ -92,6 +92,7 @@ struct Channel { orb_advert_t orb_advert{nullptr}; int instance{-1}; void *h_driver{nullptr}; + uint8_t iface_index{0}; }; } // namespace uavcan_bridge @@ -138,7 +139,34 @@ class UavcanSensorBridgeBase : public IUavcanSensorBridge, public device::Device */ virtual int init_driver(uavcan_bridge::Channel *channel) { return PX4_OK; }; - uavcan_bridge::Channel *get_channel_for_node(int node_id); + uavcan_bridge::Channel *get_channel_for_node(int node_id, uint8_t iface_index); + + /** + * Builds a unique device ID from a UAVCAN message + * @param msg UAVCAN message (must have getSrcNodeID() and getIfaceIndex() methods) + * @return Complete device ID with node address and interface encoded + */ + template + uint32_t make_uavcan_device_id(const uavcan::ReceivedDataStructure &msg) const + { + return make_uavcan_device_id(msg.getSrcNodeID().get(), msg.getIfaceIndex()); + } + + /** + * Builds a unique device ID from node ID and interface index + * @param node_id UAVCAN node ID + * @param iface_index CAN interface index (0 = CAN1, 1 = CAN2, etc.) + * @return Complete device ID with node address and interface encoded + */ + uint32_t make_uavcan_device_id(uint8_t node_id, uint8_t iface_index) const + { + device::Device::DeviceId device_id{}; + device_id.devid_s.devtype = get_device_type(); + device_id.devid_s.address = node_id; + device_id.devid_s.bus_type = device::Device::DeviceBusType_UAVCAN; + device_id.devid_s.bus = iface_index; + return device_id.devid; + } public: virtual ~UavcanSensorBridgeBase(); diff --git a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp index 417001846c8b..27ff75574159 100644 --- a/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp +++ b/src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp @@ -36,6 +36,7 @@ #include #include #include +#include namespace sensors { @@ -95,7 +96,12 @@ void VehicleGPSPosition::ParametersUpdate(bool force) _gps_blending.setBlendingUseHPosAccuracy(_param_sens_gps_mask.get() & BLEND_MASK_USE_HPOS_ACC); _gps_blending.setBlendingUseVPosAccuracy(_param_sens_gps_mask.get() & BLEND_MASK_USE_VPOS_ACC); _gps_blending.setBlendingTimeConstant(_param_sens_gps_tau.get()); - _gps_blending.setPrimaryInstance(_param_sens_gps_prime.get()); + + const int gps_prime = _param_sens_gps_prime.get(); + + if (math::isInRange(gps_prime, -1, 1)) { + _gps_blending.setPrimaryInstance(gps_prime); + } } } @@ -113,6 +119,7 @@ void VehicleGPSPosition::Run() // Check all GPS instance bool any_gps_updated = false; bool gps_updated = false; + const int32_t gps_prime = _param_sens_gps_prime.get(); for (uint8_t i = 0; i < GPS_MAX_RECEIVERS; i++) { gps_updated = _sensor_gps_sub[i].updated(); @@ -125,6 +132,16 @@ void VehicleGPSPosition::Run() _sensor_gps_sub[i].copy(&gps_data); _gps_blending.setGpsData(gps_data, i); + if (math::isInRange(static_cast(gps_prime), 2, 127)) { + device::Device::DeviceId device_id{}; + device_id.devid = gps_data.device_id; + + if (device_id.devid_s.bus_type == device::Device::DeviceBusType_UAVCAN + && device_id.devid_s.address == static_cast(gps_prime)) { + _gps_blending.setPrimaryInstance(i); + } + } + if (!_sensor_gps_sub[i].registered()) { _sensor_gps_sub[i].registerCallback(); } diff --git a/src/modules/sensors/vehicle_gps_position/params.c b/src/modules/sensors/vehicle_gps_position/params.c index 651e6e15fa5f..310c7bfc6b88 100644 --- a/src/modules/sensors/vehicle_gps_position/params.c +++ b/src/modules/sensors/vehicle_gps_position/params.c @@ -70,13 +70,16 @@ PARAM_DEFINE_FLOAT(SENS_GPS_TAU, 10.0f); * send data to the EKF even if a secondary instance is already available. * The secondary instance is then only used if the primary one times out. * - * To have an equal priority of all the instances, set this parameter to -1 and - * the best receiver will be used. + * Accepted values: + * -1 : Auto (equal priority for all instances) + * 0 : Main serial GPS instance + * 1 : Secondary serial GPS instance + * 2-127 : UAVCAN module node ID * * This parameter has no effect if blending is active. * * @group Sensors * @min -1 - * @max 1 + * @max 127 */ PARAM_DEFINE_INT32(SENS_GPS_PRIME, 0);