TEST(Frame, FrameParsing)
{
using uavcan::Frame;
using uavcan::CanFrame;
using uavcan::NodeID;
using uavcan::TransferID;
CanFrame can;
Frame frame;
ASSERT_FALSE(frame.parse(can));
for (unsigned i = 0; i < sizeof(CanFrame::data); i++)
{
can.data[i] = uint8_t(i | (i << 4));
}
/*
* Message CAN ID fields and offsets:
* 24 Priority
* 8 Message Type ID
* 7 Service Not Message (0)
* 0 Source Node ID
*
* Service CAN ID fields and offsets:
* 24 Priority
* 16 Service Type ID
* 15 Request Not Response
* 8 Destination Node ID
* 7 Service Not Message (1)
* 0 Source Node ID
*/
/*
* SFT message broadcast
*/
can.id = CanFrame::FlagEFF |
(2 << 24) |
(456 << 8) |
(0 << 7) |
(42 << 0);
can.data[7] = 0xcf; // SET=110, TID=0
ASSERT_FALSE(frame.parse(can));
can.dlc = 8;
ASSERT_TRUE(frame.parse(can));
EXPECT_TRUE(frame.isStartOfTransfer());
EXPECT_TRUE(frame.isEndOfTransfer());
EXPECT_FALSE(frame.getToggle());
ASSERT_EQ(2, frame.getPriority().get());
ASSERT_EQ(NodeID(42), frame.getSrcNodeID());
ASSERT_EQ(NodeID::Broadcast, frame.getDstNodeID());
ASSERT_EQ(456, frame.getDataTypeID().get());
ASSERT_EQ(TransferID(15), frame.getTransferID());
ASSERT_EQ(uavcan::TransferTypeMessageBroadcast, frame.getTransferType());
// TODO: test service frames
// TODO: test malformed frames
}
TEST(NodeStatusMonitor, Basic)
{
using uavcan::protocol::NodeStatus;
using uavcan::NodeID;
SystemClockMock clock_mock(100);
clock_mock.monotonic_auto_advance = 1000;
CanDriverMock can(2, clock_mock);
TestNode node(can, clock_mock, 64);
uavcan::GlobalDataTypeRegistry::instance().reset();
uavcan::DefaultDataTypeRegistrator<uavcan::protocol::NodeStatus> _reg1;
uavcan::NodeStatusMonitor nsm(node);
ASSERT_LE(0, nsm.start());
ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10)));
/*
* Empty NSM, no nodes were registered yet
*/
ASSERT_FALSE(nsm.findNodeWithWorstStatus().isValid());
uavcan::NodeStatusMonitor::NodeStatus st = nsm.getNodeStatus(uavcan::NodeID(123));
ASSERT_FALSE(st.known);
ASSERT_EQ(NodeStatus::STATUS_OFFLINE, st.status_code);
/*
* Some new status messages
*/
publishNodeStatus(can, 10, NodeStatus::STATUS_OK, 12, 0);
shortSpin(node);
ASSERT_EQ(NodeID(10), nsm.findNodeWithWorstStatus());
publishNodeStatus(can, 9, NodeStatus::STATUS_INITIALIZING, 0, 0);
shortSpin(node);
ASSERT_EQ(NodeID(9), nsm.findNodeWithWorstStatus());
publishNodeStatus(can, 11, NodeStatus::STATUS_CRITICAL, 999, 0);
shortSpin(node);
ASSERT_EQ(NodeID(11), nsm.findNodeWithWorstStatus());
st = nsm.getNodeStatus(uavcan::NodeID(10));
ASSERT_TRUE(st.known);
ASSERT_EQ(NodeStatus::STATUS_OK, st.status_code);
st = nsm.getNodeStatus(uavcan::NodeID(9));
ASSERT_TRUE(st.known);
ASSERT_EQ(NodeStatus::STATUS_INITIALIZING, st.status_code);
st = nsm.getNodeStatus(uavcan::NodeID(11));
ASSERT_TRUE(st.known);
ASSERT_EQ(NodeStatus::STATUS_CRITICAL, st.status_code);
/*
* Timeout
*/
std::cout << "Starting timeout test, current monotime is " << clock_mock.monotonic << std::endl;
clock_mock.advance(500000);
shortSpin(node);
st = nsm.getNodeStatus(uavcan::NodeID(10));
ASSERT_TRUE(st.known);
ASSERT_EQ(NodeStatus::STATUS_OK, st.status_code);
clock_mock.advance(500000);
shortSpin(node);
st = nsm.getNodeStatus(uavcan::NodeID(9));
ASSERT_TRUE(st.known);
ASSERT_EQ(NodeStatus::STATUS_INITIALIZING, st.status_code);
clock_mock.advance(500000);
shortSpin(node);
st = nsm.getNodeStatus(uavcan::NodeID(11));
ASSERT_TRUE(st.known);
ASSERT_EQ(NodeStatus::STATUS_CRITICAL, st.status_code);
/*
* Will timeout now
*/
clock_mock.advance(4000000);
shortSpin(node);
st = nsm.getNodeStatus(uavcan::NodeID(10));
ASSERT_TRUE(st.known);
ASSERT_EQ(NodeStatus::STATUS_OFFLINE, st.status_code);
st = nsm.getNodeStatus(uavcan::NodeID(9));
ASSERT_TRUE(st.known);
ASSERT_EQ(NodeStatus::STATUS_OFFLINE, st.status_code);
st = nsm.getNodeStatus(uavcan::NodeID(11));
ASSERT_TRUE(st.known);
ASSERT_EQ(NodeStatus::STATUS_OFFLINE, st.status_code);
/*
* Recovering one node, adding two extra
*/
publishNodeStatus(can, 11, NodeStatus::STATUS_WARNING, 999, 0);
shortSpin(node);
publishNodeStatus(can, 127, NodeStatus::STATUS_WARNING, 9999, 0);
shortSpin(node);
publishNodeStatus(can, 1, NodeStatus::STATUS_OK, 1234, 0);
shortSpin(node);
/*
* Making sure OFFLINE is still worst status
*/
ASSERT_EQ(NodeID(9), nsm.findNodeWithWorstStatus());
/*
* Final validation
*/
st = nsm.getNodeStatus(uavcan::NodeID(10));
ASSERT_TRUE(st.known);
ASSERT_EQ(NodeStatus::STATUS_OFFLINE, st.status_code);
st = nsm.getNodeStatus(uavcan::NodeID(9));
ASSERT_TRUE(st.known);
ASSERT_EQ(NodeStatus::STATUS_OFFLINE, st.status_code);
st = nsm.getNodeStatus(uavcan::NodeID(11));
ASSERT_TRUE(st.known);
ASSERT_EQ(NodeStatus::STATUS_WARNING, st.status_code);
st = nsm.getNodeStatus(uavcan::NodeID(127));
ASSERT_TRUE(st.known);
ASSERT_EQ(NodeStatus::STATUS_WARNING, st.status_code);
st = nsm.getNodeStatus(uavcan::NodeID(1));
ASSERT_TRUE(st.known);
ASSERT_EQ(NodeStatus::STATUS_OK, st.status_code);
/*
* Forgetting
*/
nsm.forgetNode(127);
st = nsm.getNodeStatus(uavcan::NodeID(127));
ASSERT_FALSE(st.known);
ASSERT_EQ(NodeStatus::STATUS_OFFLINE, st.status_code);
nsm.forgetNode(9);
st = nsm.getNodeStatus(uavcan::NodeID(9));
ASSERT_FALSE(st.known);
ASSERT_EQ(NodeStatus::STATUS_OFFLINE, st.status_code);
}
