Line data Source code
1 : #include "AP_Canard_iface.h"
2 : #include <AP_HAL/AP_HAL.h>
3 : #include <AP_CANManager/AP_CANManager.h>
4 : #if HAL_ENABLE_DRONECAN_DRIVERS
5 : #include <canard/handler_list.h>
6 : #include <canard/transfer_object.h>
7 : #include <AP_Math/AP_Math.h>
8 : #include <dronecan_msgs.h>
9 : extern const AP_HAL::HAL& hal;
10 : #include <canard.h>
11 : #include <AP_CANManager/AP_CANSensor.h>
12 :
13 : #define DEBUG_PKTS 0
14 :
15 : #define CANARD_MSG_TYPE_FROM_ID(x) ((uint16_t)(((x) >> 8U) & 0xFFFFU))
16 :
17 : DEFINE_HANDLER_LIST_HEADS();
18 : DEFINE_HANDLER_LIST_SEMAPHORES();
19 :
20 : DEFINE_TRANSFER_OBJECT_HEADS();
21 : DEFINE_TRANSFER_OBJECT_SEMAPHORES();
22 :
23 : #if AP_TEST_DRONECAN_DRIVERS
24 : CanardInterface* CanardInterface::canard_ifaces[] = {nullptr, nullptr, nullptr};
25 : CanardInterface CanardInterface::test_iface{2};
26 : uint8_t test_node_mem_area[1024];
27 : HAL_Semaphore test_iface_sem;
28 : #endif
29 :
30 36151 : void canard_allocate_sem_take(CanardPoolAllocator *allocator) {
31 36151 : if (allocator->semaphore == nullptr) {
32 5 : allocator->semaphore = NEW_NOTHROW HAL_Semaphore;
33 5 : if (allocator->semaphore == nullptr) {
34 : // out of memory
35 0 : CANARD_ASSERT(0);
36 0 : return;
37 : }
38 : }
39 36151 : ((HAL_Semaphore*)allocator->semaphore)->take_blocking();
40 : }
41 :
42 36151 : void canard_allocate_sem_give(CanardPoolAllocator *allocator) {
43 36151 : if (allocator->semaphore == nullptr) {
44 : // it should have been allocated by canard_allocate_sem_take
45 0 : CANARD_ASSERT(0);
46 0 : return;
47 : }
48 36151 : ((HAL_Semaphore*)allocator->semaphore)->give();
49 : }
50 :
51 5 : CanardInterface::CanardInterface(uint8_t iface_index) :
52 5 : Interface(iface_index) {
53 : #if AP_TEST_DRONECAN_DRIVERS
54 : if (iface_index < 3) {
55 : canard_ifaces[iface_index] = this;
56 : }
57 : if (iface_index == 0) {
58 : test_iface.init(test_node_mem_area, sizeof(test_node_mem_area), 125);
59 : }
60 : canardInitTxTransfer(&tx_transfer);
61 : #endif
62 5 : }
63 :
64 5 : void CanardInterface::init(void* mem_arena, size_t mem_arena_size, uint8_t node_id) {
65 5 : canardInit(&canard, mem_arena, mem_arena_size, onTransferReception, shouldAcceptTransfer, this);
66 5 : canardSetLocalNodeID(&canard, node_id);
67 5 : initialized = true;
68 5 : }
69 :
70 10201 : bool CanardInterface::broadcast(const Canard::Transfer &bcast_transfer) {
71 10201 : if (!initialized) {
72 0 : return false;
73 : }
74 10201 : WITH_SEMAPHORE(_sem_tx);
75 :
76 : #if AP_TEST_DRONECAN_DRIVERS
77 : if (this == &test_iface) {
78 : test_iface_sem.take_blocking();
79 : }
80 : #endif
81 :
82 20402 : tx_transfer = {
83 10201 : .transfer_type = bcast_transfer.transfer_type,
84 10201 : .data_type_signature = bcast_transfer.data_type_signature,
85 10201 : .data_type_id = bcast_transfer.data_type_id,
86 10201 : .inout_transfer_id = bcast_transfer.inout_transfer_id,
87 10201 : .priority = bcast_transfer.priority,
88 10201 : .payload = (const uint8_t*)bcast_transfer.payload,
89 10201 : .payload_len = uint16_t(bcast_transfer.payload_len),
90 : #if CANARD_ENABLE_CANFD
91 10201 : .canfd = bcast_transfer.canfd,
92 : #endif
93 10201 : .deadline_usec = AP_HAL::micros64() + (bcast_transfer.timeout_ms * 1000),
94 : #if CANARD_MULTI_IFACE
95 10201 : .iface_mask = uint8_t((1<<num_ifaces) - 1),
96 : #endif
97 : };
98 : // do canard broadcast
99 10201 : int16_t ret = canardBroadcastObj(&canard, &tx_transfer);
100 : #if AP_TEST_DRONECAN_DRIVERS
101 : if (this == &test_iface) {
102 : test_iface_sem.give();
103 : }
104 : #endif
105 10201 : if (ret <= 0) {
106 0 : protocol_stats.tx_errors++;
107 : } else {
108 10201 : protocol_stats.tx_frames += ret;
109 : }
110 10201 : return ret > 0;
111 10201 : }
112 :
113 10 : bool CanardInterface::request(uint8_t destination_node_id, const Canard::Transfer &req_transfer) {
114 10 : if (!initialized) {
115 0 : return false;
116 : }
117 10 : WITH_SEMAPHORE(_sem_tx);
118 :
119 20 : tx_transfer = {
120 10 : .transfer_type = req_transfer.transfer_type,
121 10 : .data_type_signature = req_transfer.data_type_signature,
122 10 : .data_type_id = req_transfer.data_type_id,
123 10 : .inout_transfer_id = req_transfer.inout_transfer_id,
124 10 : .priority = req_transfer.priority,
125 10 : .payload = (const uint8_t*)req_transfer.payload,
126 10 : .payload_len = uint16_t(req_transfer.payload_len),
127 : #if CANARD_ENABLE_CANFD
128 10 : .canfd = req_transfer.canfd,
129 : #endif
130 10 : .deadline_usec = AP_HAL::micros64() + (req_transfer.timeout_ms * 1000),
131 : #if CANARD_MULTI_IFACE
132 10 : .iface_mask = uint8_t((1<<num_ifaces) - 1),
133 : #endif
134 : };
135 : // do canard request
136 10 : int16_t ret = canardRequestOrRespondObj(&canard, destination_node_id, &tx_transfer);
137 10 : if (ret <= 0) {
138 0 : protocol_stats.tx_errors++;
139 : } else {
140 10 : protocol_stats.tx_frames += ret;
141 : }
142 10 : return ret > 0;
143 10 : }
144 :
145 0 : bool CanardInterface::respond(uint8_t destination_node_id, const Canard::Transfer &res_transfer) {
146 0 : if (!initialized) {
147 0 : return false;
148 : }
149 0 : WITH_SEMAPHORE(_sem_tx);
150 :
151 0 : tx_transfer = {
152 0 : .transfer_type = res_transfer.transfer_type,
153 0 : .data_type_signature = res_transfer.data_type_signature,
154 0 : .data_type_id = res_transfer.data_type_id,
155 0 : .inout_transfer_id = res_transfer.inout_transfer_id,
156 0 : .priority = res_transfer.priority,
157 0 : .payload = (const uint8_t*)res_transfer.payload,
158 0 : .payload_len = uint16_t(res_transfer.payload_len),
159 : #if CANARD_ENABLE_CANFD
160 0 : .canfd = res_transfer.canfd,
161 : #endif
162 0 : .deadline_usec = AP_HAL::micros64() + (res_transfer.timeout_ms * 1000),
163 : #if CANARD_MULTI_IFACE
164 0 : .iface_mask = uint8_t((1<<num_ifaces) - 1),
165 : #endif
166 : };
167 : // do canard respond
168 0 : int16_t ret = canardRequestOrRespondObj(&canard, destination_node_id, &tx_transfer);
169 0 : if (ret <= 0) {
170 0 : protocol_stats.tx_errors++;
171 : } else {
172 0 : protocol_stats.tx_frames += ret;
173 : }
174 0 : return ret > 0;
175 0 : }
176 :
177 62 : void CanardInterface::onTransferReception(CanardInstance* ins, CanardRxTransfer* transfer) {
178 62 : CanardInterface* iface = (CanardInterface*) ins->user_reference;
179 62 : iface->handle_message(*transfer);
180 62 : }
181 :
182 62 : bool CanardInterface::shouldAcceptTransfer(const CanardInstance* ins,
183 : uint64_t* out_data_type_signature,
184 : uint16_t data_type_id,
185 : CanardTransferType transfer_type,
186 : uint8_t source_node_id) {
187 62 : CanardInterface* iface = (CanardInterface*) ins->user_reference;
188 62 : return iface->accept_message(data_type_id, transfer_type, *out_data_type_signature);
189 : }
190 :
191 : #if AP_TEST_DRONECAN_DRIVERS
192 : void CanardInterface::processTestRx() {
193 : if (!test_iface.initialized) {
194 : return;
195 : }
196 : WITH_SEMAPHORE(test_iface_sem);
197 : for (const CanardCANFrame* txf = canardPeekTxQueue(&test_iface.canard); txf != NULL; txf = canardPeekTxQueue(&test_iface.canard)) {
198 : if (canard_ifaces[0]) {
199 : canardHandleRxFrame(&canard_ifaces[0]->canard, txf, AP_HAL::micros64());
200 : }
201 : canardPopTxQueue(&test_iface.canard);
202 : }
203 : }
204 : #endif
205 :
206 44563 : void CanardInterface::processTx(bool raw_commands_only = false) {
207 44563 : WITH_SEMAPHORE(_sem_tx);
208 :
209 53009 : for (uint8_t iface = 0; iface < num_ifaces; iface++) {
210 44563 : if (ifaces[iface] == NULL) {
211 0 : continue;
212 : }
213 44563 : auto txq = canard.tx_queue;
214 44563 : if (txq == nullptr) {
215 36117 : return;
216 : }
217 : // volatile as the value can change at any time during can interrupt
218 : // we need to ensure that this is not optimized
219 8446 : volatile const auto *stats = ifaces[iface]->get_statistics();
220 8446 : uint64_t last_transmit_us = stats==nullptr?0:stats->last_transmit_us;
221 8446 : bool iface_down = true;
222 8446 : if (stats == nullptr || (AP_HAL::micros64() - last_transmit_us) < 200000UL) {
223 : /*
224 : We were not able to queue the frame for
225 : sending. Only mark the send as failing if the
226 : interface is active. We consider an interface as
227 : active if it has had successful transmits for some time.
228 : */
229 8420 : iface_down = false;
230 : }
231 : // scan through list of pending transfers
232 : while (true) {
233 18064 : auto txf = &txq->frame;
234 18064 : if (raw_commands_only &&
235 0 : CANARD_MSG_TYPE_FROM_ID(txf->id) != UAVCAN_EQUIPMENT_ESC_RAWCOMMAND_ID &&
236 0 : CANARD_MSG_TYPE_FROM_ID(txf->id) != COM_HOBBYWING_ESC_RAWCOMMAND_ID) {
237 : // look at next transfer
238 0 : txq = txq->next;
239 0 : if (txq == nullptr) {
240 8446 : break;
241 : }
242 0 : continue;
243 : }
244 18064 : AP_HAL::CANFrame txmsg {};
245 18064 : txmsg.dlc = AP_HAL::CANFrame::dataLengthToDlc(txf->data_len);
246 18064 : memcpy(txmsg.data, txf->data, txf->data_len);
247 18064 : txmsg.id = (txf->id | AP_HAL::CANFrame::FlagEFF);
248 : #if HAL_CANFD_SUPPORTED
249 18064 : txmsg.canfd = txf->canfd;
250 : #endif
251 18064 : bool write = true;
252 18064 : bool read = false;
253 18064 : ifaces[iface]->select(read, write, &txmsg, 0);
254 18064 : if (!write) {
255 : // if there is no space then we need to start from the
256 : // top of the queue, so wait for the next loop
257 0 : if (!iface_down) {
258 0 : break;
259 : } else {
260 0 : txf->iface_mask &= ~(1U<<iface);
261 : }
262 18064 : } else if ((txf->iface_mask & (1U<<iface)) && (AP_HAL::micros64() < txf->deadline_usec)) {
263 : // try sending to interfaces, clearing the mask if we succeed
264 18063 : if (ifaces[iface]->send(txmsg, txf->deadline_usec, 0) > 0) {
265 18063 : txf->iface_mask &= ~(1U<<iface);
266 : } else {
267 : // if we fail to send then we try sending on next interface
268 0 : if (!iface_down) {
269 0 : break;
270 : } else {
271 0 : txf->iface_mask &= ~(1U<<iface);
272 : }
273 : }
274 : }
275 : // look at next transfer
276 18064 : txq = txq->next;
277 18064 : if (txq == nullptr) {
278 8446 : break;
279 : }
280 9618 : }
281 : }
282 :
283 44563 : }
284 :
285 152 : void CanardInterface::update_rx_protocol_stats(int16_t res)
286 : {
287 152 : switch (-res) {
288 152 : case CANARD_OK:
289 152 : protocol_stats.rx_frames++;
290 152 : break;
291 0 : case CANARD_ERROR_OUT_OF_MEMORY:
292 0 : protocol_stats.rx_error_oom++;
293 0 : break;
294 0 : case CANARD_ERROR_INTERNAL:
295 0 : protocol_stats.rx_error_internal++;
296 0 : break;
297 0 : case CANARD_ERROR_RX_INCOMPATIBLE_PACKET:
298 0 : protocol_stats.rx_ignored_not_wanted++;
299 0 : break;
300 0 : case CANARD_ERROR_RX_WRONG_ADDRESS:
301 0 : protocol_stats.rx_ignored_wrong_address++;
302 0 : break;
303 0 : case CANARD_ERROR_RX_NOT_WANTED:
304 0 : protocol_stats.rx_ignored_not_wanted++;
305 0 : break;
306 0 : case CANARD_ERROR_RX_MISSED_START:
307 0 : protocol_stats.rx_error_missed_start++;
308 0 : break;
309 0 : case CANARD_ERROR_RX_WRONG_TOGGLE:
310 0 : protocol_stats.rx_error_wrong_toggle++;
311 0 : break;
312 0 : case CANARD_ERROR_RX_UNEXPECTED_TID:
313 0 : protocol_stats.rx_ignored_unexpected_tid++;
314 0 : break;
315 0 : case CANARD_ERROR_RX_SHORT_FRAME:
316 0 : protocol_stats.rx_error_short_frame++;
317 0 : break;
318 0 : case CANARD_ERROR_RX_BAD_CRC:
319 0 : protocol_stats.rx_error_bad_crc++;
320 0 : break;
321 0 : default:
322 : // mark all other errors as internal
323 0 : protocol_stats.rx_error_internal++;
324 0 : break;
325 : }
326 152 : }
327 :
328 44563 : void CanardInterface::processRx() {
329 44563 : AP_HAL::CANFrame rxmsg;
330 89126 : for (uint8_t i=0; i<num_ifaces; i++) {
331 : while(true) {
332 44715 : if (ifaces[i] == NULL) {
333 44563 : break;
334 : }
335 44715 : bool read_select = true;
336 44715 : bool write_select = false;
337 44715 : ifaces[i]->select(read_select, write_select, nullptr, 0);
338 44715 : if (!read_select) { // No data pending
339 44563 : break;
340 : }
341 152 : CanardCANFrame rx_frame {};
342 :
343 : //palToggleLine(HAL_GPIO_PIN_LED);
344 : uint64_t timestamp;
345 : AP_HAL::CANIface::CanIOFlags flags;
346 152 : if (ifaces[i]->receive(rxmsg, timestamp, flags) <= 0) {
347 0 : break;
348 : }
349 :
350 152 : if (!rxmsg.isExtended()) {
351 : // 11 bit frame, see if we have a handler
352 0 : if (aux_11bit_driver != nullptr) {
353 0 : aux_11bit_driver->handle_frame(rxmsg);
354 : }
355 0 : continue;
356 : }
357 :
358 152 : rx_frame.data_len = AP_HAL::CANFrame::dlcToDataLength(rxmsg.dlc);
359 152 : memcpy(rx_frame.data, rxmsg.data, rx_frame.data_len);
360 : #if HAL_CANFD_SUPPORTED
361 152 : rx_frame.canfd = rxmsg.canfd;
362 : #endif
363 152 : rx_frame.id = rxmsg.id;
364 : #if CANARD_MULTI_IFACE
365 152 : rx_frame.iface_id = i;
366 : #endif
367 : {
368 152 : WITH_SEMAPHORE(_sem_rx);
369 :
370 152 : const int16_t res = canardHandleRxFrame(&canard, &rx_frame, timestamp);
371 152 : if (res == -CANARD_ERROR_RX_MISSED_START) {
372 : // this might remaining frames from a message that we don't accept, so check
373 : uint64_t dummy_signature;
374 0 : if (shouldAcceptTransfer(&canard,
375 : &dummy_signature,
376 0 : extractDataType(rx_frame.id),
377 : extractTransferType(rx_frame.id),
378 : 1)) { // doesn't matter what we pass here
379 0 : update_rx_protocol_stats(res);
380 : } else {
381 0 : protocol_stats.rx_ignored_not_wanted++;
382 : }
383 : } else {
384 152 : update_rx_protocol_stats(res);
385 : }
386 152 : }
387 152 : }
388 : }
389 44563 : }
390 :
391 21820 : void CanardInterface::process(uint32_t duration_ms) {
392 : #if AP_TEST_DRONECAN_DRIVERS
393 : const uint64_t deadline = AP_HAL::micros64() + duration_ms*1000;
394 : while (AP_HAL::micros64() < deadline) {
395 : processTestRx();
396 : hal.scheduler->delay_microseconds(1000);
397 : }
398 : #else
399 21820 : const uint64_t deadline = AP_HAL::micros64() + duration_ms*1000;
400 : while (true) {
401 44563 : processRx();
402 44563 : processTx();
403 : {
404 44563 : WITH_SEMAPHORE(_sem_rx);
405 44563 : WITH_SEMAPHORE(_sem_tx);
406 44563 : canardCleanupStaleTransfers(&canard, AP_HAL::micros64());
407 44563 : }
408 44563 : const uint64_t now = AP_HAL::micros64();
409 44563 : if (now < deadline) {
410 22744 : IGNORE_RETURN(sem_handle.wait(deadline - now));
411 : } else {
412 21819 : break;
413 : }
414 22743 : }
415 : #endif
416 21819 : }
417 :
418 5 : bool CanardInterface::add_interface(AP_HAL::CANIface *can_iface)
419 : {
420 5 : if (num_ifaces > HAL_NUM_CAN_IFACES) {
421 0 : return false;
422 : }
423 5 : if (can_iface == nullptr) {
424 0 : return false;
425 : }
426 5 : if (ifaces[num_ifaces] != nullptr) {
427 0 : return false;
428 : }
429 5 : ifaces[num_ifaces] = can_iface;
430 5 : if (ifaces[num_ifaces] == nullptr) {
431 0 : return false;
432 : }
433 5 : if (!can_iface->set_event_handle(&sem_handle)) {
434 0 : return false;
435 : }
436 5 : num_ifaces++;
437 5 : return true;
438 : }
439 :
440 : // add an 11 bit auxillary driver
441 0 : bool CanardInterface::add_11bit_driver(CANSensor *sensor)
442 : {
443 0 : if (aux_11bit_driver != nullptr) {
444 : // only allow one
445 0 : return false;
446 : }
447 0 : aux_11bit_driver = sensor;
448 0 : return true;
449 : }
450 :
451 : // handler for outgoing frames for auxillary drivers
452 0 : bool CanardInterface::write_aux_frame(AP_HAL::CANFrame &out_frame, const uint32_t timeout_us)
453 : {
454 0 : const uint64_t tx_deadline_us = AP_HAL::micros64() + timeout_us;
455 0 : bool ret = false;
456 0 : for (uint8_t iface = 0; iface < num_ifaces; iface++) {
457 0 : if (ifaces[iface] == NULL) {
458 0 : continue;
459 : }
460 0 : ret |= ifaces[iface]->send(out_frame, tx_deadline_us, 0) > 0;
461 : }
462 0 : return ret;
463 : }
464 :
465 : #endif // #if HAL_ENABLE_DRONECAN_DRIVERS
|
