Skip to content

Commit cd8c624

Browse files
serges147serges147
authored
Single copy TX pipeline for CAN (#32)
- Initialize TX block memory resource according to MTU of the transport and total number of redundant media. - Update all demos according the latest canard v4 & udpard v2 api changes
1 parent c1fbb09 commit cd8c624

File tree

17 files changed

+304
-196
lines changed

17 files changed

+304
-196
lines changed

‎.gitmodules

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,7 @@
1111
[submodule "submodules/libudpard"]
1212
path = submodules/libudpard
1313
url = https://github.com/OpenCyphal/libudpard
14+
branch = v2
1415
[submodule "submodules/cetl"]
1516
path = submodules/cetl
1617
url = https://github.com/OpenCyphal/CETL.git

‎differential_pressure_sensor/src/main.c

Lines changed: 49 additions & 44 deletions
Original file line numberDiff line numberDiff line change
@@ -48,9 +48,9 @@ typedef struct State
4848
{
4949
CanardMicrosecond started_at;
5050

51-
O1HeapInstance* heap;
52-
CanardInstance canard;
53-
CanardTxQueue canard_tx_queues[CAN_REDUNDANCY_FACTOR];
51+
O1HeapInstance* heap;
52+
struct CanardInstance canard;
53+
struct CanardTxQueue canard_tx_queues[CAN_REDUNDANCY_FACTOR];
5454

5555
/// These values are read from the registers at startup. You can also implement hot reloading if desired.
5656
struct
@@ -149,11 +149,11 @@ static CanardPortID getPublisherSubjectID(const char* const port_name, const cha
149149
return result;
150150
}
151151

152-
static void send(State* const state,
153-
const CanardMicrosecond tx_deadline_usec,
154-
const CanardTransferMetadata* const metadata,
155-
const size_t payload_size,
156-
const void* const payload_data)
152+
static void send(State* const state,
153+
const CanardMicrosecond tx_deadline_usec,
154+
const struct CanardTransferMetadata* const metadata,
155+
const size_t payload_size,
156+
const void* const payload_data)
157157
{
158158
for (uint8_t ifidx = 0; ifidx < CAN_REDUNDANCY_FACTOR; ifidx++)
159159
{
@@ -162,14 +162,14 @@ static void send(State* const state,
162162
}
163163
}
164164

165-
static void sendResponse(State* const state,
166-
const CanardMicrosecond tx_deadline_usec,
167-
const CanardTransferMetadata* const request_metadata,
168-
const size_t payload_size,
169-
const void* const payload_data)
165+
static void sendResponse(State* const state,
166+
const CanardMicrosecond tx_deadline_usec,
167+
const struct CanardTransferMetadata* const request_metadata,
168+
const size_t payload_size,
169+
const void* const payload_data)
170170
{
171-
CanardTransferMetadata meta = *request_metadata;
172-
meta.transfer_kind = CanardTransferKindResponse;
171+
struct CanardTransferMetadata meta = *request_metadata;
172+
meta.transfer_kind = CanardTransferKindResponse;
173173
send(state, tx_deadline_usec, &meta, payload_size, payload_data);
174174
}
175175

@@ -190,7 +190,7 @@ static void handleFastLoop(State* const state, const CanardMicrosecond monotonic
190190
assert(err >= 0);
191191
if (err >= 0)
192192
{
193-
const CanardTransferMetadata meta = {
193+
const struct CanardTransferMetadata meta = {
194194
.priority = CanardPriorityHigh,
195195
.transfer_kind = CanardTransferKindMessage,
196196
.port_id = state->port_id.pub.differential_pressure,
@@ -228,7 +228,7 @@ static void handle1HzLoop(State* const state, const CanardMicrosecond monotonic_
228228
assert(err >= 0);
229229
if (err >= 0)
230230
{
231-
const CanardTransferMetadata meta = {
231+
const struct CanardTransferMetadata meta = {
232232
.priority = CanardPriorityNominal,
233233
.transfer_kind = CanardTransferKindMessage,
234234
.port_id = uavcan_node_Heartbeat_1_0_FIXED_PORT_ID_,
@@ -261,7 +261,7 @@ static void handle1HzLoop(State* const state, const CanardMicrosecond monotonic_
261261
assert(err >= 0);
262262
if (err >= 0)
263263
{
264-
const CanardTransferMetadata meta = {
264+
const struct CanardTransferMetadata meta = {
265265
.priority = CanardPrioritySlow,
266266
.transfer_kind = CanardTransferKindMessage,
267267
.port_id = uavcan_pnp_NodeIDAllocationData_2_0_FIXED_PORT_ID_,
@@ -289,7 +289,7 @@ static void handle1HzLoop(State* const state, const CanardMicrosecond monotonic_
289289
assert(err >= 0);
290290
if (err >= 0)
291291
{
292-
const CanardTransferMetadata meta = {
292+
const struct CanardTransferMetadata meta = {
293293
.priority = CanardPriorityNominal,
294294
.transfer_kind = CanardTransferKindMessage,
295295
.port_id = state->port_id.pub.static_air_temperature,
@@ -302,12 +302,12 @@ static void handle1HzLoop(State* const state, const CanardMicrosecond monotonic_
302302
}
303303

304304
/// This is needed only for constructing uavcan_node_port_List_0_1.
305-
static void fillSubscriptions(const CanardTreeNode* const tree, uavcan_node_port_SubjectIDList_0_1* const obj)
305+
static void fillSubscriptions(const struct CanardTreeNode* const tree, uavcan_node_port_SubjectIDList_0_1* const obj)
306306
{
307307
if (NULL != tree)
308308
{
309309
fillSubscriptions(tree->lr[0], obj);
310-
const CanardRxSubscription* crs = (const CanardRxSubscription*) tree;
310+
const struct CanardRxSubscription* crs = (const struct CanardRxSubscription*) tree;
311311
assert(crs->port_id <= CANARD_SUBJECT_ID_MAX);
312312
assert(obj->sparse_list.count < uavcan_node_port_SubjectIDList_0_1_sparse_list_ARRAY_CAPACITY_);
313313
obj->sparse_list.elements[obj->sparse_list.count++].value = crs->port_id;
@@ -316,12 +316,12 @@ static void fillSubscriptions(const CanardTreeNode* const tree, uavcan_node_port
316316
}
317317

318318
/// This is needed only for constructing uavcan_node_port_List_0_1.
319-
static void fillServers(const CanardTreeNode* const tree, uavcan_node_port_ServiceIDList_0_1* const obj)
319+
static void fillServers(const struct CanardTreeNode* const tree, uavcan_node_port_ServiceIDList_0_1* const obj)
320320
{
321321
if (NULL != tree)
322322
{
323323
fillServers(tree->lr[0], obj);
324-
const CanardRxSubscription* crs = (const CanardRxSubscription*) tree;
324+
const struct CanardRxSubscription* crs = (const struct CanardRxSubscription*) tree;
325325
assert(crs->port_id <= CANARD_SERVICE_ID_MAX);
326326
(void) nunavutSetBit(&obj->mask_bitpacked_[0], sizeof(obj->mask_bitpacked_), crs->port_id, true);
327327
fillServers(tree->lr[1], obj);
@@ -366,7 +366,7 @@ static void handle01HzLoop(State* const state, const CanardMicrosecond monotonic
366366
size_t serialized_size = uavcan_node_port_List_0_1_SERIALIZATION_BUFFER_SIZE_BYTES_;
367367
if (uavcan_node_port_List_0_1_serialize_(&m, &serialized[0], &serialized_size) >= 0)
368368
{
369-
const CanardTransferMetadata meta = {
369+
const struct CanardTransferMetadata meta = {
370370
.priority = CanardPriorityOptional, // Mind the priority.
371371
.transfer_kind = CanardTransferKindMessage,
372372
.port_id = uavcan_node_port_List_0_1_FIXED_PORT_ID_,
@@ -516,7 +516,7 @@ static uavcan_node_GetInfo_Response_1_0 processRequestNodeGetInfo()
516516
return resp;
517517
}
518518

519-
static void processReceivedTransfer(State* const state, const CanardRxTransfer* const transfer)
519+
static void processReceivedTransfer(State* const state, const struct CanardRxTransfer* const transfer)
520520
{
521521
if (transfer->metadata.transfer_kind == CanardTransferKindMessage)
522522
{
@@ -733,8 +733,8 @@ int main(const int argc, char* const argv[])
733733
// Message subscriptions:
734734
if (state.canard.node_id > CANARD_NODE_ID_MAX)
735735
{
736-
static CanardRxSubscription rx;
737-
const int8_t res = //
736+
static struct CanardRxSubscription rx;
737+
const int8_t res = //
738738
canardRxSubscribe(&state.canard,
739739
CanardTransferKindMessage,
740740
uavcan_pnp_NodeIDAllocationData_2_0_FIXED_PORT_ID_,
@@ -748,8 +748,8 @@ int main(const int argc, char* const argv[])
748748
}
749749
// Service servers:
750750
{
751-
static CanardRxSubscription rx;
752-
const int8_t res = //
751+
static struct CanardRxSubscription rx;
752+
const int8_t res = //
753753
canardRxSubscribe(&state.canard,
754754
CanardTransferKindRequest,
755755
uavcan_node_GetInfo_1_0_FIXED_PORT_ID_,
@@ -762,8 +762,8 @@ int main(const int argc, char* const argv[])
762762
}
763763
}
764764
{
765-
static CanardRxSubscription rx;
766-
const int8_t res = //
765+
static struct CanardRxSubscription rx;
766+
const int8_t res = //
767767
canardRxSubscribe(&state.canard,
768768
CanardTransferKindRequest,
769769
uavcan_node_ExecuteCommand_1_1_FIXED_PORT_ID_,
@@ -776,8 +776,8 @@ int main(const int argc, char* const argv[])
776776
}
777777
}
778778
{
779-
static CanardRxSubscription rx;
780-
const int8_t res = //
779+
static struct CanardRxSubscription rx;
780+
const int8_t res = //
781781
canardRxSubscribe(&state.canard,
782782
CanardTransferKindRequest,
783783
uavcan_register_Access_1_0_FIXED_PORT_ID_,
@@ -790,8 +790,8 @@ int main(const int argc, char* const argv[])
790790
}
791791
}
792792
{
793-
static CanardRxSubscription rx;
794-
const int8_t res = //
793+
static struct CanardRxSubscription rx;
794+
const int8_t res = //
795795
canardRxSubscribe(&state.canard,
796796
CanardTransferKindRequest,
797797
uavcan_register_List_1_0_FIXED_PORT_ID_,
@@ -833,15 +833,18 @@ int main(const int argc, char* const argv[])
833833
// Transmit pending frames from the prioritized TX queues managed by libcanard.
834834
for (uint8_t ifidx = 0; ifidx < CAN_REDUNDANCY_FACTOR; ifidx++)
835835
{
836-
CanardTxQueue* const que = &state.canard_tx_queues[ifidx];
837-
const CanardTxQueueItem* tqi = canardTxPeek(que); // Find the highest-priority frame.
836+
struct CanardTxQueue* const que = &state.canard_tx_queues[ifidx];
837+
struct CanardTxQueueItem* tqi = canardTxPeek(que); // Find the highest-priority frame.
838838
while (tqi != NULL)
839839
{
840840
// Attempt transmission only if the frame is not yet timed out while waiting in the TX queue.
841841
// Otherwise just drop it and move on to the next one.
842842
if ((tqi->tx_deadline_usec == 0) || (tqi->tx_deadline_usec > monotonic_time))
843843
{
844-
const int16_t result = socketcanPush(sock[ifidx], &tqi->frame, 0); // Non-blocking write attempt.
844+
const struct CanardFrame canard_frame = {.extended_can_id = tqi->frame.extended_can_id,
845+
.payload = {.size = tqi->frame.payload.size,
846+
.data = tqi->frame.payload.data}};
847+
const int16_t result = socketcanPush(sock[ifidx], &canard_frame, 0); // Non-blocking write attempt.
845848
if (result == 0)
846849
{
847850
break; // The queue is full, we will try again on the next iteration.
@@ -851,8 +854,10 @@ int main(const int argc, char* const argv[])
851854
return -result; // SocketCAN interface failure (link down?)
852855
}
853856
}
854-
CanardTxQueueItem* const mut_tqi = canardTxPop(que, tqi);
855-
que->memory.deallocate(que->memory.user_reference, mut_tqi->allocated_size, mut_tqi);
857+
858+
struct CanardTxQueueItem* const mut_tqi = canardTxPop(que, tqi);
859+
canardTxFree(que, &state.canard, mut_tqi);
860+
856861
tqi = canardTxPeek(que);
857862
}
858863
}
@@ -863,9 +868,9 @@ int main(const int argc, char* const argv[])
863868
// them out and remove duplicates automatically.
864869
for (uint8_t ifidx = 0; ifidx < CAN_REDUNDANCY_FACTOR; ifidx++)
865870
{
866-
CanardFrame frame = {0};
867-
uint8_t buf[CANARD_MTU_CAN_FD] = {0};
868-
const int16_t socketcan_result = socketcanPop(sock[ifidx], &frame, NULL, sizeof(buf), buf, 0, NULL);
871+
struct CanardFrame frame = {0};
872+
uint8_t buf[CANARD_MTU_CAN_FD] = {0};
873+
const int16_t socketcan_result = socketcanPop(sock[ifidx], &frame, NULL, sizeof(buf), buf, 0, NULL);
869874
if (socketcan_result == 0) // The read operation has timed out with no frames, nothing to do here.
870875
{
871876
break;
@@ -877,7 +882,7 @@ int main(const int argc, char* const argv[])
877882
// The SocketCAN adapter uses the wall clock for timestamping, but we need monotonic.
878883
// Wall clock can only be used for time synchronization.
879884
const CanardMicrosecond timestamp_usec = getMonotonicMicroseconds();
880-
CanardRxTransfer transfer = {0};
885+
struct CanardRxTransfer transfer = {0};
881886
const int8_t canard_result = canardRxAccept(&state.canard, timestamp_usec, &frame, ifidx, &transfer, NULL);
882887
if (canard_result > 0)
883888
{

‎libcyphal_demo/src/application.cpp

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -32,6 +32,7 @@ Application::Application()
3232
: o1_heap_mr_{s_heap_arena}
3333
, storage_{"/tmp/" NODE_NAME}
3434
, registry_{o1_heap_mr_}
35+
, media_block_mr_{*cetl::pmr::new_delete_resource()}
3536
, regs_{o1_heap_mr_, registry_, media_block_mr_}
3637
{
3738
cetl::pmr::set_default_resource(&o1_heap_mr_);

‎libcyphal_demo/src/application.hpp

Lines changed: 4 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -35,11 +35,6 @@ class Application final
3535
static constexpr std::size_t MaxIfaceLen = 64;
3636
static constexpr std::size_t MaxNodeDesc = 50;
3737

38-
// Defines maximum alignment requirement for media block memory resource.
39-
// Currently, it's `std::max_align_t` but could be as small as 1-byte for raw bytes fragments.
40-
static constexpr std::size_t MediaBlockMaxAlign = alignof(std::max_align_t);
41-
using MediaMemoryResource = platform::BlockMemoryResource<MediaBlockMaxAlign>;
42-
4338
struct Regs
4439
{
4540
using Value = libcyphal::application::registry::IRegister::Value;
@@ -161,7 +156,7 @@ class Application final
161156

162157
Regs(platform::O1HeapMemoryResource& o1_heap_mr,
163158
libcyphal::application::registry::Registry& registry,
164-
MediaMemoryResource& media_block_mr)
159+
platform::BlockMemoryResource& media_block_mr)
165160
: o1_heap_mr_{o1_heap_mr}
166161
, registry_{registry}
167162
, media_block_mr_{media_block_mr}
@@ -178,7 +173,7 @@ class Application final
178173

179174
platform::O1HeapMemoryResource& o1_heap_mr_;
180175
libcyphal::application::registry::Registry& registry_;
181-
MediaMemoryResource& media_block_mr_;
176+
platform::BlockMemoryResource& media_block_mr_;
182177

183178
// clang-format off
184179
StringParam<MaxIfaceLen> can_iface_ { "uavcan.can.iface", registry_, {"vcan0"}, {true}};
@@ -221,7 +216,7 @@ class Application final
221216
return o1_heap_mr_;
222217
}
223218

224-
CETL_NODISCARD MediaMemoryResource& media_block_memory() noexcept
219+
CETL_NODISCARD platform::BlockMemoryResource& media_block_memory() noexcept
225220
{
226221
return media_block_mr_;
227222
}
@@ -251,7 +246,7 @@ class Application final
251246

252247
platform::Linux::EpollSingleThreadedExecutor executor_;
253248
platform::O1HeapMemoryResource o1_heap_mr_;
254-
MediaMemoryResource media_block_mr_;
249+
platform::BlockMemoryResource media_block_mr_;
255250
platform::storage::KeyValue storage_;
256251
libcyphal::application::registry::Registry registry_;
257252
Regs regs_;

‎libcyphal_demo/src/main.cpp

Lines changed: 5 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -128,19 +128,14 @@ libcyphal::Expected<bool, ExitCode> run_application()
128128
return ExitCode::TransportCreationFailure;
129129
}
130130

131-
// 2. Allocate block memory for media of the transport layer.
132-
//
133-
std::array<cetl::byte, 2U * 1024U> block_memory_blob; // NOLINT
134-
media_block_mr.setup(block_memory_blob.size(), block_memory_blob.data(), 256U);
135-
136-
// 3. Create the presentation layer object.
131+
// 2. Create the presentation layer object.
137132
//
138133
(void) transport_iface->setLocalNodeId(node_params.id.value()[0]);
139134
std::cout << "Node ID : " << transport_iface->getLocalNodeId().value_or(65535) << "\n";
140135
std::cout << "Node Name : '" << node_params.description.value().c_str() << "'\n";
141136
libcyphal::presentation::Presentation presentation{general_mr, executor, *transport_iface};
142137

143-
// 4. Create the node object with name.
138+
// 3. Create the node object with name.
144139
//
145140
auto maybe_node = libcyphal::application::Node::make(presentation);
146141
if (const auto* failure = cetl::get_if<libcyphal::application::Node::MakeFailure>(&maybe_node))
@@ -152,7 +147,7 @@ libcyphal::Expected<bool, ExitCode> run_application()
152147
}
153148
auto node = cetl::get<libcyphal::application::Node>(std::move(maybe_node));
154149

155-
// 5. Populate the node info.
150+
// 4. Populate the node info.
156151
//
157152
// The hardware version is not populated in this demo because it runs on no specific hardware.
158153
// An embedded node would usually determine the version by querying the hardware.
@@ -174,15 +169,15 @@ libcyphal::Expected<bool, ExitCode> run_application()
174169
}
175170
});
176171

177-
// 6. Bring up registry provider.
172+
// 5. Bring up registry provider.
178173
//
179174
if (const auto failure = node.makeRegistryProvider(application.registry()))
180175
{
181176
std::cerr << "❌ Failed to create registry provider.\n";
182177
return ExitCode::RegistryCreationFailure;
183178
}
184179

185-
// 7. Bring up the command execution provider.
180+
// 6. Bring up the command execution provider.
186181
//
187182
auto maybe_exec_cmd_provider = AppExecCmdProvider::make(presentation);
188183
if (const auto* failure = cetl::get_if<libcyphal::application::Node::MakeFailure>(&maybe_node))

0 commit comments

Comments
 (0)