Paparazzi UAS  v6.1.0_stable
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_cfg = {
54  CAN_MCR_ABOM | CAN_MCR_AWUM | CAN_MCR_TXFP,
55  CAN_BTR_SJW(0) | CAN_BTR_TS2(1) |
56  CAN_BTR_TS1(14) | CAN_BTR_BRP((STM32_PCLK1 / 18) / UAVCAN_CAN1_BAUDRATE - 1)
57  },
58  .thread_rx_wa = uavcan1_rx_wa,
59  .thread_rx_wa_size = sizeof(uavcan1_rx_wa),
60  .thread_tx_wa = uavcan1_tx_wa,
61  .thread_tx_wa_size = sizeof(uavcan1_tx_wa),
62  .node_id = UAVCAN_CAN1_NODE_ID,
63  .transfer_id = 0,
64  .initialized = false
65 };
66 #endif
67 
68 #if UAVCAN_USE_CAN2
69 #ifndef UAVCAN_CAN2_NODE_ID
70 #define UAVCAN_CAN2_NODE_ID UAVCAN_NODE_ID
71 #endif
72 
73 #ifndef UAVCAN_CAN2_BAUDRATE
74 #define UAVCAN_CAN2_BAUDRATE UAVCAN_BAUDRATE
75 #endif
76 
77 static THD_WORKING_AREA(uavcan2_rx_wa, 1024 * 2);
78 static THD_WORKING_AREA(uavcan2_tx_wa, 1024 * 2);
79 
80 struct uavcan_iface_t uavcan2 = {
81  .can_driver = &CAND2,
82  .can_cfg = {
83  CAN_MCR_ABOM | CAN_MCR_AWUM | CAN_MCR_TXFP,
84  CAN_BTR_SJW(0) | CAN_BTR_TS2(1) |
85  CAN_BTR_TS1(14) | CAN_BTR_BRP((STM32_PCLK1 / 18) / UAVCAN_CAN2_BAUDRATE - 1)
86  },
87  .thread_rx_wa = uavcan2_rx_wa,
88  .thread_rx_wa_size = sizeof(uavcan2_rx_wa),
89  .thread_tx_wa = uavcan2_tx_wa,
90  .thread_tx_wa_size = sizeof(uavcan2_tx_wa),
91  .node_id = UAVCAN_CAN2_NODE_ID,
92  .transfer_id = 0,
93  .initialized = false
94 };
95 #endif
96 
97 /*
98  * Receiver thread.
99  */
100 static THD_FUNCTION(uavcan_rx, p)
101 {
102  event_listener_t el;
103  CANRxFrame rx_msg;
104  CanardCANFrame rx_frame;
105  struct uavcan_iface_t *iface = (struct uavcan_iface_t *)p;
106 
107  chRegSetThreadName("uavcan_rx");
108  chEvtRegister(&iface->can_driver->rxfull_event, &el, EVENT_MASK(0));
109  while (true) {
110  if (chEvtWaitAnyTimeout(ALL_EVENTS, TIME_MS2I(100)) == 0) {
111  continue;
112  }
113  chMtxLock(&iface->mutex);
114 
115  // Wait until a CAN message is received
116  while (canReceive(iface->can_driver, CAN_ANY_MAILBOX, &rx_msg, TIME_IMMEDIATE) == MSG_OK) {
117  // Process message.
118  const uint32_t timestamp = TIME_I2US(chVTGetSystemTimeX());
119  memcpy(rx_frame.data, rx_msg.data8, 8);
120  rx_frame.data_len = rx_msg.DLC;
121  if (rx_msg.IDE) {
122  rx_frame.id = CANARD_CAN_FRAME_EFF | rx_msg.EID;
123  } else {
124  rx_frame.id = rx_msg.SID;
125  }
126  // Let canard handle the frame
127  canardHandleRxFrame(&iface->canard, &rx_frame, timestamp);
128  }
129  chMtxUnlock(&iface->mutex);
130  }
131  chEvtUnregister(&iface->can_driver->rxfull_event, &el);
132 }
133 
134 /*
135  * Transmitter thread.
136  */
137 static THD_FUNCTION(uavcan_tx, p)
138 {
139  event_listener_t txc, txe, txr;
140  struct uavcan_iface_t *iface = (struct uavcan_iface_t *)p;
141  uint8_t err_cnt = 0;
142 
143  chRegSetThreadName("uavcan_tx");
144  chEvtRegister(&iface->can_driver->txempty_event, &txc, EVENT_MASK(0));
145  chEvtRegister(&iface->can_driver->error_event, &txe, EVENT_MASK(1));
146  chEvtRegister(&iface->tx_request, &txr, EVENT_MASK(2));
147 
148  while (true) {
149  eventmask_t evts = chEvtWaitAnyTimeout(ALL_EVENTS, TIME_MS2I(100));
150  // Succesfull transmit
151  if (evts == 0) {
152  continue;
153  }
154 
155  // Transmit error
156  if (evts & EVENT_MASK(1)) {
157  chEvtGetAndClearFlags(&txe);
158  continue;
159  }
160 
161  chMtxLock(&iface->mutex);
162  for (const CanardCANFrame *txf = NULL; (txf = canardPeekTxQueue(&iface->canard)) != NULL;) {
163  CANTxFrame tx_msg;
164  tx_msg.DLC = txf->data_len;
165  memcpy(tx_msg.data8, txf->data, 8);
166  tx_msg.EID = txf->id & CANARD_CAN_EXT_ID_MASK;
167  tx_msg.IDE = CAN_IDE_EXT;
168  tx_msg.RTR = CAN_RTR_DATA;
169  if (!canTryTransmitI(iface->can_driver, CAN_ANY_MAILBOX, &tx_msg)) {
170  err_cnt = 0;
171  canardPopTxQueue(&iface->canard);
172  } else {
173  // After 5 retries giveup and clean full queue
174  if (err_cnt >= 5) {
175  err_cnt = 0;
176  while (canardPeekTxQueue(&iface->canard)) { canardPopTxQueue(&iface->canard); }
177  continue;
178  }
179 
180  // Timeout - just exit and try again later
181  chMtxUnlock(&iface->mutex);
182  chThdSleepMilliseconds(++err_cnt);
183  chMtxLock(&iface->mutex);
184  continue;
185  }
186  }
187  chMtxUnlock(&iface->mutex);
188  }
189 }
190 
194 static void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer)
195 {
196  struct uavcan_iface_t *iface = (struct uavcan_iface_t *)ins->user_reference;
197 
198  // Go through all registered callbacks and call function callback if found
199  for (uavcan_event *ev = uavcan_event_hd; ev; ev = ev->next) {
200  if (transfer->data_type_id == ev->data_type_id) {
201  ev->cb(iface, transfer);
202  }
203  }
204 }
205 
209 static bool shouldAcceptTransfer(const CanardInstance *ins __attribute__((unused)),
210  uint64_t *out_data_type_signature,
211  uint16_t data_type_id,
212  CanardTransferType transfer_type __attribute__((unused)),
213  uint8_t source_node_id __attribute__((unused)))
214 {
215 
216  // Go through all registered callbacks and return signature if found
217  for (uavcan_event *ev = uavcan_event_hd; ev; ev = ev->next) {
218  if (data_type_id == ev->data_type_id) {
219  *out_data_type_signature = ev->data_type_signature;
220  return true;
221  }
222  }
223  // No callback found return
224  return false;
225 }
226 
230 static void uavcanInitIface(struct uavcan_iface_t *iface)
231 {
232  // Initialize mutexes/events for multithread locking
233  chMtxObjectInit(&iface->mutex);
234  chEvtObjectInit(&iface->tx_request);
235 
236  // Initialize canard
237  canardInit(&iface->canard, iface->canard_memory_pool, sizeof(iface->canard_memory_pool),
239  // Update the uavcan node ID
240  canardSetLocalNodeID(&iface->canard, iface->node_id);
241 
242  // Start the can interface
243  canStart(iface->can_driver, &iface->can_cfg);
244 
245  // Start the receiver and transmitter thread
246  chThdCreateStatic(iface->thread_rx_wa, iface->thread_rx_wa_size, NORMALPRIO + 8, uavcan_rx, (void *)iface);
247  chThdCreateStatic(iface->thread_tx_wa, iface->thread_tx_wa_size, NORMALPRIO + 7, uavcan_tx, (void *)iface);
248  iface->initialized = true;
249 }
250 
254 void uavcan_init(void)
255 {
256 #if UAVCAN_USE_CAN1
257  uavcanInitIface(&uavcan1);
258 #endif
259 #if UAVCAN_USE_CAN2
260  uavcanInitIface(&uavcan2);
261 #endif
262 }
263 
267 void uavcan_bind(uint16_t data_type_id, uint64_t data_type_signature, uavcan_event *ev, uavcan_callback cb)
268 {
269  // Configure the event
270  ev->data_type_id = data_type_id;
271  ev->data_type_signature = data_type_signature;
272  ev->cb = cb;
274 
275  // Switch the head
277 }
278 
282 void uavcan_broadcast(struct uavcan_iface_t *iface, uint64_t data_type_signature, uint16_t data_type_id,
283  uint8_t priority, const void *payload,
284  uint16_t payload_len)
285 {
286  if (!iface->initialized) { return; }
287 
288  chMtxLock(&iface->mutex);
289  canardBroadcast(&iface->canard,
290  data_type_signature,
291  data_type_id, &iface->transfer_id,
292  priority, payload, payload_len);
293  chMtxUnlock(&iface->mutex);
294  chEvtBroadcast(&iface->tx_request);
295 }
THD_FUNCTION
static THD_FUNCTION(uavcan_rx, p)
Definition: uavcan.c:100
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:282
uavcan_iface_t::tx_request
event_source_t tx_request
Definition: uavcan.h:38
uavcan_iface_t::transfer_id
uint8_t transfer_id
Definition: uavcan.h:51
uavcan_iface_t::canard
CanardInstance canard
Definition: uavcan.h:48
uavcan_iface_t::mutex
mutex_t mutex
Definition: uavcan.h:39
uavcan_iface_t::can_cfg
CANConfig can_cfg
Definition: uavcan.h:36
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:59
uavcanInitIface
static void uavcanInitIface(struct uavcan_iface_t *iface)
Initialize uavcan interface.
Definition: uavcan.c:230
abi_struct::next
struct abi_struct * next
Definition: abi_common.h:68
uavcan_callback
void(* uavcan_callback)(struct uavcan_iface_t *iface, CanardRxTransfer *transfer)
Generic uavcan callback definition.
Definition: uavcan.h:56
uavcan_iface_t::thread_rx_wa_size
size_t thread_rx_wa_size
Definition: uavcan.h:43
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:267
onTransferReceived
static void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer)
Whenever a valid and 'accepted' transfer is received.
Definition: uavcan.c:194
uavcan.h
THD_WORKING_AREA
static THD_WORKING_AREA(wa_thd_spi1, SPI_THREAD_STACK_SIZE)
uavcan_iface_t
uavcan interface structure
Definition: uavcan.h:34
uavcan_iface_t::thread_rx_wa
void * thread_rx_wa
Definition: uavcan.h:40
ev
abi_event ev
Definition: rssi.c:45
uavcan_iface_t::thread_tx_wa_size
size_t thread_tx_wa_size
Definition: uavcan.h:44
uavcan_init
void uavcan_init(void)
Initialize all uavcan interfaces.
Definition: uavcan.c:254
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:209
uavcan_iface_t::canard_memory_pool
uint8_t canard_memory_pool[1024 *2]
Definition: uavcan.h:49
uavcan_iface_t::initialized
bool initialized
Definition: uavcan.h:52
abi_struct::cb
abi_callback cb
Definition: abi_common.h:67
uavcan_iface_t::node_id
uint8_t node_id
Definition: uavcan.h:47
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:41
uint16_t
unsigned short uint16_t
Typedef defining 16 bit unsigned short type.
Definition: vl53l1_types.h:88
p
static float p[2][2]
Definition: ins_alt_float.c:268