Paparazzi UAS v7.1_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 */
26
27#include "uavcan.h"
28#include "mcu_periph/can.h"
30#include "uavcan.protocol.NodeStatus.h"
31#include "uavcan_reporting.h"
32#include "mcu_periph/sys_time.h"
34
35#ifndef UAVCAN_NODE_ID
36#define UAVCAN_NODE_ID 100
37#endif
38
39#ifndef UAVCAN_BAUDRATE
40#define UAVCAN_BAUDRATE 1000000
41#endif
42
44
45#if UAVCAN_USE_CAN1
46#ifndef UAVCAN_CAN1_NODE_ID
47#define UAVCAN_CAN1_NODE_ID UAVCAN_NODE_ID
48#endif
49
50#ifndef UAVCAN_CAN1_BAUDRATE
51#define UAVCAN_CAN1_BAUDRATE UAVCAN_BAUDRATE
52#endif
53
54struct uavcan_iface_t uavcan1 = {
55 .can_net = {.can_ifindex = 1},
56 .can_baudrate = UAVCAN_CAN1_BAUDRATE,
57 .node_id = UAVCAN_CAN1_NODE_ID,
58 .initialized = false
59};
60#endif
61
62#if UAVCAN_USE_CAN2
63#ifndef UAVCAN_CAN2_NODE_ID
64#define UAVCAN_CAN2_NODE_ID UAVCAN_NODE_ID
65#endif
66
67#ifndef UAVCAN_CAN2_BAUDRATE
68#define UAVCAN_CAN2_BAUDRATE UAVCAN_BAUDRATE
69#endif
70
71
72struct uavcan_iface_t uavcan2 = {
73 .can_net = {.can_ifindex = 2},
74 .can_baudrate = UAVCAN_CAN2_BAUDRATE,
75 .node_id = UAVCAN_CAN2_NODE_ID,
76 .initialized = false
77};
78#endif
79
80
85static void can_frame_cb(struct pprzcan_frame* rx_msg, UNUSED struct pprzaddr_can* src_addr, void* user_data) {
86 struct uavcan_iface_t *iface = (struct uavcan_iface_t *)user_data;
87
88 struct uavcan_rx_frame rx = {0};
89 memcpy(rx.frame.data, rx_msg->data, 8);
90 rx.frame.data_len = rx_msg->len;
91 if (rx_msg->can_id & CAN_FRAME_EFF) {
92 rx.frame.id = CANARD_CAN_FRAME_EFF | (rx_msg->can_id & CAN_EID_MASK);
93 } else {
94 rx.frame.id = rx_msg->can_id & CAN_SID_MASK;
95 }
96 rx.timestamp = rx_msg->timestamp;
97
98 if (framed_ring_buffer_put(&iface->rx_queue, (const uint8_t *)&rx, sizeof(rx)) < 0) {
99 iface->rx_overflows++;
100 }
101}
102
104static void uavcan_handle_rx(struct uavcan_iface_t *iface)
105{
106 if (!iface->initialized) {
107 return;
108 }
109 struct uavcan_rx_frame rx;
110 while (framed_ring_buffer_get(&iface->rx_queue, (uint8_t *)&rx, sizeof(rx)) == sizeof(rx)) {
111 canardHandleRxFrame(&iface->canard, &rx.frame, rx.timestamp);
112 }
113}
114
115/*
116 * Transmitter thread.
117 */
118static void uavcan_tx(void* p)
119{
120 struct uavcan_iface_t *iface = (struct uavcan_iface_t *)p;
121 uint8_t err_cnt = 0;
122
123 while (true) {
124 // wait to be awaken
125 pprz_bsem_wait(&iface->bsem);
126
127 pprz_mtx_lock(&iface->tx_mutex);
128 for (const CanardCANFrame *txf = NULL; (txf = canardPeekTxQueue(&iface->canard)) != NULL;) {
129 struct pprzcan_frame tx_msg;
130 memcpy(tx_msg.data, txf->data, 8);
131 tx_msg.len = txf->data_len;
132 tx_msg.can_id = (txf->id & CANARD_CAN_EXT_ID_MASK) | CAN_FRAME_EFF;
133
134 if (!can_transmit_frame(&tx_msg, &iface->can_net)) {
135 err_cnt = 0;
136 canardPopTxQueue(&iface->canard);
137 } else {
138 // After 5 retries giveup and clean full queue
139 if (err_cnt >= 5) {
140 err_cnt = 0;
141 while (canardPeekTxQueue(&iface->canard)) { canardPopTxQueue(&iface->canard); }
142 continue;
143 }
144
145 // Timeout - just exit and try again later
146 pprz_mtx_unlock(&iface->tx_mutex);
147 sys_time_msleep(++err_cnt);
148 pprz_mtx_lock(&iface->tx_mutex);
149 continue;
150 }
151 }
152 pprz_mtx_unlock(&iface->tx_mutex);
153 }
154}
155
159static void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer)
160{
161 struct uavcan_iface_t *iface = (struct uavcan_iface_t *)ins->user_reference;
162 // Go through all registered callbacks and call function callback if found
163 for (uavcan_event *ev = uavcan_event_hd; ev; ev = ev->next) {
164 if (transfer->data_type_id == ev->data_type_id) {
165 ev->cb(iface, transfer);
166 }
167 }
168}
169
173static bool shouldAcceptTransfer(const CanardInstance *ins __attribute__((unused)),
174 uint64_t *out_data_type_signature,
175 uint16_t data_type_id,
176 CanardTransferType transfer_type __attribute__((unused)),
177 uint8_t source_node_id __attribute__((unused)))
178{
179
180 // Go through all registered callbacks and return signature if found
181 for (uavcan_event *ev = uavcan_event_hd; ev; ev = ev->next) {
182 if (data_type_id == ev->data_type_id) {
183 *out_data_type_signature = ev->data_type_signature;
184 return true;
185 }
186 }
187 return false;
188}
189
190
191
192#if CANARD_ALLOCATE_SEM
193
194void canard_allocate_sem_take(CanardPoolAllocator *allocator) {
195 pprz_bsem_t* bsem = (pprz_bsem_t*)allocator->semaphore;
197}
198
199void canard_allocate_sem_give(CanardPoolAllocator *allocator){
200 pprz_bsem_t* bsem = (pprz_bsem_t*)allocator->semaphore;
202}
203
204#endif
205
206
210static void uavcanInitIface(struct uavcan_iface_t *iface)
211{
212 pprz_bsem_init(&iface->bsem, true);
213 pprz_mtx_init(&iface->tx_mutex);
214
217
218 // Initialize canard
219 canardInit(&iface->canard, iface->canard_memory_pool, sizeof(iface->canard_memory_pool),
221
222
223#if CANARD_ALLOCATE_SEM
224 // must be initialized after canardInit
225 pprz_bsem_init(&iface->allocator_bsem, false);
226 iface->canard.allocator.semaphore = &iface->allocator_bsem;
227#endif
228
229 // Update the uavcan node ID
230 canardSetLocalNodeID(&iface->canard, iface->node_id);
231
232 // Start the receiver and transmitter thread
233 framed_ring_buffer_init(&iface->rx_queue, iface->rx_queue_buf, sizeof(iface->rx_queue_buf));
234 can_register_callback(can_frame_cb, &iface->can_net, (void *)iface);
235
236 if(!pprz_thread_create(&iface->thread_tx, 2048, "uavcan_tx", PPRZ_NORMAL_PRIO +7, uavcan_tx, (void *)iface)) {
237 iface->initialized = true;
238 }
239}
240
245{
246#if UAVCAN_USE_CAN1
247 uavcan_handle_rx(&uavcan1);
248#endif
249#if UAVCAN_USE_CAN2
250 uavcan_handle_rx(&uavcan2);
251#endif
252}
253
257void uavcan_init(void)
258{
259#if UAVCAN_USE_CAN1
260 uavcanInitIface(&uavcan1);
261#endif
262#if UAVCAN_USE_CAN2
263 uavcanInitIface(&uavcan2);
264#endif
267}
268
272void uavcan_bind(uint16_t data_type_id, uint64_t data_type_signature, uavcan_event *ev, uavcan_callback cb)
273{
274 // Configure the event
275 ev->data_type_id = data_type_id;
276 ev->data_type_signature = data_type_signature;
277 ev->cb = cb;
278 ev->next = uavcan_event_hd;
279
280 // Switch the head
282}
283
284static uint8_t* get_transfer_id(struct uavcan_iface_t *iface, uint8_t destination_node_id, CanardTxTransfer* transfer) {
285 uint32_t key = (destination_node_id << 24) | (iface->node_id <<16) | transfer->data_type_id;
286 uint8_t* transfer_id = (uint8_t*)kv_get(&iface->transfer_ids_store, key);
287
288 if (transfer_id == NULL) {
289 // key does not exists yet
290 uint8_t zero = 0;
291 kv_set(&iface->transfer_ids_store, key, &zero);
292 }
293
294 return transfer_id;
295}
296
300void uavcan_transfer(struct uavcan_iface_t *iface, CanardTxTransfer* transfer)
301{
302 if (!iface->initialized) { return; }
303 pprz_mtx_lock(&iface->tx_mutex);
304
305 transfer->transfer_type = CanardTransferTypeBroadcast;
306 transfer->inout_transfer_id = get_transfer_id(iface, 0, transfer);
307 if(canardBroadcastObj(&iface->canard, transfer) < 0) {
308 iface->nb_errors++;
309 }
310
311 pprz_mtx_unlock(&iface->tx_mutex);
312
313 // Wake Tx thread
314 pprz_bsem_signal(&iface->bsem);
315}
316
317
321void uavcan_request(struct uavcan_iface_t *iface, uint8_t destination_node_id, CanardTxTransfer* transfer)
322{
323 if (!iface->initialized) { return; }
324 pprz_mtx_lock(&iface->tx_mutex);
325
326 transfer->transfer_type = CanardTransferTypeRequest;
327 transfer->inout_transfer_id = get_transfer_id(iface, destination_node_id, transfer);
328 if(canardRequestOrRespondObj(&iface->canard, destination_node_id, transfer) < 0) {
329 iface->nb_errors++;
330 }
331
332 pprz_mtx_unlock(&iface->tx_mutex);
333
334 // Wake Tx thread
335 pprz_bsem_signal(&iface->bsem);
336}
337
338
339
340
345void uavcan_broadcast(struct uavcan_iface_t *iface, uint64_t data_type_signature, uint16_t data_type_id,
346 uint8_t priority, const void *payload,
347 uint16_t payload_len)
348{
349 CanardTxTransfer transfer;
350 canardInitTxTransfer(&transfer);
351 transfer.data_type_signature = data_type_signature;
352 transfer.data_type_id = data_type_id;
353 transfer.priority = priority;
354 transfer.payload = payload;
355 transfer.payload_len = payload_len;
356
357 uavcan_transfer(iface, &transfer);
358}
359
360
364void uavcan_response(struct uavcan_iface_t *iface, uint8_t destination_node_id, CanardTxTransfer* transfer)
365{
366 if (!iface->initialized) { return; }
367 pprz_mtx_lock(&iface->tx_mutex);
368
369 transfer->transfer_type = CanardTransferTypeResponse;
370 canardRequestOrRespondObj(&iface->canard, destination_node_id, transfer);
371
372 pprz_mtx_unlock(&iface->tx_mutex);
373
374 // Wake Tx thread
375 pprz_bsem_signal(&iface->bsem);
376}
int can_register_callback(can_rx_frame_callback_t callback, struct pprzaddr_can *src_addr, void *user_data)
Add a callback on received frames from an interface.
Definition can.c:88
uint32_t timestamp
Definition can.h:76
uint8_t len
Definition can.h:74
uint8_t data[SOCKETCAN_MAX_DLEN]
Definition can.h:77
socketcan_id_t can_id
Definition can.h:73
#define CAN_FRAME_EFF
Definition can.h:62
#define CAN_EID_MASK
Definition can.h:64
#define CAN_SID_MASK
Definition can.h:65
#define UNUSED(x)
int can_transmit_frame(struct pprzcan_frame *txframe, struct pprzaddr_can *addr)
Definition can_arch.c:159
void sys_time_msleep(uint32_t ms)
Sleep.
int pprz_mtx_unlock(pprz_mutex_t *mtx)
void pprz_bsem_init(pprz_bsem_t *bsem, bool taken)
int pprz_thread_create(pprz_thread_t *thread, size_t size, const char *name, uint8_t prio, void(*pf)(void *), void *arg)
Creates a new thread whose stack is dynamically allocated.
int pprz_mtx_init(pprz_mutex_t *mtx)
int pprz_mtx_lock(pprz_mutex_t *mtx)
void pprz_bsem_signal(pprz_bsem_t *bsem)
void pprz_bsem_wait(pprz_bsem_t *bsem)
#define PPRZ_NORMAL_PRIO
int framed_ring_buffer_put(struct framed_ring_buffer *cb, const uint8_t *buf, size_t len)
Copy buf in the circular buffer.
void framed_ring_buffer_init(struct framed_ring_buffer *cb, uint8_t *buffer, size_t len)
initialize a circular buffer.
int framed_ring_buffer_get(struct framed_ring_buffer *cb, uint8_t *buf, size_t len)
copy the next buffer available in cb to buf.
static float p[2][2]
int kv_set(kv_store_t *kv, uint32_t key, const void *value)
Sets a value for a given key.
Definition kv_store.c:79
void * kv_get(const kv_store_t *kv, uint32_t key)
Retrieves the value associated with a given key.
Definition kv_store.c:111
void kv_init(kv_store_t *kv, size_t capacity, size_t esize, uint32_t *keys, void *values, uint8_t *used)
Initializes a key-value store.
Definition kv_store.c:20
abi_event ev
Definition rssi.c:45
Architecture independent timing functions.
pprz_bsem_t bsem
Definition thd_test.c:34
struct pprzBSem pprz_bsem_t
Definition threads.h:22
static void can_frame_cb(struct pprzcan_frame *rx_msg, UNUSED struct pprzaddr_can *src_addr, void *user_data)
Called in the CAN receive thread: only queue the frame.
Definition uavcan.c:85
void uavcan_init(void)
Initialize all uavcan interfaces.
Definition uavcan.c:257
void uavcan_transfer(struct uavcan_iface_t *iface, CanardTxTransfer *transfer)
Definition uavcan.c:300
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:173
static void uavcanInitIface(struct uavcan_iface_t *iface)
Initialize uavcan interface.
Definition uavcan.c:210
static void uavcan_handle_rx(struct uavcan_iface_t *iface)
Let canard handle the queued frames of an interface, in the main thread.
Definition uavcan.c:104
void uavcan_request(struct uavcan_iface_t *iface, uint8_t destination_node_id, CanardTxTransfer *transfer)
Definition uavcan.c:321
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)
Legacy function Broadcast an uavcan message to a specific interface.
Definition uavcan.c:345
static void uavcan_tx(void *p)
Definition uavcan.c:118
static uavcan_event * uavcan_event_hd
Definition uavcan.c:43
void uavcan_rx_event(void)
Handle the received frames of all interfaces (main thread).
Definition uavcan.c:244
void uavcan_response(struct uavcan_iface_t *iface, uint8_t destination_node_id, CanardTxTransfer *transfer)
Definition uavcan.c:364
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:272
static void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer)
Whenever a valid and 'accepted' transfer is received.
Definition uavcan.c:159
static uint8_t * get_transfer_id(struct uavcan_iface_t *iface, uint8_t destination_node_id, CanardTxTransfer *transfer)
Definition uavcan.c:284
uint32_t timestamp
Reception time (us).
Definition uavcan.h:51
pprz_mutex_t tx_mutex
Definition uavcan.h:62
uint8_t transfer_ids_values[UAVCAN_TID_STORE_CAPACITY]
Definition uavcan.h:77
void(* uavcan_callback)(struct uavcan_iface_t *iface, CanardRxTransfer *transfer)
Generic uavcan callback definition.
Definition uavcan.h:89
bool initialized
Definition uavcan.h:85
uint8_t rx_queue_buf[UAVCAN_RX_BUFFER_SIZE]
Definition uavcan.h:82
uint32_t transfer_ids_keys[UAVCAN_TID_STORE_CAPACITY]
Definition uavcan.h:76
uint16_t rx_overflows
Frames dropped because the queue was full.
Definition uavcan.h:83
struct framed_ring_buffer rx_queue
Received frames, from the CAN receive thread to the main thread.
Definition uavcan.h:81
struct pprzaddr_can can_net
Definition uavcan.h:58
pprz_bsem_t bsem
Definition uavcan.h:65
CanardCANFrame frame
Definition uavcan.h:50
kv_store_t transfer_ids_store
Definition uavcan.h:79
#define UAVCAN_TID_STORE_CAPACITY
Definition uavcan.h:41
uint8_t transfer_ids_used[UAVCAN_TID_STORE_CAPACITY]
Definition uavcan.h:78
pprz_thread_t thread_tx
Definition uavcan.h:63
uint16_t nb_errors
Definition uavcan.h:73
uint8_t node_id
Definition uavcan.h:67
struct uavcan_event_t uavcan_event
Definition uavcan.h:98
CanardInstance canard
Definition uavcan.h:68
uint8_t canard_memory_pool[1024 *2]
Definition uavcan.h:69
void uavcan_allocator_init(void)
void uavcan_init_reporting()
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)