Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
uavcan.c
Go to the documentation of this file.
1/*
2 * Copyright (C) 2020 Freek van Tienen <freek.v.tienen@gmail.com>
3 *
4 * This file is part of paparazzi.
5 *
6 * paparazzi is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2, or (at your option)
9 * any later version.
10 *
11 * paparazzi is distributed in the hope that it will be useful,
12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14 * GNU General Public License for more details.
15 *
16 * You should have received a copy of the GNU General Public License
17 * along with paparazzi; see the file COPYING. If not, write to
18 * the Free Software Foundation, 59 Temple Place - Suite 330,
19 * Boston, MA 02111-1307, USA.
20 */
27#include "uavcan.h"
28
29#ifndef UAVCAN_NODE_ID
30#define UAVCAN_NODE_ID 100
31#endif
32
33#ifndef UAVCAN_BAUDRATE
34#define UAVCAN_BAUDRATE 1000000
35#endif
36
38
39#if UAVCAN_USE_CAN1
40#ifndef UAVCAN_CAN1_NODE_ID
41#define UAVCAN_CAN1_NODE_ID UAVCAN_NODE_ID
42#endif
43
44#ifndef UAVCAN_CAN1_BAUDRATE
45#define UAVCAN_CAN1_BAUDRATE UAVCAN_BAUDRATE
46#endif
47
48static THD_WORKING_AREA(uavcan1_rx_wa, 1024 * 2);
49static THD_WORKING_AREA(uavcan1_tx_wa, 1024 * 2);
50
51struct uavcan_iface_t uavcan1 = {
52 .can_driver = &CAND1,
53 .can_baudrate = UAVCAN_CAN1_BAUDRATE,
54 .can_cfg = {0},
55 .thread_rx_wa = uavcan1_rx_wa,
56 .thread_rx_wa_size = sizeof(uavcan1_rx_wa),
59 .node_id = UAVCAN_CAN1_NODE_ID,
60 .transfer_id = 0,
61 .initialized = false
62};
63#endif
64
65#if UAVCAN_USE_CAN2
66#ifndef UAVCAN_CAN2_NODE_ID
67#define UAVCAN_CAN2_NODE_ID UAVCAN_NODE_ID
68#endif
69
70#ifndef UAVCAN_CAN2_BAUDRATE
71#define UAVCAN_CAN2_BAUDRATE UAVCAN_BAUDRATE
72#endif
73
74static THD_WORKING_AREA(uavcan2_rx_wa, 1024 * 2);
75static THD_WORKING_AREA(uavcan2_tx_wa, 1024 * 2);
76
77struct uavcan_iface_t uavcan2 = {
78 .can_driver = &CAND2,
79 .can_baudrate = UAVCAN_CAN2_BAUDRATE,
80 .can_cfg = {0},
81 .thread_rx_wa = uavcan2_rx_wa,
82 .thread_rx_wa_size = sizeof(uavcan2_rx_wa),
85 .node_id = UAVCAN_CAN2_NODE_ID,
86 .transfer_id = 0,
87 .initialized = false
88};
89#endif
90
91/*
92 * Receiver thread.
93 */
95{
99 struct uavcan_iface_t *iface = (struct uavcan_iface_t *)p;
100
101 chRegSetThreadName("uavcan_rx");
102 chEvtRegister(&iface->can_driver->rxfull_event, &el, EVENT_MASK(0));
103 while (true) {
104 if (chEvtWaitAnyTimeout(ALL_EVENTS, TIME_MS2I(100)) == 0) {
105 continue;
106 }
107 chMtxLock(&iface->mutex);
108
109 // Wait until a CAN message is received
110 while (canReceive(iface->can_driver, CAN_ANY_MAILBOX, &rx_msg, TIME_IMMEDIATE) == MSG_OK) {
111 // Process message.
112 const uint32_t timestamp = TIME_I2US(chVTGetSystemTimeX());
113 memcpy(rx_frame.data, rx_msg.data8, 8);
114 rx_frame.data_len = rx_msg.DLC;
115#if defined(STM32_CAN_USE_FDCAN1) || defined(STM32_CAN_USE_FDCAN2)
116 if (rx_msg.common.XTD) {
117 rx_frame.id = CANARD_CAN_FRAME_EFF | rx_msg.ext.EID;
118 } else {
119 rx_frame.id = rx_msg.std.SID;
120 }
121#else
122 if (rx_msg.IDE) {
124 } else {
125 rx_frame.id = rx_msg.SID;
126 }
127#endif
128
129 // Let canard handle the frame
130 canardHandleRxFrame(&iface->canard, &rx_frame, timestamp);
131 }
132 chMtxUnlock(&iface->mutex);
133 }
134 chEvtUnregister(&iface->can_driver->rxfull_event, &el);
135}
136
137/*
138 * Transmitter thread.
139 */
141{
143 struct uavcan_iface_t *iface = (struct uavcan_iface_t *)p;
144 uint8_t err_cnt = 0;
145
146 chRegSetThreadName("uavcan_tx");
147 chEvtRegister(&iface->can_driver->txempty_event, &txc, EVENT_MASK(0));
148 chEvtRegister(&iface->can_driver->error_event, &txe, EVENT_MASK(1));
149 chEvtRegister(&iface->tx_request, &txr, EVENT_MASK(2));
150
151 while (true) {
153 // Succesfull transmit
154 if (evts == 0) {
155 continue;
156 }
157
158 // Transmit error
159 if (evts & EVENT_MASK(1)) {
161 continue;
162 }
163
164 chMtxLock(&iface->mutex);
165 for (const CanardCANFrame *txf = NULL; (txf = canardPeekTxQueue(&iface->canard)) != NULL;) {
166 CANTxFrame tx_msg;
167 memcpy(tx_msg.data8, txf->data, 8);
168#if defined(STM32_CAN_USE_FDCAN1) || defined(STM32_CAN_USE_FDCAN2)
169 tx_msg.DLC = txf->data_len; // TODO fixme for FDCAN (>8 bytes)
170 tx_msg.ext.EID = txf->id & CANARD_CAN_EXT_ID_MASK;
171 tx_msg.common.XTD = 1;
172 tx_msg.common.RTR = 0;
173#else
174 tx_msg.DLC = txf->data_len;
175 tx_msg.EID = txf->id & CANARD_CAN_EXT_ID_MASK;
176 tx_msg.IDE = CAN_IDE_EXT;
177 tx_msg.RTR = CAN_RTR_DATA;
178#endif
179 if (!canTryTransmitI(iface->can_driver, CAN_ANY_MAILBOX, &tx_msg)) {
180 err_cnt = 0;
181 canardPopTxQueue(&iface->canard);
182 } else {
183 // After 5 retries giveup and clean full queue
184 if (err_cnt >= 5) {
185 err_cnt = 0;
186 while (canardPeekTxQueue(&iface->canard)) { canardPopTxQueue(&iface->canard); }
187 continue;
188 }
189
190 // Timeout - just exit and try again later
191 chMtxUnlock(&iface->mutex);
193 chMtxLock(&iface->mutex);
194 continue;
195 }
196 }
197 chMtxUnlock(&iface->mutex);
198 }
199}
200
205{
206 struct uavcan_iface_t *iface = (struct uavcan_iface_t *)ins->user_reference;
207
208 // Go through all registered callbacks and call function callback if found
209 for (uavcan_event *ev = uavcan_event_hd; ev; ev = ev->next) {
210 if (transfer->data_type_id == ev->data_type_id) {
211 ev->cb(iface, transfer);
212 }
213 }
214}
215
221 uint16_t data_type_id,
224{
225
226 // Go through all registered callbacks and return signature if found
227 for (uavcan_event *ev = uavcan_event_hd; ev; ev = ev->next) {
228 if (data_type_id == ev->data_type_id) {
229 *out_data_type_signature = ev->data_type_signature;
230 return true;
231 }
232 }
233 // No callback found return
234 return false;
235}
236
241{
242 if (iface->can_baudrate < 1) {
243 return false;
244 }
245
246 // Hardware configurationn
247#if defined(STM32_CAN_USE_FDCAN1) || defined(STM32_CAN_USE_FDCAN2)
249#else
250 const uint32_t pclk = STM32_PCLK1;
251#endif
252 static const int MaxBS1 = 16;
253 static const int MaxBS2 = 8;
254
255 /*
256 * Ref. "Automatic Baudrate Detection in CANopen Networks", U. Koppe, MicroControl GmbH & Co. KG
257 * CAN in Automation, 2003
258 *
259 * According to the source, optimal quanta per bit are:
260 * Bitrate Optimal Maximum
261 * 1000 kbps 8 10
262 * 500 kbps 16 17
263 * 250 kbps 16 17
264 * 125 kbps 16 17
265 */
266 const int max_quanta_per_bit = (iface->can_baudrate >= 1000000) ? 10 : 17;
267 static const int MaxSamplePointLocation = 900;
268
269 /*
270 * Computing (prescaler * BS):
271 * BITRATE = 1 / (PRESCALER * (1 / PCLK) * (1 + BS1 + BS2)) -- See the Reference Manual
272 * BITRATE = PCLK / (PRESCALER * (1 + BS1 + BS2)) -- Simplified
273 * let:
274 * BS = 1 + BS1 + BS2 -- Number of time quanta per bit
275 * PRESCALER_BS = PRESCALER * BS
276 * ==>
277 * PRESCALER_BS = PCLK / BITRATE
278 */
279 const uint32_t prescaler_bs = pclk / iface->can_baudrate;
280
281// Searching for such prescaler value so that the number of quanta per bit is highest.
283 while ((prescaler_bs % (1 + bs1_bs2_sum)) != 0) {
284 if (bs1_bs2_sum <= 2) {
285 return false; // No solution
286 }
287 bs1_bs2_sum--;
288 }
289
290 const uint32_t prescaler = prescaler_bs / (1 + bs1_bs2_sum);
291 if ((prescaler < 1U) || (prescaler > 1024U)) {
292 return false; // No solution
293 }
294
295 /*
296 * Now we have a constraint: (BS1 + BS2) == bs1_bs2_sum.
297 * We need to find the values so that the sample point is as close as possible to the optimal value.
298 *
299 * Solve[(1 + bs1)/(1 + bs1 + bs2) == 7/8, bs2] (* Where 7/8 is 0.875, the recommended sample point location *)
300 * {{bs2 -> (1 + bs1)/7}}
301 *
302 * Hence:
303 * bs2 = (1 + bs1) / 7
304 * bs1 = (7 * bs1_bs2_sum - 1) / 8
305 *
306 * Sample point location can be computed as follows:
307 * Sample point location = (1 + bs1) / (1 + bs1 + bs2)
308 *
309 * Since the optimal solution is so close to the maximum, we prepare two solutions, and then pick the best one:
310 * - With rounding to nearest
311 * - With rounding to zero
312 */
313// First attempt with rounding to nearest
314 uint8_t bs1 = ((7 * bs1_bs2_sum - 1) + 4) / 8;
316 uint16_t sample_point_permill = 1000 * (1 + bs1) / (1 + bs1 + bs2);
317
318// Second attempt with rounding to zero
320 bs1 = (7 * bs1_bs2_sum - 1) / 8;
321 bs2 = bs1_bs2_sum - bs1;
322 sample_point_permill = 1000 * (1 + bs1) / (1 + bs1 + bs2);
323 }
324
325 /*
326 * Final validation
327 * Helpful Python:
328 * def sample_point_from_btr(x):
329 * assert 0b0011110010000000111111000000000 & x == 0
330 * ts2,ts1,brp = (x>>20)&7, (x>>16)&15, x&511
331 * return (1+ts1+1)/(1+ts1+1+ts2+1)
332 *
333 */
334 if ((iface->can_baudrate != (pclk / (prescaler * (1 + bs1 + bs2)))) || (bs1 < 1) || (bs1 > MaxBS1) || (bs2 < 1)
335 || (bs2 > MaxBS2)) {
336 return false;
337 }
338
339 // Configure the interface
340#if defined(STM32_CAN_USE_FDCAN1) || defined(STM32_CAN_USE_FDCAN2)
341 iface->can_cfg.NBTP = (0 << FDCAN_NBTP_NSJW_Pos) | ((bs1 - 1) << FDCAN_NBTP_NTSEG1_Pos) | ((
342 bs2 - 1) << FDCAN_NBTP_NTSEG2_Pos) | ((prescaler - 1) << FDCAN_NBTP_NBRP_Pos);
343 iface->can_cfg.CCCR = FDCAN_CCCR_FDOE | FDCAN_CCCR_BRSE;
344#else
345 iface->can_cfg.mcr = CAN_MCR_ABOM | CAN_MCR_AWUM | CAN_MCR_TXFP;
346 iface->can_cfg.btr = CAN_BTR_SJW(0) | CAN_BTR_TS1(bs1 - 1) | CAN_BTR_TS2(bs2 - 1) | CAN_BTR_BRP(prescaler - 1);
347#endif
348 return true;
349}
350
355{
356 // First try to configure abort if failed
358 return;
359 }
360
361 // Initialize mutexes/events for multithread locking
362 chMtxObjectInit(&iface->mutex);
363 chEvtObjectInit(&iface->tx_request);
364
365 // Initialize canard
366 canardInit(&iface->canard, iface->canard_memory_pool, sizeof(iface->canard_memory_pool),
368 // Update the uavcan node ID
369 canardSetLocalNodeID(&iface->canard, iface->node_id);
370
371 // Start the can interface
372 canStart(iface->can_driver, &iface->can_cfg);
373
374 // Start the receiver and transmitter thread
375 chThdCreateStatic(iface->thread_rx_wa, iface->thread_rx_wa_size, NORMALPRIO + 8, uavcan_rx, (void *)iface);
376 chThdCreateStatic(iface->thread_tx_wa, iface->thread_tx_wa_size, NORMALPRIO + 7, uavcan_tx, (void *)iface);
377 iface->initialized = true;
378}
379
383void uavcan_init(void)
384{
385#if UAVCAN_USE_CAN1
386#if defined(STM32_CAN_USE_FDCAN1) || defined(STM32_CAN_USE_FDCAN2)
387 // Configure the RAM
388 uavcan1.can_cfg.RXF0C = (32 << FDCAN_RXF0C_F0S_Pos) | (0 << FDCAN_RXF0C_F0SA_Pos);
389 uavcan1.can_cfg.RXF1C = (32 << FDCAN_RXF1C_F1S_Pos) | (128 << FDCAN_RXF1C_F1SA_Pos);
390 uavcan1.can_cfg.TXBC = (32 << FDCAN_TXBC_TFQS_Pos) | (256 << FDCAN_TXBC_TBSA_Pos);
391 uavcan1.can_cfg.TXESC = 0x000; // 8 Byte mode only (4 words per message)
392 uavcan1.can_cfg.RXESC = 0x000; // 8 Byte mode only (4 words per message)
393#endif
395#endif
396#if UAVCAN_USE_CAN2
397#if defined(STM32_CAN_USE_FDCAN1) || defined(STM32_CAN_USE_FDCAN2)
398 // Configure the RAM
399 uavcan2.can_cfg.RXF0C = (32 << FDCAN_RXF0C_F0S_Pos) | (384 << FDCAN_RXF0C_F0SA_Pos);
400 uavcan2.can_cfg.RXF1C = (32 << FDCAN_RXF1C_F1S_Pos) | (512 << FDCAN_RXF1C_F1SA_Pos);
401 uavcan2.can_cfg.TXBC = (32 << FDCAN_TXBC_TFQS_Pos) | (640 << FDCAN_TXBC_TBSA_Pos);
402 uavcan2.can_cfg.TXESC = 0x000; // 8 Byte mode only (4 words per message)
403 uavcan2.can_cfg.RXESC = 0x000; // 8 Byte mode only (4 words per message)
404#endif
406#endif
407}
408
412void uavcan_bind(uint16_t data_type_id, uint64_t data_type_signature, uavcan_event *ev, uavcan_callback cb)
413{
414 // Configure the event
415 ev->data_type_id = data_type_id;
416 ev->data_type_signature = data_type_signature;
417 ev->cb = cb;
419
420 // Switch the head
422}
423
427void uavcan_broadcast(struct uavcan_iface_t *iface, uint64_t data_type_signature, uint16_t data_type_id,
428 uint8_t priority, const void *payload,
429 uint16_t payload_len)
430{
431 if (!iface->initialized) { return; }
432
433 chMtxLock(&iface->mutex);
434 canardBroadcast(&iface->canard,
435 data_type_signature,
436 data_type_id, &iface->transfer_id,
437 priority, payload, payload_len);
438 chMtxUnlock(&iface->mutex);
439 chEvtBroadcast(&iface->tx_request);
440}
struct abi_struct * next
Definition abi_common.h:70
abi_callback cb
Definition abi_common.h:69
static THD_WORKING_AREA(wa_thd_spi1, SPI_THREAD_STACK_SIZE)
void(* uavcan_callback)(struct uavcan_iface_t *iface, CanardRxTransfer *transfer)
Generic uavcan callback definition.
Definition uavcan.h:57
Main uavcan event structure for registering/calling callbacks.
Definition uavcan.h:60
static float p[2][2]
uint16_t foo
Definition main_demo5.c:58
abi_event ev
Definition rssi.c:45
CANDriver * can_driver
Definition uavcan.h:35
void * thread_tx_wa
Definition uavcan.h:42
size_t thread_tx_wa_size
Definition uavcan.h:45
uavcan interface structure
Definition uavcan.h:34
static THD_FUNCTION(uavcan_rx, p)
Definition uavcan.c:94
void uavcan_init(void)
Initialize all uavcan interfaces.
Definition uavcan.c:383
static bool shouldAcceptTransfer(const CanardInstance *ins, uint64_t *out_data_type_signature, uint16_t data_type_id, CanardTransferType transfer_type, uint8_t source_node_id)
If we should accept this transfer.
Definition uavcan.c:219
static void uavcanInitIface(struct uavcan_iface_t *iface)
Initialize uavcan interface.
Definition uavcan.c:354
static bool uavcanConfigureIface(struct uavcan_iface_t *iface)
Try to compute the timing registers for the can interface and set the configuration.
Definition uavcan.c:240
void uavcan_broadcast(struct uavcan_iface_t *iface, uint64_t data_type_signature, uint16_t data_type_id, uint8_t priority, const void *payload, uint16_t payload_len)
Broadcast an uavcan message to a specific interface.
Definition uavcan.c:427
static uavcan_event * uavcan_event_hd
Definition uavcan.c:37
void uavcan_bind(uint16_t data_type_id, uint64_t data_type_signature, uavcan_event *ev, uavcan_callback cb)
Bind to a receiving message from uavcan.
Definition uavcan.c:412
static void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer)
Whenever a valid and 'accepted' transfer is received.
Definition uavcan.c:204
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
unsigned long long uint64_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
int transfer(const Mat *from, const image_t *to)