From 917fef546a5332a4c3886e2079c370c5bd61e8f3 Mon Sep 17 00:00:00 2001 From: JacobCrabill Date: Tue, 12 Jan 2021 17:50:38 -0800 Subject: [PATCH] uavcan: Remove single-GNSS limitation from UavcanGnssBridge --- src/drivers/uavcan/sensors/gnss.cpp | 11 ----------- src/drivers/uavcan/sensors/gnss.hpp | 1 - src/drivers/uavcan/sensors/sensor_bridge.cpp | 8 ++++---- 3 files changed, 4 insertions(+), 16 deletions(-) diff --git a/src/drivers/uavcan/sensors/gnss.cpp b/src/drivers/uavcan/sensors/gnss.cpp index a4e0f97c5c..8dcc8d4741 100644 --- a/src/drivers/uavcan/sensors/gnss.cpp +++ b/src/drivers/uavcan/sensors/gnss.cpp @@ -282,17 +282,6 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure const float (&pos_cov)[9], const float (&vel_cov)[9], const bool valid_pos_cov, const bool valid_vel_cov) { - // This bridge does not support redundant GNSS receivers yet. - if (_receiver_node_id < 0) { - _receiver_node_id = msg.getSrcNodeID().get(); - PX4_INFO("GNSS receiver node ID: %d", _receiver_node_id); - - } else { - if (_receiver_node_id != msg.getSrcNodeID().get()) { - return; // This GNSS receiver is the redundant one, ignore it. - } - } - sensor_gps_s report{}; /* diff --git a/src/drivers/uavcan/sensors/gnss.hpp b/src/drivers/uavcan/sensors/gnss.hpp index 0405738cc9..d0d2c33777 100644 --- a/src/drivers/uavcan/sensors/gnss.hpp +++ b/src/drivers/uavcan/sensors/gnss.hpp @@ -122,7 +122,6 @@ private: uORB::Subscription _orb_inject_data_sub{ORB_ID(gps_inject_data)}; uORB::PublicationMulti _gps_pub{ORB_ID(sensor_gps)}; - int _receiver_node_id{-1}; bool _old_fix_subscriber_active{true}; orb_advert_t _report_pub; ///< uORB pub for gnss position diff --git a/src/drivers/uavcan/sensors/sensor_bridge.cpp b/src/drivers/uavcan/sensors/sensor_bridge.cpp index fbd2d070e0..9d65d49955 100644 --- a/src/drivers/uavcan/sensors/sensor_bridge.cpp +++ b/src/drivers/uavcan/sensors/sensor_bridge.cpp @@ -91,7 +91,7 @@ UavcanSensorBridgeBase::publish(const int node_id, const void *report) return; // Give up immediately - saves some CPU time } - DEVICE_LOG("adding channel %d...", node_id); + DEVICE_LOG("adding channel for topic %s node %d...", _orb_topic->o_name, node_id); // Search for the first free channel for (unsigned i = 0; i < _max_channels; i++) { @@ -115,7 +115,7 @@ UavcanSensorBridgeBase::publish(const int node_id, const void *report) channel->orb_advert = orb_advertise_multi(_orb_topic, report, &channel->instance); channel->node_id = node_id; - DEVICE_LOG("channel %d instance %d ok", channel->node_id, channel->instance); + DEVICE_LOG("node %d instance %d ok", channel->node_id, channel->instance); if (channel->orb_advert == nullptr) { DEVICE_LOG("uORB advertise failed. Out of instances?"); @@ -124,7 +124,7 @@ UavcanSensorBridgeBase::publish(const int node_id, const void *report) return; } - DEVICE_LOG("channel %d instance %d ok", channel->node_id, channel->instance); + DEVICE_LOG("node %d topic %s instance %d ok", channel->node_id, _orb_topic->o_name, channel->instance); } assert(channel != nullptr); @@ -151,7 +151,7 @@ uavcan_bridge::Channel *UavcanSensorBridgeBase::get_channel_for_node(int node_id return channel; } - DEVICE_LOG("adding channel %d...", node_id); + DEVICE_LOG("adding channel for topic %s node %d...", _orb_topic->o_name, node_id); // Search for the first free channel for (unsigned i = 0; i < _max_channels; i++) {