From b4d93df4506ce5630fd2329605ae033dfdb43ac2 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 5 Apr 2015 11:51:58 +0300 Subject: [PATCH] TransferSender is now capable of broadcasting in passive mode; Frame::isValid() was modified to accept SFT broadcasts with zero SNID --- .../include/uavcan/transport/dispatcher.hpp | 1 + libuavcan/include/uavcan/transport/frame.hpp | 2 +- .../uavcan/transport/transfer_sender.hpp | 9 ++++++ libuavcan/src/transport/uc_frame.cpp | 7 +++-- .../src/transport/uc_transfer_sender.cpp | 28 +++++++++++++++++-- libuavcan/test/transport/frame.cpp | 13 ++++++++- libuavcan/test/transport/transfer_sender.cpp | 15 +++++++++- 7 files changed, 67 insertions(+), 8 deletions(-) diff --git a/libuavcan/include/uavcan/transport/dispatcher.hpp b/libuavcan/include/uavcan/transport/dispatcher.hpp index ac12bbd2b7..a671f4ac85 100644 --- a/libuavcan/include/uavcan/transport/dispatcher.hpp +++ b/libuavcan/include/uavcan/transport/dispatcher.hpp @@ -115,6 +115,7 @@ public: : canio_(driver, allocator, sysclock) , sysclock_(sysclock) , outgoing_transfer_reg_(otr) + , self_node_id_(NodeID::Broadcast) // Default , self_node_id_is_set_(false) { } diff --git a/libuavcan/include/uavcan/transport/frame.hpp b/libuavcan/include/uavcan/transport/frame.hpp index 49d1da2214..5d149ba837 100644 --- a/libuavcan/include/uavcan/transport/frame.hpp +++ b/libuavcan/include/uavcan/transport/frame.hpp @@ -50,7 +50,7 @@ public: { UAVCAN_ASSERT((transfer_type == TransferTypeMessageBroadcast) == dst_node_id.isBroadcast()); UAVCAN_ASSERT(data_type_id.isValid()); - UAVCAN_ASSERT(src_node_id != dst_node_id); + UAVCAN_ASSERT(src_node_id.isUnicast() ? (src_node_id != dst_node_id) : true); UAVCAN_ASSERT(frame_index <= MaxIndex); } diff --git a/libuavcan/include/uavcan/transport/transfer_sender.hpp b/libuavcan/include/uavcan/transport/transfer_sender.hpp index 27ab8a1038..0ffc232ebf 100644 --- a/libuavcan/include/uavcan/transport/transfer_sender.hpp +++ b/libuavcan/include/uavcan/transport/transfer_sender.hpp @@ -25,6 +25,7 @@ class UAVCAN_EXPORT TransferSender const TransferCRC crc_base_; CanIOFlags flags_; uint8_t iface_mask_; + bool allow_broadcasting_in_passive_mode_; Dispatcher& dispatcher_; @@ -46,6 +47,7 @@ public: , crc_base_(data_type.getSignature().toTransferCRC()) , flags_(CanIOFlags(0)) , iface_mask_(AllIfacesMask) + , allow_broadcasting_in_passive_mode_(false) , dispatcher_(dispatcher) { } @@ -59,6 +61,13 @@ public: iface_mask_ = iface_mask; } + /** + * By default, this class will return an error on any attempt to publish a message while the + * dispatcher is configured in passive mode. This method allows to permanently enable sending + * broadcast transfers in passive mode for this class instance. + */ + void allowBroadcastingInPassiveMode() { allow_broadcasting_in_passive_mode_ = true; } + /** * Send with explicit Transfer ID. * Should be used only for service responses, where response TID should match request TID. diff --git a/libuavcan/src/transport/uc_frame.cpp b/libuavcan/src/transport/uc_frame.cpp index 3a3938a01b..3a6f6ac814 100644 --- a/libuavcan/src/transport/uc_frame.cpp +++ b/libuavcan/src/transport/uc_frame.cpp @@ -170,9 +170,12 @@ bool Frame::isValid() const const bool invalid = (frame_index_ > MaxIndex) || ((frame_index_ == MaxIndex) && !last_frame_) || - (!src_node_id_.isUnicast()) || + (!src_node_id_.isValid()) || (!dst_node_id_.isValid()) || - (src_node_id_ == dst_node_id_) || + (src_node_id_.isUnicast() ? (src_node_id_ == dst_node_id_) : false) || + (src_node_id_.isBroadcast() + ? (!last_frame_ || (frame_index_ > 0) || (transfer_type_ != TransferTypeMessageBroadcast)) + : false) || ((transfer_type_ == TransferTypeMessageBroadcast) != dst_node_id_.isBroadcast()) || (transfer_type_ >= NumTransferTypes) || (static_cast(payload_len_) > getMaxPayloadLen()) || diff --git a/libuavcan/src/transport/uc_transfer_sender.cpp b/libuavcan/src/transport/uc_transfer_sender.cpp index 1fda8c6a46..38766ddc9b 100644 --- a/libuavcan/src/transport/uc_transfer_sender.cpp +++ b/libuavcan/src/transport/uc_transfer_sender.cpp @@ -19,15 +19,37 @@ int TransferSender::send(const uint8_t* payload, unsigned payload_len, Monotonic MonotonicTime blocking_deadline, TransferType transfer_type, NodeID dst_node_id, TransferID tid) { + if (payload_len > MaxTransferPayloadLen) + { + UAVCAN_ASSERT(0); + return -ErrInvalidParam; + } + + Frame frame(data_type_.getID(), transfer_type, dispatcher_.getNodeID(), dst_node_id, 0, tid); + UAVCAN_TRACE("TransferSender", "%s", frame.toString().c_str()); + + /* + * Checking if we're allowed to send. In passive mode we can send only if: + * - Passive broadcasting is enabled + * - Transfer type is broadcast + * - Transfer payload fits one CAN frame + */ if (dispatcher_.isPassiveMode()) { - return -ErrPassiveMode; + const bool allow = allow_broadcasting_in_passive_mode_ && + (transfer_type == TransferTypeMessageBroadcast) && + (int(payload_len) <= frame.getMaxPayloadLen()); + if (!allow) + { + return -ErrPassiveMode; + } } dispatcher_.getTransferPerfCounter().addTxTransfer(); - Frame frame(data_type_.getID(), transfer_type, dispatcher_.getNodeID(), dst_node_id, 0, tid); - + /* + * Sending frames + */ if (frame.getMaxPayloadLen() >= int(payload_len)) // Single Frame Transfer { const int res = frame.setPayload(payload, payload_len); diff --git a/libuavcan/test/transport/frame.cpp b/libuavcan/test/transport/frame.cpp index 5f55033f91..719dcf325b 100644 --- a/libuavcan/test/transport/frame.cpp +++ b/libuavcan/test/transport/frame.cpp @@ -162,7 +162,18 @@ TEST(Frame, FrameParsing) can.id = CanFrame::FlagEFF | // cppcheck-suppress duplicateExpression (2 << 0) | (1 << 3) | (0 << 4) | (0 << 10) | (uavcan::TransferTypeMessageUnicast << 17) | (456 << 19); - ASSERT_FALSE(frame.parse(can)); // Broadcast Src Node ID + ASSERT_FALSE(frame.parse(can)); // Broadcast Src Node ID with unicast transfer + + can.id = CanFrame::FlagEFF | // cppcheck-suppress duplicateExpression + (2 << 0) | (0 << 3) | (0 << 4) | (0 << 10) | (uavcan::TransferTypeMessageBroadcast << 17) | (456 << 19); + ASSERT_FALSE(frame.parse(can)); // Broadcast Src Node ID with multiframe broadcast transfer + + /* + * Broadcast SNID exceptions + */ + can.id = CanFrame::FlagEFF | // cppcheck-suppress duplicateExpression + (2 << 0) | (1 << 3) | (0 << 4) | (0 << 10) | (uavcan::TransferTypeMessageBroadcast << 17) | (456 << 19); + ASSERT_TRUE(frame.parse(can)); // Broadcast Src Node ID with single frame broadcast transfer } diff --git a/libuavcan/test/transport/transfer_sender.cpp b/libuavcan/test/transport/transfer_sender.cpp index 5d65943511..4719543ec4 100644 --- a/libuavcan/test/transport/transfer_sender.cpp +++ b/libuavcan/test/transport/transfer_sender.cpp @@ -235,11 +235,24 @@ TEST(TransferSender, PassiveMode) static const uint8_t Payload[] = {1, 2, 3, 4, 5}; + // By default, sending in passive mode is not enabled ASSERT_EQ(-uavcan::ErrPassiveMode, sender.send(Payload, sizeof(Payload), tsMono(1000), uavcan::MonotonicTime(), uavcan::TransferTypeMessageBroadcast, uavcan::NodeID::Broadcast)); + // Overriding the default + sender.allowBroadcastingInPassiveMode(); + + // OK, now we can broadcast in any mode + ASSERT_LE(0, sender.send(Payload, sizeof(Payload), tsMono(1000), uavcan::MonotonicTime(), + uavcan::TransferTypeMessageBroadcast, uavcan::NodeID::Broadcast)); + + // ...but not unicast or anything else + ASSERT_EQ(-uavcan::ErrPassiveMode, + sender.send(Payload, sizeof(Payload), tsMono(1000), uavcan::MonotonicTime(), + uavcan::TransferTypeMessageUnicast, uavcan::NodeID(42))); + EXPECT_EQ(0, dispatcher.getTransferPerfCounter().getErrorCount()); - EXPECT_EQ(0, dispatcher.getTransferPerfCounter().getTxTransferCount()); + EXPECT_EQ(1, dispatcher.getTransferPerfCounter().getTxTransferCount()); EXPECT_EQ(0, dispatcher.getTransferPerfCounter().getRxTransferCount()); }