Browse Source

uavcan: Remove single-GNSS limitation from UavcanGnssBridge

release/1.12
JacobCrabill 4 years ago committed by Daniel Agar
parent
commit
917fef546a
  1. 11
      src/drivers/uavcan/sensors/gnss.cpp
  2. 1
      src/drivers/uavcan/sensors/gnss.hpp
  3. 8
      src/drivers/uavcan/sensors/sensor_bridge.cpp

11
src/drivers/uavcan/sensors/gnss.cpp

@ -282,17 +282,6 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType> @@ -282,17 +282,6 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure<FixType>
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{};
/*

1
src/drivers/uavcan/sensors/gnss.hpp

@ -122,7 +122,6 @@ private: @@ -122,7 +122,6 @@ private:
uORB::Subscription _orb_inject_data_sub{ORB_ID(gps_inject_data)};
uORB::PublicationMulti<sensor_gps_s> _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

8
src/drivers/uavcan/sensors/sensor_bridge.cpp

@ -91,7 +91,7 @@ UavcanSensorBridgeBase::publish(const int node_id, const void *report) @@ -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) @@ -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) @@ -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 @@ -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++) {

Loading…
Cancel
Save