Paparazzi UAS  v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
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 
48 static THD_WORKING_AREA(uavcan1_rx_wa, 1024 * 2);
49 static THD_WORKING_AREA(uavcan1_tx_wa, 1024 * 2);
50 
51 struct 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),
57  .thread_tx_wa = uavcan1_tx_wa,
58  .thread_tx_wa_size = sizeof(uavcan1_tx_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 
74 static THD_WORKING_AREA(uavcan2_rx_wa, 1024 * 2);
75 static THD_WORKING_AREA(uavcan2_tx_wa, 1024 * 2);
76 
77 struct 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),
83  .thread_tx_wa = uavcan2_tx_wa,
84  .thread_tx_wa_size = sizeof(uavcan2_tx_wa),
85  .node_id = UAVCAN_CAN2_NODE_ID,
86  .transfer_id = 0,
87  .initialized = false
88 };
89 #endif
90 
91 /*
92  * Receiver thread.
93  */
94 static THD_FUNCTION(uavcan_rx, p)
95 {
96  event_listener_t el;
97  CANRxFrame rx_msg;
98  CanardCANFrame rx_frame;
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) {
123  rx_frame.id = CANARD_CAN_FRAME_EFF | rx_msg.EID;
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  */
140 static THD_FUNCTION(uavcan_tx, p)
141 {
142  event_listener_t txc, txe, txr;
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) {
152  eventmask_t evts = chEvtWaitAnyTimeout(ALL_EVENTS, TIME_MS2I(100));
153  // Succesfull transmit
154  if (evts == 0) {
155  continue;
156  }
157 
158  // Transmit error
159  if (evts & EVENT_MASK(1)) {
160  chEvtGetAndClearFlags(&txe);
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);
192  chThdSleepMilliseconds(++err_cnt);
193  chMtxLock(&iface->mutex);
194  continue;
195  }
196  }
197  chMtxUnlock(&iface->mutex);
198  }
199 }
200 
204 static void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer)
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 
219 static bool shouldAcceptTransfer(const CanardInstance *ins __attribute__((unused)),
220  uint64_t *out_data_type_signature,
221  uint16_t data_type_id,
222  CanardTransferType transfer_type __attribute__((unused)),
223  uint8_t source_node_id __attribute__((unused)))
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 
240 static bool uavcanConfigureIface(struct uavcan_iface_t *iface)
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)
248  const uint32_t pclk = STM32_FDCANCLK;
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.
282  uint8_t bs1_bs2_sum = max_quanta_per_bit - 1;
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;
315  uint8_t bs2 = bs1_bs2_sum - bs1;
316  uint16_t sample_point_permill = 1000 * (1 + bs1) / (1 + bs1 + bs2);
317 
318 // Second attempt with rounding to zero
319  if (sample_point_permill > MaxSamplePointLocation) {
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 
354 static void uavcanInitIface(struct uavcan_iface_t *iface)
355 {
356  // First try to configure abort if failed
357  if (!uavcanConfigureIface(iface)) {
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 
383 void 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
394  uavcanInitIface(&uavcan1);
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
405  uavcanInitIface(&uavcan2);
406 #endif
407 }
408 
412 void 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 
427 void 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 }
THD_FUNCTION
static THD_FUNCTION(uavcan_rx, p)
Definition: uavcan.c:94
uint32_t
unsigned int uint32_t
Typedef defining 32 bit unsigned int type.
Definition: vl53l1_types.h:78
uint8_t
unsigned char uint8_t
Typedef defining 8 bit unsigned char type.
Definition: vl53l1_types.h:98
transfer
int transfer(const Mat *from, const image_t *to)
Definition: wedgebug_opencv.cpp:40
uint64_t
unsigned long long uint64_t
Definition: vl53l1_types.h:72
uavcan_broadcast
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
uavcan_iface_t::tx_request
event_source_t tx_request
Definition: uavcan.h:39
uavcan_iface_t::transfer_id
uint8_t transfer_id
Definition: uavcan.h:52
uavcan_iface_t::canard
CanardInstance canard
Definition: uavcan.h:49
uavcan_iface_t::mutex
mutex_t mutex
Definition: uavcan.h:40
uavcan_iface_t::can_cfg
CANConfig can_cfg
Definition: uavcan.h:37
uavcan_iface_t::can_driver
CANDriver * can_driver
Definition: uavcan.h:35
uavcan_event_t
Main uavcan event structure for registering/calling callbacks.
Definition: uavcan.h:60
uavcan_iface_t::can_baudrate
uint32_t can_baudrate
Definition: uavcan.h:36
uavcanInitIface
static void uavcanInitIface(struct uavcan_iface_t *iface)
Initialize uavcan interface.
Definition: uavcan.c:354
abi_struct::next
struct abi_struct * next
Definition: abi_common.h:69
uavcan_callback
void(* uavcan_callback)(struct uavcan_iface_t *iface, CanardRxTransfer *transfer)
Generic uavcan callback definition.
Definition: uavcan.h:57
uavcan_iface_t::thread_rx_wa_size
size_t thread_rx_wa_size
Definition: uavcan.h:44
uavcan_bind
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
onTransferReceived
static void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer)
Whenever a valid and 'accepted' transfer is received.
Definition: uavcan.c:204
uavcan.h
uavcan_iface_t
uavcan interface structure
Definition: uavcan.h:34
uavcan_iface_t::thread_rx_wa
void * thread_rx_wa
Definition: uavcan.h:41
ev
abi_event ev
Definition: rssi.c:45
uavcan_iface_t::thread_tx_wa_size
size_t thread_tx_wa_size
Definition: uavcan.h:45
uavcan_init
void uavcan_init(void)
Initialize all uavcan interfaces.
Definition: uavcan.c:383
shouldAcceptTransfer
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
uavcanConfigureIface
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
uavcan_iface_t::canard_memory_pool
uint8_t canard_memory_pool[1024 *2]
Definition: uavcan.h:50
uavcan_iface_t::initialized
bool initialized
Definition: uavcan.h:53
abi_struct::cb
abi_callback cb
Definition: abi_common.h:68
uavcan_iface_t::node_id
uint8_t node_id
Definition: uavcan.h:48
uavcan_event_hd
static uavcan_event * uavcan_event_hd
Definition: uavcan.c:37
uavcan_iface_t::thread_tx_wa
void * thread_tx_wa
Definition: uavcan.h:42
uint16_t
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
THD_WORKING_AREA
THD_WORKING_AREA(wa_thd_ap, THD_WORKING_AREA_MAIN)
p
static float p[2][2]
Definition: ins_alt_float.c:268