@@ -153,28 +153,35 @@ static void send(State* const state,
153
153
const CanardMicrosecond tx_deadline_usec ,
154
154
const struct CanardTransferMetadata * const metadata ,
155
155
const size_t payload_size ,
156
- const void * const payload_data )
156
+ const void * const payload_data ,
157
+ const CanardMicrosecond now_usec )
157
158
{
158
159
for (uint8_t ifidx = 0 ; ifidx < CAN_REDUNDANCY_FACTOR ; ifidx ++ )
159
160
{
160
161
const struct CanardPayload payload = {.size = payload_size , .data = payload_data };
161
- (void ) canardTxPush (& state -> canard_tx_queues [ifidx ], & state -> canard , tx_deadline_usec , metadata , payload );
162
+ (void ) canardTxPush (& state -> canard_tx_queues [ifidx ],
163
+ & state -> canard ,
164
+ tx_deadline_usec ,
165
+ metadata ,
166
+ payload ,
167
+ now_usec );
162
168
}
163
169
}
164
170
165
171
static void sendResponse (State * const state ,
166
172
const CanardMicrosecond tx_deadline_usec ,
167
173
const struct CanardTransferMetadata * const request_metadata ,
168
174
const size_t payload_size ,
169
- const void * const payload_data )
175
+ const void * const payload_data ,
176
+ const CanardMicrosecond now_usec )
170
177
{
171
178
struct CanardTransferMetadata meta = * request_metadata ;
172
179
meta .transfer_kind = CanardTransferKindResponse ;
173
- send (state , tx_deadline_usec , & meta , payload_size , payload_data );
180
+ send (state , tx_deadline_usec , & meta , payload_size , payload_data , now_usec );
174
181
}
175
182
176
183
/// Invoked at the rate of the fastest loop.
177
- static void handleFastLoop (State * const state , const CanardMicrosecond monotonic_time )
184
+ static void handleFastLoop (State * const state , const CanardMicrosecond now_usec )
178
185
{
179
186
const bool anonymous = state -> canard .node_id > CANARD_NODE_ID_MAX ;
180
187
@@ -197,20 +204,20 @@ static void handleFastLoop(State* const state, const CanardMicrosecond monotonic
197
204
.remote_node_id = CANARD_NODE_ID_UNSET ,
198
205
.transfer_id = (CanardTransferID ) state -> next_transfer_id .differential_pressure ++ , // Increment!
199
206
};
200
- send (state , monotonic_time + 10 * KILO , & meta , serialized_size , & serialized [0 ]);
207
+ send (state , now_usec + 10 * KILO , & meta , serialized_size , & serialized [0 ], now_usec );
201
208
}
202
209
}
203
210
}
204
211
205
212
/// Invoked every second.
206
- static void handle1HzLoop (State * const state , const CanardMicrosecond monotonic_time )
213
+ static void handle1HzLoop (State * const state , const CanardMicrosecond now_usec )
207
214
{
208
215
const bool anonymous = state -> canard .node_id > CANARD_NODE_ID_MAX ;
209
216
// Publish heartbeat every second unless the local node is anonymous. Anonymous nodes shall not publish heartbeat.
210
217
if (!anonymous )
211
218
{
212
219
uavcan_node_Heartbeat_1_0 heartbeat = {0 };
213
- heartbeat .uptime = (uint32_t ) ((monotonic_time - state -> started_at ) / MEGA );
220
+ heartbeat .uptime = (uint32_t ) ((now_usec - state -> started_at ) / MEGA );
214
221
heartbeat .mode .value = uavcan_node_Mode_1_0_OPERATIONAL ;
215
222
const O1HeapDiagnostics heap_diag = o1heapGetDiagnostics (state -> heap );
216
223
if (heap_diag .oom_count > 0 )
@@ -236,10 +243,11 @@ static void handle1HzLoop(State* const state, const CanardMicrosecond monotonic_
236
243
.transfer_id = (CanardTransferID ) (state -> next_transfer_id .uavcan_node_heartbeat ++ ),
237
244
};
238
245
send (state ,
239
- monotonic_time + MEGA , // Set transmission deadline 1 second, optimal for heartbeat.
246
+ now_usec + MEGA , // Set transmission deadline 1 second, optimal for heartbeat.
240
247
& meta ,
241
248
serialized_size ,
242
- & serialized [0 ]);
249
+ & serialized [0 ],
250
+ now_usec );
243
251
}
244
252
}
245
253
else // If we don't have a node-ID, obtain one by publishing allocation request messages until we get a response.
@@ -269,10 +277,11 @@ static void handle1HzLoop(State* const state, const CanardMicrosecond monotonic_
269
277
.transfer_id = (CanardTransferID ) (state -> next_transfer_id .uavcan_pnp_allocation ++ ),
270
278
};
271
279
send (state , // The response will arrive asynchronously eventually.
272
- monotonic_time + MEGA ,
280
+ now_usec + MEGA ,
273
281
& meta ,
274
282
serialized_size ,
275
- & serialized [0 ]);
283
+ & serialized [0 ],
284
+ now_usec );
276
285
}
277
286
}
278
287
}
@@ -296,7 +305,7 @@ static void handle1HzLoop(State* const state, const CanardMicrosecond monotonic_
296
305
.remote_node_id = CANARD_NODE_ID_UNSET ,
297
306
.transfer_id = (CanardTransferID ) state -> next_transfer_id .static_air_temperature ++ , // Increment!
298
307
};
299
- send (state , monotonic_time + MEGA , & meta , serialized_size , & serialized [0 ]);
308
+ send (state , now_usec + MEGA , & meta , serialized_size , & serialized [0 ], now_usec );
300
309
}
301
310
}
302
311
}
@@ -329,7 +338,7 @@ static void fillServers(const struct CanardTreeNode* const tree, uavcan_node_por
329
338
}
330
339
331
340
/// Invoked every 10 seconds.
332
- static void handle01HzLoop (State * const state , const CanardMicrosecond monotonic_time )
341
+ static void handle01HzLoop (State * const state , const CanardMicrosecond now_usec )
333
342
{
334
343
// Publish the recommended (not required) port introspection message. No point publishing it if we're anonymous.
335
344
// The message is a bit heavy on the stack (about 2 KiB) but this is not a problem for a modern MCU.
@@ -373,7 +382,7 @@ static void handle01HzLoop(State* const state, const CanardMicrosecond monotonic
373
382
.remote_node_id = CANARD_NODE_ID_UNSET ,
374
383
.transfer_id = (CanardTransferID ) (state -> next_transfer_id .uavcan_node_port_list ++ ),
375
384
};
376
- send (state , monotonic_time + MEGA , & meta , serialized_size , & serialized [0 ]);
385
+ send (state , now_usec + MEGA , & meta , serialized_size , & serialized [0 ], now_usec );
377
386
}
378
387
}
379
388
}
@@ -516,7 +525,9 @@ static uavcan_node_GetInfo_Response_1_0 processRequestNodeGetInfo()
516
525
return resp ;
517
526
}
518
527
519
- static void processReceivedTransfer (State * const state , const struct CanardRxTransfer * const transfer )
528
+ static void processReceivedTransfer (State * const state ,
529
+ const struct CanardRxTransfer * const transfer ,
530
+ const CanardMicrosecond now_usec )
520
531
{
521
532
if (transfer -> metadata .transfer_kind == CanardTransferKindMessage )
522
533
{
@@ -549,7 +560,8 @@ static void processReceivedTransfer(State* const state, const struct CanardRxTra
549
560
transfer -> timestamp_usec + MEGA ,
550
561
& transfer -> metadata ,
551
562
serialized_size ,
552
- & serialized [0 ]);
563
+ & serialized [0 ],
564
+ now_usec );
553
565
}
554
566
else
555
567
{
@@ -571,7 +583,8 @@ static void processReceivedTransfer(State* const state, const struct CanardRxTra
571
583
transfer -> timestamp_usec + MEGA ,
572
584
& transfer -> metadata ,
573
585
serialized_size ,
574
- & serialized [0 ]);
586
+ & serialized [0 ],
587
+ now_usec );
575
588
}
576
589
}
577
590
}
@@ -590,7 +603,8 @@ static void processReceivedTransfer(State* const state, const struct CanardRxTra
590
603
transfer -> timestamp_usec + MEGA ,
591
604
& transfer -> metadata ,
592
605
serialized_size ,
593
- & serialized [0 ]);
606
+ & serialized [0 ],
607
+ now_usec );
594
608
}
595
609
}
596
610
}
@@ -609,7 +623,8 @@ static void processReceivedTransfer(State* const state, const struct CanardRxTra
609
623
transfer -> timestamp_usec + MEGA ,
610
624
& transfer -> metadata ,
611
625
serialized_size ,
612
- & serialized [0 ]);
626
+ & serialized [0 ],
627
+ now_usec );
613
628
}
614
629
}
615
630
}
@@ -813,21 +828,21 @@ int main(const int argc, char* const argv[])
813
828
do
814
829
{
815
830
// Run a trivial scheduler polling the loops that run the business logic.
816
- CanardMicrosecond monotonic_time = getMonotonicMicroseconds ();
817
- if (monotonic_time >= next_fast_iter_at )
831
+ const CanardMicrosecond now_usec = getMonotonicMicroseconds ();
832
+ if (now_usec >= next_fast_iter_at )
818
833
{
819
834
next_fast_iter_at += fast_loop_period ;
820
- handleFastLoop (& state , monotonic_time );
835
+ handleFastLoop (& state , now_usec );
821
836
}
822
- if (monotonic_time >= next_1_hz_iter_at )
837
+ if (now_usec >= next_1_hz_iter_at )
823
838
{
824
839
next_1_hz_iter_at += MEGA ;
825
- handle1HzLoop (& state , monotonic_time );
840
+ handle1HzLoop (& state , now_usec );
826
841
}
827
- if (monotonic_time >= next_01_hz_iter_at )
842
+ if (now_usec >= next_01_hz_iter_at )
828
843
{
829
844
next_01_hz_iter_at += MEGA * 10 ;
830
- handle01HzLoop (& state , monotonic_time );
845
+ handle01HzLoop (& state , now_usec );
831
846
}
832
847
833
848
// Transmit pending frames from the prioritized TX queues managed by libcanard.
@@ -839,7 +854,7 @@ int main(const int argc, char* const argv[])
839
854
{
840
855
// Attempt transmission only if the frame is not yet timed out while waiting in the TX queue.
841
856
// Otherwise just drop it and move on to the next one.
842
- if ((tqi -> tx_deadline_usec == 0 ) || (tqi -> tx_deadline_usec > monotonic_time ))
857
+ if ((tqi -> tx_deadline_usec == 0 ) || (tqi -> tx_deadline_usec > now_usec ))
843
858
{
844
859
const struct CanardFrame canard_frame = {.extended_can_id = tqi -> frame .extended_can_id ,
845
860
.payload = {.size = tqi -> frame .payload .size ,
@@ -886,7 +901,7 @@ int main(const int argc, char* const argv[])
886
901
const int8_t canard_result = canardRxAccept (& state .canard , timestamp_usec , & frame , ifidx , & transfer , NULL );
887
902
if (canard_result > 0 )
888
903
{
889
- processReceivedTransfer (& state , & transfer );
904
+ processReceivedTransfer (& state , & transfer , now_usec );
890
905
state .canard .memory .deallocate (state .canard .memory .user_reference ,
891
906
transfer .payload .allocated_size ,
892
907
transfer .payload .data );
0 commit comments