fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-18 07:10:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

uavcan: Remove single-GNSS limitation from UavcanGnssBridge

Browse Source
This commit is contained in:
JacobCrabill
2021-01-12 17:50:38 -08:00
committed by Daniel Agar
parent a57b9f9381
commit 917fef546a
3 changed files with 4 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/uavcan/sensors/gnss.cpp
-11
View File
@@ -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{};
/*
src/drivers/uavcan/sensors/gnss.hpp
-1
View File
@@ -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
src/drivers/uavcan/sensors/sensor_bridge.cpp
+4 -4
View File
@@ -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++) {