29 #ifndef UAVCAN_NODE_ID
30 #define UAVCAN_NODE_ID 100
33 #ifndef UAVCAN_BAUDRATE
34 #define UAVCAN_BAUDRATE 1000000
40 #ifndef UAVCAN_CAN1_NODE_ID
41 #define UAVCAN_CAN1_NODE_ID UAVCAN_NODE_ID
44 #ifndef UAVCAN_CAN1_BAUDRATE
45 #define UAVCAN_CAN1_BAUDRATE UAVCAN_BAUDRATE
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)
58 .thread_rx_wa = uavcan1_rx_wa,
59 .thread_rx_wa_size =
sizeof(uavcan1_rx_wa),
62 .node_id = UAVCAN_CAN1_NODE_ID,
69 #ifndef UAVCAN_CAN2_NODE_ID
70 #define UAVCAN_CAN2_NODE_ID UAVCAN_NODE_ID
73 #ifndef UAVCAN_CAN2_BAUDRATE
74 #define UAVCAN_CAN2_BAUDRATE UAVCAN_BAUDRATE
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)
87 .thread_rx_wa = uavcan2_rx_wa,
88 .thread_rx_wa_size =
sizeof(uavcan2_rx_wa),
91 .node_id = UAVCAN_CAN2_NODE_ID,
104 CanardCANFrame rx_frame;
107 chRegSetThreadName(
"uavcan_rx");
108 chEvtRegister(&iface->
can_driver->rxfull_event, &el, EVENT_MASK(0));
110 if (chEvtWaitAnyTimeout(ALL_EVENTS, TIME_MS2I(100)) == 0) {
113 chMtxLock(&iface->
mutex);
116 while (canReceive(iface->
can_driver, CAN_ANY_MAILBOX, &rx_msg, TIME_IMMEDIATE) == MSG_OK) {
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;
122 rx_frame.id = CANARD_CAN_FRAME_EFF | rx_msg.EID;
124 rx_frame.id = rx_msg.SID;
127 canardHandleRxFrame(&iface->
canard, &rx_frame, timestamp);
129 chMtxUnlock(&iface->
mutex);
131 chEvtUnregister(&iface->
can_driver->rxfull_event, &el);
139 event_listener_t txc, txe, txr;
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));
149 eventmask_t evts = chEvtWaitAnyTimeout(ALL_EVENTS, TIME_MS2I(100));
156 if (evts & EVENT_MASK(1)) {
157 chEvtGetAndClearFlags(&txe);
161 chMtxLock(&iface->
mutex);
162 for (
const CanardCANFrame *txf = NULL; (txf = canardPeekTxQueue(&iface->
canard)) != NULL;) {
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)) {
171 canardPopTxQueue(&iface->
canard);
176 while (canardPeekTxQueue(&iface->
canard)) { canardPopTxQueue(&iface->
canard); }
181 chMtxUnlock(&iface->
mutex);
182 chThdSleepMilliseconds(++err_cnt);
183 chMtxLock(&iface->
mutex);
187 chMtxUnlock(&iface->
mutex);
200 if (
transfer->data_type_id ==
ev->data_type_id) {
212 CanardTransferType transfer_type __attribute__((unused)),
213 uint8_t source_node_id __attribute__((unused)))
218 if (data_type_id ==
ev->data_type_id) {
219 *out_data_type_signature =
ev->data_type_signature;
233 chMtxObjectInit(&iface->
mutex);
247 chThdCreateStatic(iface->thread_tx_wa, iface->thread_tx_wa_size, NORMALPRIO + 7, uavcan_tx, (
void *)iface);
248 iface->initialized =
true;
270 ev->data_type_id = data_type_id;
271 ev->data_type_signature = data_type_signature;
283 uint8_t priority,
const void *payload,
288 chMtxLock(&iface->
mutex);
289 canardBroadcast(&iface->
canard,
292 priority, payload, payload_len);
293 chMtxUnlock(&iface->
mutex);