From 71e4a36ba4faeaec28f3d2f7e794ce5bf4de5975 Mon Sep 17 00:00:00 2001 From: JacobCrabill Date: Wed, 26 Feb 2020 16:03:53 -0800 Subject: [PATCH] UAVCAN: GNSS sensor bridge: Support multiple pubs UavcanGnssBridge did not support more than 1 GNSS callback/publisher. This has now been fixed; it works the same as the baro, mag, and flow sensor bridges. The EKF2 still doesn't support more than 2 GPS publishers, however. --- src/drivers/uavcan/sensors/gnss.cpp | 81 ++++++++------------ src/drivers/uavcan/sensors/gnss.hpp | 14 ++-- src/drivers/uavcan/sensors/sensor_bridge.cpp | 17 +++- src/drivers/uavcan/sensors/sensor_bridge.hpp | 8 +- 4 files changed, 61 insertions(+), 59 deletions(-) diff --git a/src/drivers/uavcan/sensors/gnss.cpp b/src/drivers/uavcan/sensors/gnss.cpp index e0c384783a..f9a153022b 100644 --- a/src/drivers/uavcan/sensors/gnss.cpp +++ b/src/drivers/uavcan/sensors/gnss.cpp @@ -52,19 +52,36 @@ using namespace time_literals; const char *const UavcanGnssBridge::NAME = "gnss"; UavcanGnssBridge::UavcanGnssBridge(uavcan::INode &node) : + UavcanCDevSensorBridgeBase("uavcan_gnss", "/dev/uavcan/gnss", "/dev/gnss", ORB_ID(vehicle_gps_position)), _node(node), _sub_auxiliary(node), _sub_fix(node), _sub_fix2(node), _pub_fix2(node), - _orb_to_uavcan_pub_timer(node, TimerCbBinder(this, &UavcanGnssBridge::broadcast_from_orb)) + _orb_to_uavcan_pub_timer(node, TimerCbBinder(this, &UavcanGnssBridge::broadcast_from_orb)), + _report_pub(nullptr), + _channel_using_fix2(new bool[_max_channels]) { + for (uint8_t i = 0; i < _max_channels; i++) { + _channel_using_fix2[i] = false; + } +} + +UavcanGnssBridge::~UavcanGnssBridge() +{ + delete [] _channel_using_fix2; } int UavcanGnssBridge::init() { - int res = _pub_fix2.init(uavcan::TransferPriority::MiddleLower); + int res = device::CDev::init(); + + if (res < 0) { + return res; + } + + res = _pub_fix2.init(uavcan::TransferPriority::MiddleLower); if (res < 0) { PX4_WARN("GNSS fix2 pub failed %i", res); @@ -97,26 +114,6 @@ UavcanGnssBridge::init() return res; } -unsigned -UavcanGnssBridge::get_num_redundant_channels() const -{ - return (_receiver_node_id < 0) ? 0 : 1; -} - -void -UavcanGnssBridge::print_status() const -{ - printf("RX errors: %d, using old Fix: %d, receiver node id: ", - _sub_fix.getFailureCount(), int(_old_fix_subscriber_active)); - - if (_receiver_node_id < 0) { - printf("N/A\n"); - - } else { - printf("%d\n", _receiver_node_id); - } -} - void UavcanGnssBridge::gnss_auxiliary_sub_cb(const uavcan::ReceivedDataStructure &msg) { @@ -129,6 +126,14 @@ UavcanGnssBridge::gnss_auxiliary_sub_cb(const uavcan::ReceivedDataStructure &msg) { + // Check to see if this node is also publishing a Fix2 message. + // If so, ignore the old "Fix" message for this node. + const int8_t ch = get_channel_index_for_node(msg.getSrcNodeID().get()); + + if (ch > -1 && _channel_using_fix2[ch]) { + return; + } + const bool valid_pos_cov = !msg.position_covariance.empty(); const bool valid_vel_cov = !msg.velocity_covariance.empty(); @@ -144,11 +149,11 @@ UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure &msg) { - if (_old_fix_subscriber_active) { - PX4_INFO("GNSS Fix2 message detected, disabling support for the old Fix message"); - _sub_fix.stop(); - _old_fix_subscriber_active = false; - _receiver_node_id = -1; + const int8_t ch = get_channel_index_for_node(msg.getSrcNodeID().get()); + + if (ch > -1 && !_channel_using_fix2[ch]) { + PX4_WARN("GNSS Fix2 msg detected for ch %d; disabling Fix msg for this node", ch); + _channel_using_fix2[ch] = true; } float pos_cov[9] {}; @@ -263,18 +268,7 @@ 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. - } - } - - vehicle_gps_position_s report{}; + auto report = ::vehicle_gps_position_s(); /* * FIXME HACK @@ -400,14 +394,7 @@ void UavcanGnssBridge::process_fixx(const uavcan::ReceivedDataStructure report.heading = NAN; report.heading_offset = NAN; - // Publish to a multi-topic - _gps_pub.publish(report); - - // Doing less time critical stuff here - if (_orb_to_uavcan_pub_timer.isRunning()) { - _orb_to_uavcan_pub_timer.stop(); - PX4_INFO("GNSS ORB->UAVCAN bridge stopped, because there are other GNSS publishers"); - } + publish(msg.getSrcNodeID().get(), &report); } void diff --git a/src/drivers/uavcan/sensors/gnss.hpp b/src/drivers/uavcan/sensors/gnss.hpp index cef060148c..24a934b5df 100644 --- a/src/drivers/uavcan/sensors/gnss.hpp +++ b/src/drivers/uavcan/sensors/gnss.hpp @@ -55,7 +55,7 @@ #include "sensor_bridge.hpp" -class UavcanGnssBridge : public IUavcanSensorBridge +class UavcanGnssBridge : public UavcanCDevSensorBridgeBase { static constexpr unsigned ORB_TO_UAVCAN_FREQUENCY_HZ = 10; @@ -63,16 +63,12 @@ public: static const char *const NAME; UavcanGnssBridge(uavcan::INode &node); - ~UavcanGnssBridge() = default; + ~UavcanGnssBridge(); const char *get_name() const override { return NAME; } int init() override; - unsigned get_num_redundant_channels() const override; - - void print_status() const override; - private: /** * GNSS fix message will be reported via this callback. @@ -123,6 +119,10 @@ private: int _receiver_node_id{-1}; bool _old_fix_subscriber_active{true}; - bool _system_clock_set{false}; ///< Have we set the system clock at least once from GNSS data? + orb_advert_t _report_pub; ///< uORB pub for gnss position + + bool _system_clock_set{false}; ///< Have we set the system clock at least once from GNSS data? + + bool *_channel_using_fix2; ///< Flag for whether each channel is using Fix2 or Fix msg }; diff --git a/src/drivers/uavcan/sensors/sensor_bridge.cpp b/src/drivers/uavcan/sensors/sensor_bridge.cpp index b20a3dc1e9..e9ea0ad643 100644 --- a/src/drivers/uavcan/sensors/sensor_bridge.cpp +++ b/src/drivers/uavcan/sensors/sensor_bridge.cpp @@ -154,8 +154,21 @@ UavcanCDevSensorBridgeBase::get_num_redundant_channels() const return out; } -void -UavcanCDevSensorBridgeBase::print_status() const +int8_t UavcanCDevSensorBridgeBase::get_channel_index_for_node(int node_id) +{ + int8_t ch = -1; + + for (unsigned i = 0; i < _max_channels; i++) { + if (_channels[i].node_id == node_id) { + ch = i; + break; + } + } + + return ch; +} + +void UavcanCDevSensorBridgeBase::print_status() const { printf("devname: %s\n", _class_devname); diff --git a/src/drivers/uavcan/sensors/sensor_bridge.hpp b/src/drivers/uavcan/sensors/sensor_bridge.hpp index 921c358493..8ec20f6797 100644 --- a/src/drivers/uavcan/sensors/sensor_bridge.hpp +++ b/src/drivers/uavcan/sensors/sensor_bridge.hpp @@ -94,7 +94,6 @@ class UavcanCDevSensorBridgeBase : public IUavcanSensorBridge, public device::CD int orb_instance = -1; }; - const unsigned _max_channels; const char *const _class_devname; const orb_id_t _orb_topic; Channel *const _channels; @@ -102,15 +101,16 @@ class UavcanCDevSensorBridgeBase : public IUavcanSensorBridge, public device::CD protected: static constexpr unsigned DEFAULT_MAX_CHANNELS = 5; + const unsigned _max_channels; UavcanCDevSensorBridgeBase(const char *name, const char *devname, const char *class_devname, const orb_id_t orb_topic_sensor, const unsigned max_channels = DEFAULT_MAX_CHANNELS) : device::CDev(name, devname), - _max_channels(max_channels), _class_devname(class_devname), _orb_topic(orb_topic_sensor), - _channels(new Channel[max_channels]) + _channels(new Channel[max_channels]), + _max_channels(max_channels) { _device_id.devid_s.bus_type = DeviceBusType_UAVCAN; _device_id.devid_s.bus = 0; @@ -129,5 +129,7 @@ public: unsigned get_num_redundant_channels() const override; + int8_t get_channel_index_for_node(int node_id); + void print_status() const override; };