Skip to content

Commit b96bab3

Browse files
committed
build fix
1 parent d97c84f commit b96bab3

File tree

3 files changed

+90
-64
lines changed

3 files changed

+90
-64
lines changed

.clang-format

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -9,7 +9,7 @@ AlignEscapedNewlines: Left
99
AlignOperands: true
1010
AlignTrailingComments: true
1111
AllowAllParametersOfDeclarationOnNextLine: false
12-
AllowShortBlocksOnASingleLine: false
12+
AllowShortBlocksOnASingleLine: Never
1313
AllowShortCaseLabelsOnASingleLine: false
1414
AllowShortFunctionsOnASingleLine: Inline
1515
AllowShortIfStatementsOnASingleLine: Never
@@ -73,7 +73,7 @@ PenaltyExcessCharacter: 1000000
7373
PenaltyReturnTypeOnItsOwnLine: 10000 # Raised intentionally because it hurts readability
7474
PointerAlignment: Left
7575
ReflowComments: true
76-
SortIncludes: false
76+
SortIncludes: Never
7777
SortUsingDeclarations: false
7878
SpaceAfterCStyleCast: true
7979
SpaceAfterTemplateKeyword: true
@@ -90,7 +90,7 @@ SpacesInCStyleCastParentheses: false
9090
SpacesInContainerLiterals: false
9191
SpacesInParentheses: false
9292
SpacesInSquareBrackets: false
93-
Standard: Cpp11
93+
Standard: c++14
9494
TabWidth: 8
9595
UseTab: Never
9696
...

differential_pressure_sensor/src/main.c

Lines changed: 44 additions & 29 deletions
Original file line numberDiff line numberDiff line change
@@ -153,28 +153,35 @@ static void send(State* const state,
153153
const CanardMicrosecond tx_deadline_usec,
154154
const struct CanardTransferMetadata* const metadata,
155155
const size_t payload_size,
156-
const void* const payload_data)
156+
const void* const payload_data,
157+
const CanardMicrosecond now_usec)
157158
{
158159
for (uint8_t ifidx = 0; ifidx < CAN_REDUNDANCY_FACTOR; ifidx++)
159160
{
160161
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);
162168
}
163169
}
164170

165171
static void sendResponse(State* const state,
166172
const CanardMicrosecond tx_deadline_usec,
167173
const struct CanardTransferMetadata* const request_metadata,
168174
const size_t payload_size,
169-
const void* const payload_data)
175+
const void* const payload_data,
176+
const CanardMicrosecond now_usec)
170177
{
171178
struct CanardTransferMetadata meta = *request_metadata;
172179
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);
174181
}
175182

176183
/// 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)
178185
{
179186
const bool anonymous = state->canard.node_id > CANARD_NODE_ID_MAX;
180187

@@ -197,20 +204,20 @@ static void handleFastLoop(State* const state, const CanardMicrosecond monotonic
197204
.remote_node_id = CANARD_NODE_ID_UNSET,
198205
.transfer_id = (CanardTransferID) state->next_transfer_id.differential_pressure++, // Increment!
199206
};
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);
201208
}
202209
}
203210
}
204211

205212
/// 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)
207214
{
208215
const bool anonymous = state->canard.node_id > CANARD_NODE_ID_MAX;
209216
// Publish heartbeat every second unless the local node is anonymous. Anonymous nodes shall not publish heartbeat.
210217
if (!anonymous)
211218
{
212219
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);
214221
heartbeat.mode.value = uavcan_node_Mode_1_0_OPERATIONAL;
215222
const O1HeapDiagnostics heap_diag = o1heapGetDiagnostics(state->heap);
216223
if (heap_diag.oom_count > 0)
@@ -236,10 +243,11 @@ static void handle1HzLoop(State* const state, const CanardMicrosecond monotonic_
236243
.transfer_id = (CanardTransferID) (state->next_transfer_id.uavcan_node_heartbeat++),
237244
};
238245
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.
240247
&meta,
241248
serialized_size,
242-
&serialized[0]);
249+
&serialized[0],
250+
now_usec);
243251
}
244252
}
245253
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_
269277
.transfer_id = (CanardTransferID) (state->next_transfer_id.uavcan_pnp_allocation++),
270278
};
271279
send(state, // The response will arrive asynchronously eventually.
272-
monotonic_time + MEGA,
280+
now_usec + MEGA,
273281
&meta,
274282
serialized_size,
275-
&serialized[0]);
283+
&serialized[0],
284+
now_usec);
276285
}
277286
}
278287
}
@@ -296,7 +305,7 @@ static void handle1HzLoop(State* const state, const CanardMicrosecond monotonic_
296305
.remote_node_id = CANARD_NODE_ID_UNSET,
297306
.transfer_id = (CanardTransferID) state->next_transfer_id.static_air_temperature++, // Increment!
298307
};
299-
send(state, monotonic_time + MEGA, &meta, serialized_size, &serialized[0]);
308+
send(state, now_usec + MEGA, &meta, serialized_size, &serialized[0], now_usec);
300309
}
301310
}
302311
}
@@ -329,7 +338,7 @@ static void fillServers(const struct CanardTreeNode* const tree, uavcan_node_por
329338
}
330339

331340
/// 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)
333342
{
334343
// Publish the recommended (not required) port introspection message. No point publishing it if we're anonymous.
335344
// 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
373382
.remote_node_id = CANARD_NODE_ID_UNSET,
374383
.transfer_id = (CanardTransferID) (state->next_transfer_id.uavcan_node_port_list++),
375384
};
376-
send(state, monotonic_time + MEGA, &meta, serialized_size, &serialized[0]);
385+
send(state, now_usec + MEGA, &meta, serialized_size, &serialized[0], now_usec);
377386
}
378387
}
379388
}
@@ -516,7 +525,9 @@ static uavcan_node_GetInfo_Response_1_0 processRequestNodeGetInfo()
516525
return resp;
517526
}
518527

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)
520531
{
521532
if (transfer->metadata.transfer_kind == CanardTransferKindMessage)
522533
{
@@ -549,7 +560,8 @@ static void processReceivedTransfer(State* const state, const struct CanardRxTra
549560
transfer->timestamp_usec + MEGA,
550561
&transfer->metadata,
551562
serialized_size,
552-
&serialized[0]);
563+
&serialized[0],
564+
now_usec);
553565
}
554566
else
555567
{
@@ -571,7 +583,8 @@ static void processReceivedTransfer(State* const state, const struct CanardRxTra
571583
transfer->timestamp_usec + MEGA,
572584
&transfer->metadata,
573585
serialized_size,
574-
&serialized[0]);
586+
&serialized[0],
587+
now_usec);
575588
}
576589
}
577590
}
@@ -590,7 +603,8 @@ static void processReceivedTransfer(State* const state, const struct CanardRxTra
590603
transfer->timestamp_usec + MEGA,
591604
&transfer->metadata,
592605
serialized_size,
593-
&serialized[0]);
606+
&serialized[0],
607+
now_usec);
594608
}
595609
}
596610
}
@@ -609,7 +623,8 @@ static void processReceivedTransfer(State* const state, const struct CanardRxTra
609623
transfer->timestamp_usec + MEGA,
610624
&transfer->metadata,
611625
serialized_size,
612-
&serialized[0]);
626+
&serialized[0],
627+
now_usec);
613628
}
614629
}
615630
}
@@ -813,21 +828,21 @@ int main(const int argc, char* const argv[])
813828
do
814829
{
815830
// 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)
818833
{
819834
next_fast_iter_at += fast_loop_period;
820-
handleFastLoop(&state, monotonic_time);
835+
handleFastLoop(&state, now_usec);
821836
}
822-
if (monotonic_time >= next_1_hz_iter_at)
837+
if (now_usec >= next_1_hz_iter_at)
823838
{
824839
next_1_hz_iter_at += MEGA;
825-
handle1HzLoop(&state, monotonic_time);
840+
handle1HzLoop(&state, now_usec);
826841
}
827-
if (monotonic_time >= next_01_hz_iter_at)
842+
if (now_usec >= next_01_hz_iter_at)
828843
{
829844
next_01_hz_iter_at += MEGA * 10;
830-
handle01HzLoop(&state, monotonic_time);
845+
handle01HzLoop(&state, now_usec);
831846
}
832847

833848
// Transmit pending frames from the prioritized TX queues managed by libcanard.
@@ -839,7 +854,7 @@ int main(const int argc, char* const argv[])
839854
{
840855
// Attempt transmission only if the frame is not yet timed out while waiting in the TX queue.
841856
// 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))
843858
{
844859
const struct CanardFrame canard_frame = {.extended_can_id = tqi->frame.extended_can_id,
845860
.payload = {.size = tqi->frame.payload.size,
@@ -886,7 +901,7 @@ int main(const int argc, char* const argv[])
886901
const int8_t canard_result = canardRxAccept(&state.canard, timestamp_usec, &frame, ifidx, &transfer, NULL);
887902
if (canard_result > 0)
888903
{
889-
processReceivedTransfer(&state, &transfer);
904+
processReceivedTransfer(&state, &transfer, now_usec);
890905
state.canard.memory.deallocate(state.canard.memory.user_reference,
891906
transfer.payload.allocated_size,
892907
transfer.payload.data);

0 commit comments

Comments
 (0)