mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-07-07 18:40:35 +08:00
TransferSender implementation and tests
This commit is contained in:
Pavel Kirienko
@@ -0,0 +1,55 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <cstdlib>
|
||||
#include <cassert>
|
||||
#include <uavcan/internal/data_type.hpp>
|
||||
#include <uavcan/internal/transport/crc.hpp>
|
||||
#include <uavcan/internal/transport/transfer.hpp>
|
||||
#include <uavcan/internal/transport/dispatcher.hpp>
|
||||
#include <uavcan/internal/transport/transfer_buffer.hpp>
|
||||
|
||||
namespace uavcan
|
||||
{
|
||||
|
||||
class TransferSender
|
||||
{
|
||||
static const uint64_t DEFAULT_MAX_TRANSFER_INTERVAL = 60 * 1000 * 1000;
|
||||
|
||||
const uint64_t max_transfer_interval_;
|
||||
const DataTypeDescriptor& data_type_;
|
||||
const CanTxQueue::Qos qos_;
|
||||
const Crc16 crc_base_;
|
||||
|
||||
Dispatcher& dispatcher_;
|
||||
|
||||
public:
|
||||
TransferSender(Dispatcher& dispatcher, const DataTypeDescriptor& data_type, CanTxQueue::Qos qos,
|
||||
uint64_t max_transfer_interval = DEFAULT_MAX_TRANSFER_INTERVAL)
|
||||
: max_transfer_interval_(max_transfer_interval)
|
||||
, data_type_(data_type)
|
||||
, qos_(qos)
|
||||
, crc_base_(data_type.hash.value, DataTypeHash::NUM_BYTES)
|
||||
, dispatcher_(dispatcher)
|
||||
{ }
|
||||
|
||||
/**
|
||||
* Send with explicit Transfer ID.
|
||||
* Should be used only for service responses, where response TID should match request TID.
|
||||
*/
|
||||
int send(const uint8_t* payload, int payload_len, uint64_t monotonic_tx_deadline,
|
||||
uint64_t monotonic_blocking_deadline, TransferType transfer_type, NodeID dst_node_id,
|
||||
TransferID tid);
|
||||
|
||||
/**
|
||||
* Send with automatic Transfer ID.
|
||||
* TID is managed by OutgoingTransferRegistry.
|
||||
*/
|
||||
int send(const uint8_t* payload, int payload_len, uint64_t monotonic_tx_deadline,
|
||||
uint64_t monotonic_blocking_deadline, TransferType transfer_type, NodeID dst_node_id);
|
||||
};
|
||||
|
||||
}
|
||||
@@ -0,0 +1,109 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#include <cassert>
|
||||
#include <uavcan/internal/debug.hpp>
|
||||
#include <uavcan/internal/transport/transfer_sender.hpp>
|
||||
|
||||
|
||||
namespace uavcan
|
||||
{
|
||||
|
||||
int TransferSender::send(const uint8_t* payload, int payload_len, uint64_t monotonic_tx_deadline,
|
||||
uint64_t monotonic_blocking_deadline, TransferType transfer_type, NodeID dst_node_id,
|
||||
TransferID tid)
|
||||
{
|
||||
Frame frame(data_type_.id, transfer_type, dispatcher_.getSelfNodeID(), dst_node_id, 0, tid);
|
||||
|
||||
if (frame.getMaxPayloadLen() >= payload_len) // Single Frame Transfer
|
||||
{
|
||||
const int res = frame.setPayload(payload, payload_len);
|
||||
if (res != payload_len)
|
||||
{
|
||||
assert(0);
|
||||
UAVCAN_TRACE("TransferSender", "Frame payload write failure, %i", res);
|
||||
return (res < 0) ? res : -1;
|
||||
}
|
||||
frame.makeLast();
|
||||
assert(frame.isLast() && frame.isFirst());
|
||||
return dispatcher_.send(frame, monotonic_tx_deadline, monotonic_blocking_deadline, qos_);
|
||||
}
|
||||
else // Multi Frame Transfer
|
||||
{
|
||||
int offset = 0;
|
||||
{
|
||||
Crc16 crc = crc_base_;
|
||||
crc.add(payload, payload_len);
|
||||
|
||||
static const int BUFLEN = sizeof(CanFrame::data);
|
||||
uint8_t buf[BUFLEN];
|
||||
|
||||
buf[0] = crc.get() & 0xFF; // Transfer CRC, little endian
|
||||
buf[1] = (crc.get() >> 8) & 0xFF;
|
||||
std::copy(payload, payload + BUFLEN - 2, buf + 2);
|
||||
|
||||
const int write_res = frame.setPayload(buf, BUFLEN);
|
||||
if (write_res < 2)
|
||||
{
|
||||
UAVCAN_TRACE("TransferSender", "Frame payload write failure, %i", write_res);
|
||||
return write_res;
|
||||
}
|
||||
offset = write_res - 2;
|
||||
assert(payload_len > offset);
|
||||
}
|
||||
|
||||
int next_frame_index = 1;
|
||||
|
||||
while (true)
|
||||
{
|
||||
const int send_res = dispatcher_.send(frame, monotonic_tx_deadline, monotonic_blocking_deadline, qos_);
|
||||
if (send_res < 0)
|
||||
return send_res;
|
||||
|
||||
if (frame.isLast())
|
||||
return next_frame_index; // Number of frames transmitted
|
||||
|
||||
frame.setIndex(next_frame_index++);
|
||||
|
||||
const int write_res = frame.setPayload(payload + offset, payload_len - offset);
|
||||
if (write_res < 0)
|
||||
{
|
||||
UAVCAN_TRACE("TransferSender", "Frame payload write failure, %i", write_res);
|
||||
return write_res;
|
||||
}
|
||||
|
||||
offset += write_res;
|
||||
assert(offset <= payload_len);
|
||||
if (offset >= payload_len)
|
||||
frame.makeLast();
|
||||
}
|
||||
}
|
||||
|
||||
assert(0);
|
||||
return -1; // Return path analysis is apparently broken. There should be no warning, this 'return' is unreachable.
|
||||
}
|
||||
|
||||
int TransferSender::send(const uint8_t* payload, int payload_len, uint64_t monotonic_tx_deadline,
|
||||
uint64_t monotonic_blocking_deadline, TransferType transfer_type, NodeID dst_node_id)
|
||||
{
|
||||
const OutgoingTransferRegistryKey otr_key(data_type_.id, transfer_type, dst_node_id);
|
||||
|
||||
assert(monotonic_tx_deadline > 0);
|
||||
const uint64_t otr_deadline = monotonic_tx_deadline + max_transfer_interval_;
|
||||
|
||||
TransferID* const tid = dispatcher_.getOutgoingTransferRegistry()->accessOrCreate(otr_key, otr_deadline);
|
||||
if (tid == NULL)
|
||||
{
|
||||
UAVCAN_TRACE("TransferSender", "OTR access failure, dtid=%i tt=%i", int(data_type_.id), int(transfer_type));
|
||||
return -1;
|
||||
}
|
||||
|
||||
const TransferID this_tid = tid->get();
|
||||
tid->increment();
|
||||
|
||||
return send(payload, payload_len, monotonic_tx_deadline, monotonic_blocking_deadline, transfer_type,
|
||||
dst_node_id, this_tid);
|
||||
}
|
||||
|
||||
}
|
||||
@@ -0,0 +1,143 @@
|
||||
/*
|
||||
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
||||
*/
|
||||
|
||||
#include <algorithm>
|
||||
#include <gtest/gtest.h>
|
||||
#include "transfer_test_helpers.hpp"
|
||||
#include "can/iface_mock.hpp"
|
||||
#include <uavcan/internal/transport/transfer_sender.hpp>
|
||||
|
||||
|
||||
static int sendOne(uavcan::TransferSender& sender, const std::string& data,
|
||||
uint64_t monotonic_tx_deadline, uint64_t monotonic_blocking_deadline,
|
||||
uavcan::TransferType transfer_type, uavcan::NodeID dst_node_id)
|
||||
{
|
||||
return sender.send(reinterpret_cast<const uint8_t*>(data.c_str()), data.length(), monotonic_tx_deadline,
|
||||
monotonic_blocking_deadline, transfer_type, dst_node_id);
|
||||
}
|
||||
|
||||
static int sendOne(uavcan::TransferSender& sender, const std::string& data,
|
||||
uint64_t monotonic_tx_deadline, uint64_t monotonic_blocking_deadline,
|
||||
uavcan::TransferType transfer_type, uavcan::NodeID dst_node_id, uavcan::TransferID tid)
|
||||
{
|
||||
return sender.send(reinterpret_cast<const uint8_t*>(data.c_str()), data.length(), monotonic_tx_deadline,
|
||||
monotonic_blocking_deadline, transfer_type, dst_node_id, tid);
|
||||
}
|
||||
|
||||
|
||||
TEST(TransferSender, Basic)
|
||||
{
|
||||
uavcan::PoolManager<1> poolmgr;
|
||||
|
||||
SystemClockMock clockmock(100);
|
||||
CanDriverMock driver(2, clockmock);
|
||||
|
||||
uavcan::OutgoingTransferRegistry<8> out_trans_reg(&poolmgr);
|
||||
|
||||
|
||||
static const uavcan::NodeID TX_NODE_ID(64);
|
||||
static const uavcan::NodeID RX_NODE_ID(65);
|
||||
uavcan::Dispatcher dispatcher_tx(&driver, &poolmgr, &clockmock, &out_trans_reg, TX_NODE_ID);
|
||||
uavcan::Dispatcher dispatcher_rx(&driver, &poolmgr, &clockmock, &out_trans_reg, RX_NODE_ID);
|
||||
|
||||
/*
|
||||
* Test environment
|
||||
*/
|
||||
static const uavcan::DataTypeDescriptor TYPES[2] =
|
||||
{
|
||||
makeDataType(uavcan::DATA_TYPE_KIND_MESSAGE, 1),
|
||||
makeDataType(uavcan::DATA_TYPE_KIND_SERVICE, 1)
|
||||
};
|
||||
|
||||
uavcan::TransferSender senders[2] =
|
||||
{
|
||||
uavcan::TransferSender(dispatcher_tx, TYPES[0], uavcan::CanTxQueue::VOLATILE),
|
||||
uavcan::TransferSender(dispatcher_tx, TYPES[1], uavcan::CanTxQueue::PERSISTENT)
|
||||
};
|
||||
|
||||
static const std::string DATA[4] =
|
||||
{
|
||||
"Don't panic.",
|
||||
|
||||
"The ships hung in the sky in much the same way that bricks don't.",
|
||||
|
||||
"Would it save you a lot of time if I just gave up and went mad now?",
|
||||
|
||||
"If there's anything more important than my ego around, I want it caught and shot now."
|
||||
};
|
||||
|
||||
/*
|
||||
* Transmission
|
||||
*/
|
||||
static const uint64_t TX_DEADLINE = 1000000;
|
||||
|
||||
sendOne(senders[0], DATA[0], TX_DEADLINE, 0, uavcan::TRANSFER_TYPE_MESSAGE_BROADCAST, 0);
|
||||
sendOne(senders[0], DATA[1], TX_DEADLINE, 0, uavcan::TRANSFER_TYPE_MESSAGE_UNICAST, RX_NODE_ID);
|
||||
sendOne(senders[0], "123", TX_DEADLINE, 0, uavcan::TRANSFER_TYPE_MESSAGE_BROADCAST, 0);
|
||||
sendOne(senders[0], "456", TX_DEADLINE, 0, uavcan::TRANSFER_TYPE_MESSAGE_UNICAST, RX_NODE_ID);
|
||||
|
||||
sendOne(senders[1], DATA[2], TX_DEADLINE, 0, uavcan::TRANSFER_TYPE_SERVICE_REQUEST, RX_NODE_ID);
|
||||
sendOne(senders[1], DATA[3], TX_DEADLINE, 0, uavcan::TRANSFER_TYPE_SERVICE_RESPONSE, RX_NODE_ID, 1);
|
||||
sendOne(senders[1], "", TX_DEADLINE, 0, uavcan::TRANSFER_TYPE_SERVICE_REQUEST, RX_NODE_ID);
|
||||
sendOne(senders[1], "", TX_DEADLINE, 0, uavcan::TRANSFER_TYPE_SERVICE_RESPONSE, RX_NODE_ID, 2);
|
||||
|
||||
static const Transfer TRANSFERS[8] =
|
||||
{
|
||||
Transfer(TX_DEADLINE, 0, uavcan::TRANSFER_TYPE_MESSAGE_BROADCAST, 0, TX_NODE_ID, 0, DATA[0], TYPES[0]),
|
||||
Transfer(TX_DEADLINE, 0, uavcan::TRANSFER_TYPE_MESSAGE_UNICAST, 0, TX_NODE_ID, RX_NODE_ID, DATA[1], TYPES[0]),
|
||||
Transfer(TX_DEADLINE, 0, uavcan::TRANSFER_TYPE_MESSAGE_BROADCAST, 1, TX_NODE_ID, 0, "123", TYPES[0]),
|
||||
Transfer(TX_DEADLINE, 0, uavcan::TRANSFER_TYPE_MESSAGE_UNICAST, 1, TX_NODE_ID, RX_NODE_ID, "456", TYPES[0]),
|
||||
|
||||
Transfer(TX_DEADLINE, 0, uavcan::TRANSFER_TYPE_SERVICE_REQUEST, 0, TX_NODE_ID, RX_NODE_ID, DATA[2], TYPES[1]),
|
||||
Transfer(TX_DEADLINE, 0, uavcan::TRANSFER_TYPE_SERVICE_RESPONSE, 1, TX_NODE_ID, RX_NODE_ID, DATA[3], TYPES[1]),
|
||||
Transfer(TX_DEADLINE, 0, uavcan::TRANSFER_TYPE_SERVICE_REQUEST, 1, TX_NODE_ID, RX_NODE_ID, "", TYPES[1]),
|
||||
Transfer(TX_DEADLINE, 0, uavcan::TRANSFER_TYPE_SERVICE_RESPONSE, 2, TX_NODE_ID, RX_NODE_ID, "", TYPES[1])
|
||||
};
|
||||
|
||||
/*
|
||||
* Receiving on the other side.
|
||||
*/
|
||||
for (int i = 0; i < driver.getNumIfaces(); i++) // Moving the frames from TX to RX side
|
||||
{
|
||||
CanIfaceMock& iface = driver.ifaces.at(i);
|
||||
std::cout << "Num frames: " << iface.tx.size() << std::endl;
|
||||
while (!iface.tx.empty())
|
||||
{
|
||||
CanIfaceMock::FrameWithTime ft = iface.tx.front();
|
||||
iface.tx.pop();
|
||||
iface.rx.push(ft);
|
||||
}
|
||||
}
|
||||
|
||||
TestSubscriber<512, 2, 2> sub_msg(TYPES + 0, &poolmgr);
|
||||
TestSubscriber<512, 2, 2> sub_srv_req(TYPES + 1, &poolmgr);
|
||||
TestSubscriber<512, 2, 2> sub_srv_resp(TYPES + 1, &poolmgr);
|
||||
|
||||
dispatcher_rx.registerMessageListener(&sub_msg);
|
||||
dispatcher_rx.registerServiceRequestListener(&sub_srv_req);
|
||||
dispatcher_rx.registerServiceResponseListener(&sub_srv_resp);
|
||||
|
||||
while (true)
|
||||
{
|
||||
const int res = dispatcher_rx.spin(0);
|
||||
ASSERT_LE(0, res);
|
||||
clockmock.advance(100);
|
||||
if (res == 0)
|
||||
break;
|
||||
}
|
||||
|
||||
/*
|
||||
* Validation
|
||||
*/
|
||||
ASSERT_TRUE(sub_msg.matchAndPop(TRANSFERS[0]));
|
||||
ASSERT_TRUE(sub_msg.matchAndPop(TRANSFERS[1]));
|
||||
ASSERT_TRUE(sub_msg.matchAndPop(TRANSFERS[2]));
|
||||
ASSERT_TRUE(sub_msg.matchAndPop(TRANSFERS[3]));
|
||||
|
||||
ASSERT_TRUE(sub_srv_req.matchAndPop(TRANSFERS[4]));
|
||||
ASSERT_TRUE(sub_srv_req.matchAndPop(TRANSFERS[6]));
|
||||
|
||||
ASSERT_TRUE(sub_srv_resp.matchAndPop(TRANSFERS[5]));
|
||||
ASSERT_TRUE(sub_srv_resp.matchAndPop(TRANSFERS[7]));
|
||||
}
|
||||
@@ -85,6 +85,7 @@ struct Transfer
|
||||
<< " tid=" << int(transfer_id.get())
|
||||
<< " snid=" << int(src_node_id.get())
|
||||
<< " dnid=" << int(dst_node_id.get())
|
||||
<< " dtid=" << int(data_type.id)
|
||||
<< "\n\t'" << payload << "'";
|
||||
return os.str();
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user