Skip to content

Commit 4c824be

Browse files
pavel-kirienkopavel-kirienko
committed
Always accept heartbeat and nodestatus for occupancy tracking
1 parent 56f6556 commit 4c824be

2 files changed

Lines changed: 297 additions & 21 deletions

File tree

‎libcanard/canard.c‎

Lines changed: 50 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -1566,39 +1566,41 @@ static canard_subscription_t* rx_route(const canard_t* const self, const frame_t
15661566
self->rx.subscriptions[fr->kind], &fr->port_id, rx_subscription_cavl_compare);
15671567
}
15681568

1569-
// Builds an acceptance filter that only admits frames that match the subscription.
1570-
static canard_filter_t rx_filter_for_subscription(const canard_t* const self, const canard_subscription_t* const sub)
1569+
// Builds an acceptance filter that only admits frames matching the given kind and port-ID.
1570+
static canard_filter_t rx_filter_for_subscription(const canard_t* const self,
1571+
const canard_kind_t kind,
1572+
const uint16_t port_id)
15711573
{
1572-
CANARD_ASSERT((self != NULL) && (sub != NULL));
1574+
CANARD_ASSERT(self != NULL);
15731575
canard_filter_t f = { 0 };
15741576
const uint32_t id = self->node_id & CANARD_NODE_ID_MAX;
1575-
switch (sub->kind) {
1577+
switch (kind) {
15761578
case canard_kind_message_16b:
1577-
f.extended_can_id = ((uint32_t)sub->port_id << 8U) | (UINT32_C(1) << 7U);
1579+
f.extended_can_id = ((uint32_t)port_id << 8U) | (UINT32_C(1) << 7U);
15781580
f.extended_mask = 0x03FFFF80U;
15791581
break;
15801582
case canard_kind_message_13b:
1581-
CANARD_ASSERT(sub->port_id <= CANARD_SUBJECT_ID_MAX_13b);
1582-
f.extended_can_id = (uint32_t)sub->port_id << 8U;
1583+
CANARD_ASSERT(port_id <= CANARD_SUBJECT_ID_MAX_13b);
1584+
f.extended_can_id = (uint32_t)port_id << 8U;
15831585
f.extended_mask = 0x029fff80U;
15841586
break;
15851587
case canard_kind_response:
15861588
case canard_kind_request: {
1587-
CANARD_ASSERT(sub->port_id <= CANARD_SERVICE_ID_MAX);
1588-
const uint32_t rnr = (sub->kind == canard_kind_request) ? (UINT32_C(1) << 24U) : 0U;
1589-
f.extended_can_id = (UINT32_C(1) << 25U) | rnr | ((uint32_t)sub->port_id << 14U) | (id << 7U);
1589+
CANARD_ASSERT(port_id <= CANARD_SERVICE_ID_MAX);
1590+
const uint32_t rnr = (kind == canard_kind_request) ? (UINT32_C(1) << 24U) : 0U;
1591+
f.extended_can_id = (UINT32_C(1) << 25U) | rnr | ((uint32_t)port_id << 14U) | (id << 7U);
15901592
f.extended_mask = 0x03FFFF80U;
15911593
break;
15921594
}
15931595
case canard_kind_v0_message:
1594-
f.extended_can_id = (uint32_t)sub->port_id << 8U;
1596+
f.extended_can_id = (uint32_t)port_id << 8U;
15951597
f.extended_mask = 0x00FFFF80;
15961598
break;
15971599
case canard_kind_v0_response:
15981600
case canard_kind_v0_request: {
1599-
CANARD_ASSERT(sub->port_id <= 0xFFU);
1600-
const uint32_t rnr = (sub->kind == canard_kind_v0_request) ? (UINT32_C(1) << 15U) : 0;
1601-
f.extended_can_id = ((sub->port_id & 0xFFU) << 16U) | rnr | (id << 8U) | (UINT32_C(1) << 7U);
1601+
CANARD_ASSERT(port_id <= 0xFFU);
1602+
const uint32_t rnr = (kind == canard_kind_v0_request) ? (UINT32_C(1) << 15U) : 0;
1603+
f.extended_can_id = ((port_id & 0xFFU) << 16U) | rnr | (id << 8U) | (UINT32_C(1) << 7U);
16021604
f.extended_mask = 0x00FFFF80U;
16031605
break;
16041606
}
@@ -1618,6 +1620,17 @@ static canard_filter_t rx_filter_fuse(const canard_filter_t a, const canard_filt
16181620
// Filter selectivity metric; see the Cyphal/CAN specification. Greater values ==> stronger filter.
16191621
static byte_t rx_filter_rank(const canard_filter_t a) { return popcount(a.extended_mask); }
16201622

1623+
// Returns true if any filter in the array accepts the given extended CAN ID.
1624+
static bool rx_filter_match(const size_t count, const canard_filter_t* const filters, const uint32_t extended_can_id)
1625+
{
1626+
for (size_t i = 0; i < count; i++) {
1627+
if ((extended_can_id & filters[i].extended_mask) == (filters[i].extended_can_id & filters[i].extended_mask)) {
1628+
return true;
1629+
}
1630+
}
1631+
return false;
1632+
}
1633+
16211634
// Modifies the filter array such that the new filter is accepted. See Cyphal/CAN Spec. Requires count>0.
16221635
static void rx_filter_coalesce_into(const size_t count, canard_filter_t* const into, const canard_filter_t new)
16231636
{
@@ -1674,7 +1687,29 @@ static bool rx_filter_configure(canard_t* const self)
16741687
for (const canard_subscription_t* sub = (canard_subscription_t*)(void*)cavl2_min(self->rx.subscriptions[kind]);
16751688
sub != NULL;
16761689
sub = (canard_subscription_t*)(void*)cavl2_next_greater((canard_tree_t*)sub)) {
1677-
const canard_filter_t f = rx_filter_for_subscription(self, sub);
1690+
const canard_filter_t f = rx_filter_for_subscription(self, sub->kind, sub->port_id);
1691+
if (n < capacity) {
1692+
filters[n++] = f;
1693+
} else {
1694+
rx_filter_coalesce_into(n, filters, f);
1695+
}
1696+
}
1697+
}
1698+
CANARD_ASSERT(n <= capacity);
1699+
1700+
// Force-admit Heartbeat/NodeStatus for node-ID occupancy tracking.
1701+
// This may be made optional at some point if the arrival load becomes a problem for small nodes.
1702+
static const struct
1703+
{
1704+
canard_kind_t kind;
1705+
uint16_t port_id;
1706+
} forced[] = {
1707+
{ canard_kind_message_13b, 7509U }, // Cyphal v1.0 Heartbeat
1708+
{ canard_kind_v0_message, 341U }, // DroneCAN NodeStatus
1709+
};
1710+
for (size_t i = 0; i < sizeof(forced) / sizeof(forced[0]); i++) {
1711+
const canard_filter_t f = rx_filter_for_subscription(self, forced[i].kind, forced[i].port_id);
1712+
if (!rx_filter_match(n, filters, f.extended_can_id)) {
16781713
if (n < capacity) {
16791714
filters[n++] = f;
16801715
} else {

‎tests/src/test_intrusive_rx_filter.c‎

Lines changed: 247 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -10,14 +10,10 @@ void tearDown(void) {}
1010

1111
static canard_filter_t make_filter(const canard_kind_t kind, const uint16_t port_id, const byte_t node_id)
1212
{
13-
canard_t self;
14-
canard_subscription_t sub;
13+
canard_t self;
1514
memset(&self, 0, sizeof(self));
16-
memset(&sub, 0, sizeof(sub));
1715
self.node_id = node_id;
18-
sub.kind = kind;
19-
sub.port_id = port_id;
20-
return rx_filter_for_subscription(&self, &sub);
16+
return rx_filter_for_subscription(&self, kind, port_id);
2117
}
2218

2319
static bool filter_accepts(const canard_filter_t filter, const uint32_t can_id)
@@ -933,6 +929,237 @@ static void test_rx_filter_configure_coalescence_overflow(void)
933929
canard_destroy(&self);
934930
}
935931

932+
// =====================================================================================================================
933+
// rx_filter_match()
934+
935+
static void test_rx_filter_match_empty(void)
936+
{
937+
TEST_ASSERT_FALSE(rx_filter_match(0, NULL, 0x12345678U));
938+
TEST_ASSERT_FALSE(rx_filter_match(0, NULL, 0U));
939+
}
940+
941+
static void test_rx_filter_match_single_hit(void)
942+
{
943+
const canard_filter_t f = make_filter(canard_kind_message_13b, 7509U, 42);
944+
TEST_ASSERT_TRUE(rx_filter_match(1, &f, f.extended_can_id));
945+
// Also matches with different source node-ID (bits 6:0 are not masked).
946+
TEST_ASSERT_TRUE(rx_filter_match(1, &f, f.extended_can_id | 1U));
947+
TEST_ASSERT_TRUE(rx_filter_match(1, &f, f.extended_can_id | 0x7FU));
948+
}
949+
950+
static void test_rx_filter_match_single_miss(void)
951+
{
952+
const canard_filter_t f = make_filter(canard_kind_message_13b, 7509U, 42);
953+
// A completely different subject-ID should not match.
954+
const canard_filter_t other = make_filter(canard_kind_message_13b, 100U, 42);
955+
TEST_ASSERT_FALSE(rx_filter_match(1, &f, other.extended_can_id));
956+
// v0 message with same numeric ID should also not match (different mask/bits).
957+
const canard_filter_t v0 = make_filter(canard_kind_v0_message, 341U, 42);
958+
TEST_ASSERT_FALSE(rx_filter_match(1, &f, v0.extended_can_id));
959+
}
960+
961+
static void test_rx_filter_match_multiple(void)
962+
{
963+
const canard_filter_t arr[] = {
964+
make_filter(canard_kind_message_16b, 100U, 42),
965+
make_filter(canard_kind_message_13b, 200U, 42),
966+
make_filter(canard_kind_v0_message, 300U, 42),
967+
};
968+
// Each filter's own CAN ID should be matched.
969+
TEST_ASSERT_TRUE(rx_filter_match(3, arr, arr[0].extended_can_id));
970+
TEST_ASSERT_TRUE(rx_filter_match(3, arr, arr[1].extended_can_id));
971+
TEST_ASSERT_TRUE(rx_filter_match(3, arr, arr[2].extended_can_id));
972+
// Unrelated CAN ID should not match any.
973+
const canard_filter_t unrelated = make_filter(canard_kind_message_13b, 999U, 42);
974+
TEST_ASSERT_FALSE(rx_filter_match(3, arr, unrelated.extended_can_id));
975+
}
976+
977+
// =====================================================================================================================
978+
// rx_filter_configure() forced Heartbeat/NodeStatus filters
979+
980+
#define HEARTBEAT_SUBJECT_ID 7509U
981+
#define NODESTATUS_DTYPE_ID 341U
982+
983+
static size_t g_cap_count;
984+
static canard_filter_t g_cap_filters[32];
985+
986+
static bool capturing_filter_cb(canard_t* const self, const size_t filter_count, const canard_filter_t* const filters)
987+
{
988+
(void)self;
989+
g_cap_count = filter_count;
990+
for (size_t i = 0; i < filter_count && i < 32U; i++) {
991+
g_cap_filters[i] = filters[i];
992+
}
993+
return true;
994+
}
995+
996+
static const canard_vtable_t capturing_vtable = { .now = test_now_cb, .tx = test_tx_cb, .filter = capturing_filter_cb };
997+
998+
// Checks whether any of the captured filters accepts a given CAN ID.
999+
static bool captured_accepts(const uint32_t can_id)
1000+
{
1001+
for (size_t i = 0; i < g_cap_count; i++) {
1002+
if (filter_accepts(g_cap_filters[i], can_id)) {
1003+
return true;
1004+
}
1005+
}
1006+
return false;
1007+
}
1008+
1009+
// Build a representative CAN ID for the forced message from an arbitrary source node.
1010+
static uint32_t heartbeat_can_id(const byte_t source)
1011+
{
1012+
return (HEARTBEAT_SUBJECT_ID << 8U) | (source & CANARD_NODE_ID_MAX);
1013+
}
1014+
static uint32_t nodestatus_can_id(const byte_t source)
1015+
{
1016+
return (NODESTATUS_DTYPE_ID << 8U) | (source & CANARD_NODE_ID_MAX);
1017+
}
1018+
1019+
static canard_t make_instance(const size_t filter_count)
1020+
{
1021+
const canard_mem_t real_mem = { .vtable = &real_mem_vtable, .context = NULL };
1022+
const canard_mem_set_t mem = { .tx_transfer = real_mem,
1023+
.tx_frame = real_mem,
1024+
.rx_session = real_mem,
1025+
.rx_payload = real_mem,
1026+
.rx_filters = real_mem };
1027+
canard_t self;
1028+
memset(&self, 0, sizeof(self));
1029+
(void)canard_new(&self, &capturing_vtable, mem, 16U, 1234U, filter_count);
1030+
g_cap_count = 0;
1031+
memset(g_cap_filters, 0, sizeof(g_cap_filters));
1032+
return self;
1033+
}
1034+
1035+
static void test_rx_filter_configure_forced_no_subs(void)
1036+
{
1037+
canard_t self = make_instance(4);
1038+
TEST_ASSERT_TRUE(rx_filter_configure(&self));
1039+
TEST_ASSERT_EQUAL_size_t(2U, g_cap_count); // Heartbeat + NodeStatus
1040+
TEST_ASSERT_TRUE(captured_accepts(heartbeat_can_id(0)));
1041+
TEST_ASSERT_TRUE(captured_accepts(heartbeat_can_id(42)));
1042+
TEST_ASSERT_TRUE(captured_accepts(heartbeat_can_id(127)));
1043+
TEST_ASSERT_TRUE(captured_accepts(nodestatus_can_id(0)));
1044+
TEST_ASSERT_TRUE(captured_accepts(nodestatus_can_id(42)));
1045+
TEST_ASSERT_TRUE(captured_accepts(nodestatus_can_id(127)));
1046+
canard_destroy(&self);
1047+
}
1048+
1049+
static void test_rx_filter_configure_forced_heartbeat_subscribed(void)
1050+
{
1051+
canard_t self = make_instance(4);
1052+
canard_subscription_t sub;
1053+
TEST_ASSERT_TRUE(canard_subscribe_13b(&self, &sub, HEARTBEAT_SUBJECT_ID, 64U, 1000000, &dummy_sub_vtable));
1054+
TEST_ASSERT_TRUE(rx_filter_configure(&self));
1055+
// 1 subscription filter + 1 forced NodeStatus = 2
1056+
TEST_ASSERT_EQUAL_size_t(2U, g_cap_count);
1057+
TEST_ASSERT_TRUE(captured_accepts(heartbeat_can_id(1)));
1058+
TEST_ASSERT_TRUE(captured_accepts(nodestatus_can_id(1)));
1059+
canard_unsubscribe(&self, &sub);
1060+
canard_destroy(&self);
1061+
}
1062+
1063+
static void test_rx_filter_configure_forced_nodestatus_subscribed(void)
1064+
{
1065+
canard_t self = make_instance(4);
1066+
canard_subscription_t sub;
1067+
TEST_ASSERT_TRUE(canard_v0_subscribe(&self, &sub, NODESTATUS_DTYPE_ID, 0, 64U, 1000000, &dummy_sub_vtable));
1068+
TEST_ASSERT_TRUE(rx_filter_configure(&self));
1069+
// 1 subscription filter + 1 forced Heartbeat = 2
1070+
TEST_ASSERT_EQUAL_size_t(2U, g_cap_count);
1071+
TEST_ASSERT_TRUE(captured_accepts(heartbeat_can_id(1)));
1072+
TEST_ASSERT_TRUE(captured_accepts(nodestatus_can_id(1)));
1073+
canard_unsubscribe(&self, &sub);
1074+
canard_destroy(&self);
1075+
}
1076+
1077+
static void test_rx_filter_configure_forced_both_subscribed(void)
1078+
{
1079+
canard_t self = make_instance(4);
1080+
canard_subscription_t sub_hb;
1081+
canard_subscription_t sub_ns;
1082+
TEST_ASSERT_TRUE(canard_subscribe_13b(&self, &sub_hb, HEARTBEAT_SUBJECT_ID, 64U, 1000000, &dummy_sub_vtable));
1083+
TEST_ASSERT_TRUE(canard_v0_subscribe(&self, &sub_ns, NODESTATUS_DTYPE_ID, 0, 64U, 1000000, &dummy_sub_vtable));
1084+
TEST_ASSERT_TRUE(rx_filter_configure(&self));
1085+
// Both already covered by subscriptions, no extras.
1086+
TEST_ASSERT_EQUAL_size_t(2U, g_cap_count);
1087+
TEST_ASSERT_TRUE(captured_accepts(heartbeat_can_id(1)));
1088+
TEST_ASSERT_TRUE(captured_accepts(nodestatus_can_id(1)));
1089+
canard_unsubscribe(&self, &sub_hb);
1090+
canard_unsubscribe(&self, &sub_ns);
1091+
canard_destroy(&self);
1092+
}
1093+
1094+
static void test_rx_filter_configure_forced_capacity_1(void)
1095+
{
1096+
canard_t self = make_instance(1);
1097+
TEST_ASSERT_TRUE(rx_filter_configure(&self));
1098+
// Heartbeat fills the slot, NodeStatus is coalesced into it.
1099+
TEST_ASSERT_EQUAL_size_t(1U, g_cap_count);
1100+
// The coalesced filter must still accept both.
1101+
TEST_ASSERT_TRUE(captured_accepts(heartbeat_can_id(1)));
1102+
TEST_ASSERT_TRUE(captured_accepts(nodestatus_can_id(1)));
1103+
canard_destroy(&self);
1104+
}
1105+
1106+
static void test_rx_filter_configure_forced_capacity_2_no_subs(void)
1107+
{
1108+
canard_t self = make_instance(2);
1109+
TEST_ASSERT_TRUE(rx_filter_configure(&self));
1110+
TEST_ASSERT_EQUAL_size_t(2U, g_cap_count);
1111+
// Each forced filter gets its own slot.
1112+
TEST_ASSERT_TRUE(captured_accepts(heartbeat_can_id(1)));
1113+
TEST_ASSERT_TRUE(captured_accepts(nodestatus_can_id(1)));
1114+
canard_destroy(&self);
1115+
}
1116+
1117+
static void test_rx_filter_configure_forced_with_unrelated_subs(void)
1118+
{
1119+
canard_t self = make_instance(10);
1120+
canard_subscription_t sub1, sub2, sub3;
1121+
TEST_ASSERT_TRUE(canard_subscribe_16b(&self, &sub1, 100U, 64U, 1000000, &dummy_sub_vtable));
1122+
TEST_ASSERT_TRUE(canard_subscribe_16b(&self, &sub2, 200U, 64U, 1000000, &dummy_sub_vtable));
1123+
TEST_ASSERT_TRUE(canard_subscribe_16b(&self, &sub3, 300U, 64U, 1000000, &dummy_sub_vtable));
1124+
TEST_ASSERT_TRUE(rx_filter_configure(&self));
1125+
// 3 subs + 2 forced = 5
1126+
TEST_ASSERT_EQUAL_size_t(5U, g_cap_count);
1127+
TEST_ASSERT_TRUE(captured_accepts(heartbeat_can_id(1)));
1128+
TEST_ASSERT_TRUE(captured_accepts(nodestatus_can_id(1)));
1129+
// Subscriptions still work.
1130+
const canard_filter_t f1 = make_filter(canard_kind_message_16b, 100U, 0);
1131+
const canard_filter_t f2 = make_filter(canard_kind_message_16b, 200U, 0);
1132+
const canard_filter_t f3 = make_filter(canard_kind_message_16b, 300U, 0);
1133+
TEST_ASSERT_TRUE(captured_accepts(f1.extended_can_id));
1134+
TEST_ASSERT_TRUE(captured_accepts(f2.extended_can_id));
1135+
TEST_ASSERT_TRUE(captured_accepts(f3.extended_can_id));
1136+
canard_unsubscribe(&self, &sub1);
1137+
canard_unsubscribe(&self, &sub2);
1138+
canard_unsubscribe(&self, &sub3);
1139+
canard_destroy(&self);
1140+
}
1141+
1142+
static void test_rx_filter_configure_forced_overflow(void)
1143+
{
1144+
// capacity=1 with 2 unrelated subs: subs fill+coalesce, then forced filters also coalesce in.
1145+
canard_t self = make_instance(1);
1146+
canard_subscription_t sub1, sub2;
1147+
TEST_ASSERT_TRUE(canard_subscribe_16b(&self, &sub1, 100U, 64U, 1000000, &dummy_sub_vtable));
1148+
TEST_ASSERT_TRUE(canard_subscribe_16b(&self, &sub2, 200U, 64U, 1000000, &dummy_sub_vtable));
1149+
TEST_ASSERT_TRUE(rx_filter_configure(&self));
1150+
TEST_ASSERT_EQUAL_size_t(1U, g_cap_count);
1151+
// After heavy coalescence the single filter should still accept all four CAN IDs.
1152+
const canard_filter_t f1 = make_filter(canard_kind_message_16b, 100U, 0);
1153+
const canard_filter_t f2 = make_filter(canard_kind_message_16b, 200U, 0);
1154+
TEST_ASSERT_TRUE(captured_accepts(f1.extended_can_id));
1155+
TEST_ASSERT_TRUE(captured_accepts(f2.extended_can_id));
1156+
TEST_ASSERT_TRUE(captured_accepts(heartbeat_can_id(1)));
1157+
TEST_ASSERT_TRUE(captured_accepts(nodestatus_can_id(1)));
1158+
canard_unsubscribe(&self, &sub1);
1159+
canard_unsubscribe(&self, &sub2);
1160+
canard_destroy(&self);
1161+
}
1162+
9361163
// =====================================================================================================================
9371164

9381165
int main(void)
@@ -991,5 +1218,19 @@ int main(void)
9911218
RUN_TEST(test_rx_filter_configure_oom);
9921219
RUN_TEST(test_rx_filter_configure_coalescence_overflow);
9931220

1221+
RUN_TEST(test_rx_filter_match_empty);
1222+
RUN_TEST(test_rx_filter_match_single_hit);
1223+
RUN_TEST(test_rx_filter_match_single_miss);
1224+
RUN_TEST(test_rx_filter_match_multiple);
1225+
1226+
RUN_TEST(test_rx_filter_configure_forced_no_subs);
1227+
RUN_TEST(test_rx_filter_configure_forced_heartbeat_subscribed);
1228+
RUN_TEST(test_rx_filter_configure_forced_nodestatus_subscribed);
1229+
RUN_TEST(test_rx_filter_configure_forced_both_subscribed);
1230+
RUN_TEST(test_rx_filter_configure_forced_capacity_1);
1231+
RUN_TEST(test_rx_filter_configure_forced_capacity_2_no_subs);
1232+
RUN_TEST(test_rx_filter_configure_forced_with_unrelated_subs);
1233+
RUN_TEST(test_rx_filter_configure_forced_overflow);
1234+
9941235
return UNITY_END();
9951236
}

0 commit comments

Comments
 (0)