Paparazzi UAS v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
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#include "mcu_periph/can.h"
30
31#ifndef UAVCAN_NODE_ID
32#define UAVCAN_NODE_ID 100
33#endif
34
35#ifndef UAVCAN_BAUDRATE
36#define UAVCAN_BAUDRATE 1000000
37#endif
38
40
41#if UAVCAN_USE_CAN1
42#ifndef UAVCAN_CAN1_NODE_ID
43#define UAVCAN_CAN1_NODE_ID UAVCAN_NODE_ID
44#endif
45
46#ifndef UAVCAN_CAN1_BAUDRATE
47#define UAVCAN_CAN1_BAUDRATE UAVCAN_BAUDRATE
48#endif
49
50
51struct uavcan_iface_t uavcan1 = {
52 .can_net = {.can_ifindex = 1},
53 .can_baudrate = UAVCAN_CAN1_BAUDRATE,
54 .node_id = UAVCAN_CAN1_NODE_ID,
55 .transfer_id = 0,
56 .initialized = false
57};
58#endif
59
60#if UAVCAN_USE_CAN2
61#ifndef UAVCAN_CAN2_NODE_ID
62#define UAVCAN_CAN2_NODE_ID UAVCAN_NODE_ID
63#endif
64
65#ifndef UAVCAN_CAN2_BAUDRATE
66#define UAVCAN_CAN2_BAUDRATE UAVCAN_BAUDRATE
67#endif
68
69
70struct uavcan_iface_t uavcan2 = {
71 .can_net = {.can_ifindex = 2},
72 .can_baudrate = UAVCAN_CAN2_BAUDRATE,
73 .node_id = UAVCAN_CAN2_NODE_ID,
74 .transfer_id = 0,
75 .initialized = false
76};
77#endif
78
79
80static void can_frame_cb(struct pprzcan_frame* rx_msg, UNUSED struct pprzaddr_can* src_addr, void* user_data) {
81 struct uavcan_iface_t *iface = (struct uavcan_iface_t *)user_data;
82
83 pprz_mtx_lock(&iface->mutex);
84
85 CanardCANFrame rx_frame;
86 memcpy(rx_frame.data, rx_msg->data, 8);
87 rx_frame.data_len = rx_msg->len;
88 if (rx_msg->can_id & CAN_FRAME_EFF) {
89 rx_frame.id = CANARD_CAN_FRAME_EFF | (rx_msg->can_id & CAN_EID_MASK);
90 } else {
91 rx_frame.id = rx_msg->can_id & CAN_SID_MASK;
92 }
93
94 // Let canard handle the frame
95 canardHandleRxFrame(&iface->canard, &rx_frame, rx_msg->timestamp);
96
97 pprz_mtx_unlock(&iface->mutex);
98}
99
100/*
101 * Transmitter thread.
102 */
103static void uavcan_tx(void* p)
104{
105 struct uavcan_iface_t *iface = (struct uavcan_iface_t *)p;
106 uint8_t err_cnt = 0;
107
108 while (true) {
109 pprz_bsem_wait(&iface->bsem);
110
111 pprz_mtx_lock(&iface->mutex);
112 for (const CanardCANFrame *txf = NULL; (txf = canardPeekTxQueue(&iface->canard)) != NULL;) {
113 struct pprzcan_frame tx_msg;
114 memcpy(tx_msg.data, txf->data, 8);
115 tx_msg.len = txf->data_len;
117
118 if (!can_transmit_frame(&tx_msg, &iface->can_net)) {
119 err_cnt = 0;
120 canardPopTxQueue(&iface->canard);
121 } else {
122 // After 5 retries giveup and clean full queue
123 if (err_cnt >= 5) {
124 err_cnt = 0;
125 while (canardPeekTxQueue(&iface->canard)) { canardPopTxQueue(&iface->canard); }
126 continue;
127 }
128
129 // Timeout - just exit and try again later
130 pprz_mtx_unlock(&iface->mutex);
132 pprz_mtx_lock(&iface->mutex);
133 continue;
134 }
135 }
136 pprz_mtx_unlock(&iface->mutex);
137 }
138}
139
144{
145 struct uavcan_iface_t *iface = (struct uavcan_iface_t *)ins->user_reference;
146
147 // Go through all registered callbacks and call function callback if found
148 for (uavcan_event *ev = uavcan_event_hd; ev; ev = ev->next) {
149 if (transfer->data_type_id == ev->data_type_id) {
150 ev->cb(iface, transfer);
151 }
152 }
153}
154
160 uint16_t data_type_id,
163{
164
165 // Go through all registered callbacks and return signature if found
166 for (uavcan_event *ev = uavcan_event_hd; ev; ev = ev->next) {
167 if (data_type_id == ev->data_type_id) {
168 *out_data_type_signature = ev->data_type_signature;
169 return true;
170 }
171 }
172 // No callback found return
173 return false;
174}
175
176
181{
182 pprz_mtx_init(&iface->mutex);
183 pprz_bsem_init(&iface->bsem, true);
184
185 // Initialize canard
186 canardInit(&iface->canard, iface->canard_memory_pool, sizeof(iface->canard_memory_pool),
188 // Update the uavcan node ID
189 canardSetLocalNodeID(&iface->canard, iface->node_id);
190
191 // Start the receiver and transmitter thread
192 can_register_callback(can_frame_cb, &iface->can_net, (void *)iface);
193
194 if(!pprz_thread_create(&iface->thread_tx, 2048, "uavcan_tx", NORMALPRIO+7, uavcan_tx, (void *)iface)) {
195 iface->initialized = true;
196 }
197}
198
202void uavcan_init(void)
203{
204#if UAVCAN_USE_CAN1
206#endif
207#if UAVCAN_USE_CAN2
209#endif
210}
211
215void uavcan_bind(uint16_t data_type_id, uint64_t data_type_signature, uavcan_event *ev, uavcan_callback cb)
216{
217 // Configure the event
218 ev->data_type_id = data_type_id;
219 ev->data_type_signature = data_type_signature;
220 ev->cb = cb;
222
223 // Switch the head
225}
226
230void uavcan_broadcast(struct uavcan_iface_t *iface, uint64_t data_type_signature, uint16_t data_type_id,
231 uint8_t priority, const void *payload,
232 uint16_t payload_len)
233{
234 if (!iface->initialized) { return; }
235 pprz_mtx_lock(&iface->mutex);
236 canardBroadcast(&iface->canard,
237 data_type_signature,
238 data_type_id, &iface->transfer_id,
239 priority, payload, payload_len);
240 pprz_mtx_unlock(&iface->mutex);
241 pprz_bsem_signal(&iface->bsem);
242}
struct abi_struct * next
Definition abi_common.h:70
abi_callback cb
Definition abi_common.h:69
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
uint8_t len
Definition can.h:74
int can_ifindex
Definition can.h:83
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:149
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)
void(* uavcan_callback)(struct uavcan_iface_t *iface, CanardRxTransfer *transfer)
Generic uavcan callback definition.
Definition uavcan.h:58
Main uavcan event structure for registering/calling callbacks.
Definition uavcan.h:61
static float p[2][2]
uint16_t foo
Definition main_demo5.c:58
abi_event ev
Definition rssi.c:45
struct pprzaddr_can can_net
Definition uavcan.h:36
uavcan interface structure
Definition uavcan.h:35
static void can_frame_cb(struct pprzcan_frame *rx_msg, UNUSED struct pprzaddr_can *src_addr, void *user_data)
Definition uavcan.c:80
void uavcan_init(void)
Initialize all uavcan interfaces.
Definition uavcan.c:202
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:158
static void uavcanInitIface(struct uavcan_iface_t *iface)
Initialize uavcan interface.
Definition uavcan.c:180
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:230
static void uavcan_tx(void *p)
Definition uavcan.c:103
static uavcan_event * uavcan_event_hd
Definition uavcan.c:39
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:215
static void onTransferReceived(CanardInstance *ins, CanardRxTransfer *transfer)
Whenever a valid and 'accepted' transfer is received.
Definition uavcan.c:143
unsigned short uint16_t
Typedef defining 16 bit unsigned short 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)