PX4 Firmware
PX4 Autopilot Software http://px4.io
uc_stm32_can.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
3  */
4 
5 #include <cassert>
6 #include <cstring>
7 #include <uavcan_stm32/can.hpp>
8 #include <uavcan_stm32/clock.hpp>
9 #include "internal.hpp"
10 
11 #if UAVCAN_STM32_NUTTX
12 # include <nuttx/arch.h>
13 # include <nuttx/irq.h>
14 # include <arch/board/board.h>
15 #else
16 # error "Unknown OS"
17 #endif
18 
19 #if UAVCAN_STM32_NUTTX
20 # if !defined(STM32_IRQ_CAN1TX) && !defined(STM32_IRQ_CAN1RX0)
21 # define STM32_IRQ_CAN1TX STM32_IRQ_USBHPCANTX
22 # define STM32_IRQ_CAN1RX0 STM32_IRQ_USBLPCANRX0
23 # endif
24 extern "C"
25 {
26  static int can1_irq(const int irq, void *, void *);
27 #if UAVCAN_STM32_NUM_IFACES > 1
28  static int can2_irq(const int irq, void *, void *);
29 #endif
30 }
31 #endif
32 
33 /* STM32F3's only CAN inteface does not have a number. */
34 #if defined(STM32F3XX)
35 #define RCC_APB1ENR_CAN1EN RCC_APB1ENR_CANEN
36 #define RCC_APB1RSTR_CAN1RST RCC_APB1RSTR_CANRST
37 #define CAN1_TX_IRQn CAN_TX_IRQn
38 #define CAN1_RX0_IRQn CAN_RX0_IRQn
39 #define CAN1_RX1_IRQn CAN_RX1_IRQn
40 #endif
41 
42 
43 namespace uavcan_stm32
44 {
45 namespace
46 {
47 
48 CanIface *ifaces[UAVCAN_STM32_NUM_IFACES] = {
49  UAVCAN_NULLPTR
50 #if UAVCAN_STM32_NUM_IFACES > 1
51  , UAVCAN_NULLPTR
52 #endif
53 };
54 
55 inline void handleTxInterrupt(uavcan::uint8_t iface_index)
56 {
57  UAVCAN_ASSERT(iface_index < UAVCAN_STM32_NUM_IFACES);
58  uavcan::uint64_t utc_usec = clock::getUtcUSecFromCanInterrupt();
59 
60  if (utc_usec > 0) {
61  utc_usec--;
62  }
63 
64  if (ifaces[iface_index] != UAVCAN_NULLPTR) {
65  ifaces[iface_index]->handleTxInterrupt(utc_usec);
66 
67  } else {
68  UAVCAN_ASSERT(0);
69  }
70 }
71 
72 inline void handleRxInterrupt(uavcan::uint8_t iface_index, uavcan::uint8_t fifo_index)
73 {
74  UAVCAN_ASSERT(iface_index < UAVCAN_STM32_NUM_IFACES);
75  uavcan::uint64_t utc_usec = clock::getUtcUSecFromCanInterrupt();
76 
77  if (utc_usec > 0) {
78  utc_usec--;
79  }
80 
81  if (ifaces[iface_index] != UAVCAN_NULLPTR) {
82  ifaces[iface_index]->handleRxInterrupt(fifo_index, utc_usec);
83 
84  } else {
85  UAVCAN_ASSERT(0);
86  }
87 }
88 
89 } // namespace
90 
91 /*
92  * CanIface::RxQueue
93  */
95 {
96  if (overflow_cnt_ < 0xFFFFFFFF) {
97  overflow_cnt_++;
98  }
99 }
100 
101 void CanIface::RxQueue::push(const uavcan::CanFrame &frame, const uint64_t &utc_usec, uavcan::CanIOFlags flags)
102 {
103  buf_[in_].frame = frame;
104  buf_[in_].utc_usec = utc_usec;
105  buf_[in_].flags = flags;
106  in_++;
107 
108  if (in_ >= capacity_) {
109  in_ = 0;
110  }
111 
112  len_++;
113 
114  if (len_ > capacity_) {
115  len_ = capacity_;
117  out_++;
118 
119  if (out_ >= capacity_) {
120  out_ = 0;
121  }
122  }
123 }
124 
125 void CanIface::RxQueue::pop(uavcan::CanFrame &out_frame, uavcan::uint64_t &out_utc_usec, uavcan::CanIOFlags &out_flags)
126 {
127  if (len_ > 0) {
128  out_frame = buf_[out_].frame;
129  out_utc_usec = buf_[out_].utc_usec;
130  out_flags = buf_[out_].flags;
131  out_++;
132 
133  if (out_ >= capacity_) {
134  out_ = 0;
135  }
136 
137  len_--;
138 
139  } else { UAVCAN_ASSERT(0); }
140 }
141 
143 {
144  in_ = 0;
145  out_ = 0;
146  len_ = 0;
147  overflow_cnt_ = 0;
148 }
149 
150 /*
151  * CanIface
152  */
153 const uavcan::uint32_t CanIface::TSR_ABRQx[CanIface::NumTxMailboxes] = {
157 };
158 
159 int CanIface::computeTimings(const uavcan::uint32_t target_bitrate, Timings &out_timings)
160 {
161  if (target_bitrate < 1) {
162  return -ErrInvalidBitRate;
163  }
164 
165  /*
166  * Hardware configuration
167  */
168 #if UAVCAN_STM32_NUTTX
169  const uavcan::uint32_t pclk = STM32_PCLK1_FREQUENCY;
170 #else
171 # error "Unknown OS"
172 #endif
173 
174  static const int MaxBS1 = 16;
175  static const int MaxBS2 = 8;
176 
177  /*
178  * Ref. "Automatic Baudrate Detection in CANopen Networks", U. Koppe, MicroControl GmbH & Co. KG
179  * CAN in Automation, 2003
180  *
181  * According to the source, optimal quanta per bit are:
182  * Bitrate Optimal Maximum
183  * 1000 kbps 8 10
184  * 500 kbps 16 17
185  * 250 kbps 16 17
186  * 125 kbps 16 17
187  */
188  const int max_quanta_per_bit = (target_bitrate >= 1000000) ? 10 : 17;
189 
190  UAVCAN_ASSERT(max_quanta_per_bit <= (MaxBS1 + MaxBS2));
191 
192  static const int MaxSamplePointLocation = 900;
193 
194  /*
195  * Computing (prescaler * BS):
196  * BITRATE = 1 / (PRESCALER * (1 / PCLK) * (1 + BS1 + BS2)) -- See the Reference Manual
197  * BITRATE = PCLK / (PRESCALER * (1 + BS1 + BS2)) -- Simplified
198  * let:
199  * BS = 1 + BS1 + BS2 -- Number of time quanta per bit
200  * PRESCALER_BS = PRESCALER * BS
201  * ==>
202  * PRESCALER_BS = PCLK / BITRATE
203  */
204  const uavcan::uint32_t prescaler_bs = pclk / target_bitrate;
205 
206  /*
207  * Searching for such prescaler value so that the number of quanta per bit is highest.
208  */
209  uavcan::uint8_t bs1_bs2_sum = uavcan::uint8_t(max_quanta_per_bit - 1);
210 
211  while ((prescaler_bs % (1 + bs1_bs2_sum)) != 0) {
212  if (bs1_bs2_sum <= 2) {
213  return -ErrInvalidBitRate; // No solution
214  }
215 
216  bs1_bs2_sum--;
217  }
218 
219  const uavcan::uint32_t prescaler = prescaler_bs / (1 + bs1_bs2_sum);
220 
221  if ((prescaler < 1U) || (prescaler > 1024U)) {
222  return -ErrInvalidBitRate; // No solution
223  }
224 
225  /*
226  * Now we have a constraint: (BS1 + BS2) == bs1_bs2_sum.
227  * We need to find the values so that the sample point is as close as possible to the optimal value.
228  *
229  * Solve[(1 + bs1)/(1 + bs1 + bs2) == 7/8, bs2] (* Where 7/8 is 0.875, the recommended sample point location *)
230  * {{bs2 -> (1 + bs1)/7}}
231  *
232  * Hence:
233  * bs2 = (1 + bs1) / 7
234  * bs1 = (7 * bs1_bs2_sum - 1) / 8
235  *
236  * Sample point location can be computed as follows:
237  * Sample point location = (1 + bs1) / (1 + bs1 + bs2)
238  *
239  * Since the optimal solution is so close to the maximum, we prepare two solutions, and then pick the best one:
240  * - With rounding to nearest
241  * - With rounding to zero
242  */
243  struct BsPair {
244  uavcan::uint8_t bs1;
245  uavcan::uint8_t bs2;
246  uavcan::uint16_t sample_point_permill;
247 
248  BsPair() :
249  bs1(0),
250  bs2(0),
251  sample_point_permill(0)
252  { }
253 
254  BsPair(uavcan::uint8_t bs1_bs2_sum, uavcan::uint8_t arg_bs1) :
255  bs1(arg_bs1),
256  bs2(uavcan::uint8_t(bs1_bs2_sum - bs1)),
257  sample_point_permill(uavcan::uint16_t(1000 * (1 + bs1) / (1 + bs1 + bs2)))
258  {
259  UAVCAN_ASSERT(bs1_bs2_sum > arg_bs1);
260  }
261 
262  bool isValid() const { return (bs1 >= 1) && (bs1 <= MaxBS1) && (bs2 >= 1) && (bs2 <= MaxBS2); }
263  };
264 
265  // First attempt with rounding to nearest
266  BsPair solution(bs1_bs2_sum, uavcan::uint8_t(((7 * bs1_bs2_sum - 1) + 4) / 8));
267 
268  if (solution.sample_point_permill > MaxSamplePointLocation) {
269  // Second attempt with rounding to zero
270  solution = BsPair(bs1_bs2_sum, uavcan::uint8_t((7 * bs1_bs2_sum - 1) / 8));
271  }
272 
273  /*
274  * Final validation
275  * Helpful Python:
276  * def sample_point_from_btr(x):
277  * assert 0b0011110010000000111111000000000 & x == 0
278  * ts2,ts1,brp = (x>>20)&7, (x>>16)&15, x&511
279  * return (1+ts1+1)/(1+ts1+1+ts2+1)
280  *
281  */
282  if ((target_bitrate != (pclk / (prescaler * (1 + solution.bs1 + solution.bs2)))) || !solution.isValid()) {
283  UAVCAN_ASSERT(0);
284  return -ErrLogic;
285  }
286 
287  UAVCAN_STM32_LOG("Timings: quanta/bit: %d, sample point location: %.1f%%",
288  int(1 + solution.bs1 + solution.bs2), float(solution.sample_point_permill) / 10.F);
289 
290  out_timings.prescaler = uavcan::uint16_t(prescaler - 1U);
291  out_timings.sjw = 0; // Which means one
292  out_timings.bs1 = uavcan::uint8_t(solution.bs1 - 1);
293  out_timings.bs2 = uavcan::uint8_t(solution.bs2 - 1);
294  return 0;
295 }
296 
297 uavcan::int16_t CanIface::send(const uavcan::CanFrame &frame, uavcan::MonotonicTime tx_deadline,
298  uavcan::CanIOFlags flags)
299 {
300  if (frame.isErrorFrame() || frame.dlc > 8) {
301  return -ErrUnsupportedFrame;
302  }
303 
304  /*
305  * Normally we should perform the same check as in @ref canAcceptNewTxFrame(), because
306  * it is possible that the highest-priority frame between select() and send() could have been
307  * replaced with a lower priority one due to TX timeout. But we don't do this check because:
308  *
309  * - It is a highly unlikely scenario.
310  *
311  * - Frames do not timeout on a properly functioning bus. Since frames do not timeout, the new
312  * frame can only have higher priority, which doesn't break the logic.
313  *
314  * - If high-priority frames are timing out in the TX queue, there's probably a lot of other
315  * issues to take care of before this one becomes relevant.
316  *
317  * - It takes CPU time. Not just CPU time, but critical section time, which is expensive.
318  */
319  CriticalSectionLocker lock;
320 
321  /*
322  * Seeking for an empty slot
323  */
324  uavcan::uint8_t txmailbox = 0xFF;
325 
326  if ((can_->TSR & bxcan::TSR_TME0) == bxcan::TSR_TME0) {
327  txmailbox = 0;
328 
329  } else if ((can_->TSR & bxcan::TSR_TME1) == bxcan::TSR_TME1) {
330  txmailbox = 1;
331 
332  } else if ((can_->TSR & bxcan::TSR_TME2) == bxcan::TSR_TME2) {
333  txmailbox = 2;
334 
335  } else {
336  return 0; // No transmission for you.
337  }
338 
339  peak_tx_mailbox_index_ = uavcan::max(peak_tx_mailbox_index_, txmailbox); // Statistics
340 
341  /*
342  * Setting up the mailbox
343  */
344  bxcan::TxMailboxType &mb = can_->TxMailbox[txmailbox];
345 
346  if (frame.isExtended()) {
347  mb.TIR = ((frame.id & uavcan::CanFrame::MaskExtID) << 3) | bxcan::TIR_IDE;
348 
349  } else {
350  mb.TIR = ((frame.id & uavcan::CanFrame::MaskStdID) << 21);
351  }
352 
353  if (frame.isRemoteTransmissionRequest()) {
354  mb.TIR |= bxcan::TIR_RTR;
355  }
356 
357  mb.TDTR = frame.dlc;
358 
359  mb.TDHR = (uavcan::uint32_t(frame.data[7]) << 24) |
360  (uavcan::uint32_t(frame.data[6]) << 16) |
361  (uavcan::uint32_t(frame.data[5]) << 8) |
362  (uavcan::uint32_t(frame.data[4]) << 0);
363  mb.TDLR = (uavcan::uint32_t(frame.data[3]) << 24) |
364  (uavcan::uint32_t(frame.data[2]) << 16) |
365  (uavcan::uint32_t(frame.data[1]) << 8) |
366  (uavcan::uint32_t(frame.data[0]) << 0);
367 
368  mb.TIR |= bxcan::TIR_TXRQ; // Go.
369 
370  /*
371  * Registering the pending transmission so we can track its deadline and loopback it as needed
372  */
373  TxItem &txi = pending_tx_[txmailbox];
374  txi.deadline = tx_deadline;
375  txi.frame = frame;
376  txi.loopback = (flags & uavcan::CanIOFlagLoopback) != 0;
377  txi.abort_on_error = (flags & uavcan::CanIOFlagAbortOnError) != 0;
378  txi.pending = true;
379  return 1;
380 }
381 
382 uavcan::int16_t CanIface::receive(uavcan::CanFrame &out_frame, uavcan::MonotonicTime &out_ts_monotonic,
383  uavcan::UtcTime &out_ts_utc, uavcan::CanIOFlags &out_flags)
384 {
385  out_ts_monotonic = clock::getMonotonic(); // High precision is not required for monotonic timestamps
386  uavcan::uint64_t utc_usec = 0;
387  {
388  CriticalSectionLocker lock;
389 
390  if (rx_queue_.getLength() == 0) {
391  return 0;
392  }
393 
394  rx_queue_.pop(out_frame, utc_usec, out_flags);
395  }
396  out_ts_utc = uavcan::UtcTime::fromUSec(utc_usec);
397  return 1;
398 }
399 
400 uavcan::int16_t CanIface::configureFilters(const uavcan::CanFilterConfig *filter_configs,
401  uavcan::uint16_t num_configs)
402 {
403  if (num_configs <= NumFilters) {
404  CriticalSectionLocker lock;
405 
407 
408  // Slave (CAN2) gets half of the filters
409  can_->FMR &= ~0x00003F00UL;
410  can_->FMR |= static_cast<uint32_t>(NumFilters) << 8;
411 
412  can_->FFA1R = 0x0AAAAAAA; // FIFO's are interleaved between filters
413  can_->FM1R = 0; // Identifier Mask mode
414  can_->FS1R = 0x7ffffff; // Single 32-bit for all
415 
416  const uint8_t filter_start_index = (self_index_ == 0) ? 0 : NumFilters;
417 
418  if (num_configs == 0) {
419  can_->FilterRegister[filter_start_index].FR1 = 0;
420  can_->FilterRegister[filter_start_index].FR2 = 0;
421  // We can't directly overwrite FA1R because that breaks the other CAN interface
422  can_->FA1R |= 1U << filter_start_index; // Other filters may still be enabled, we don't care
423 
424  } else {
425  for (uint8_t i = 0; i < NumFilters; i++) {
426  if (i < num_configs) {
427  uint32_t id = 0;
428  uint32_t mask = 0;
429 
430  const uavcan::CanFilterConfig *const cfg = filter_configs + i;
431 
432  if ((cfg->id & uavcan::CanFrame::FlagEFF) || !(cfg->mask & uavcan::CanFrame::FlagEFF)) {
433  id = (cfg->id & uavcan::CanFrame::MaskExtID) << 3;
434  mask = (cfg->mask & uavcan::CanFrame::MaskExtID) << 3;
435  id |= bxcan::RIR_IDE;
436 
437  } else {
438  id = (cfg->id & uavcan::CanFrame::MaskStdID) << 21; // Regular std frames, nothing fancy.
439  mask = (cfg->mask & uavcan::CanFrame::MaskStdID) << 21; // Boring.
440  }
441 
442  if (cfg->id & uavcan::CanFrame::FlagRTR) {
443  id |= bxcan::RIR_RTR;
444  }
445 
446  if (cfg->mask & uavcan::CanFrame::FlagEFF) {
447  mask |= bxcan::RIR_IDE;
448  }
449 
450  if (cfg->mask & uavcan::CanFrame::FlagRTR) {
451  mask |= bxcan::RIR_RTR;
452  }
453 
454  can_->FilterRegister[filter_start_index + i].FR1 = id;
455  can_->FilterRegister[filter_start_index + i].FR2 = mask;
456 
457  can_->FA1R |= (1 << (filter_start_index + i));
458 
459  } else {
460  can_->FA1R &= ~(1 << (filter_start_index + i));
461  }
462  }
463  }
464 
465  can_->FMR &= ~bxcan::FMR_FINIT;
466 
467  return 0;
468  }
469 
470  return -ErrFilterNumConfigs;
471 }
472 
473 bool CanIface::waitMsrINakBitStateChange(bool target_state)
474 {
475 #if UAVCAN_STM32_NUTTX
476  const unsigned Timeout = 1000;
477 #else
478  const unsigned Timeout = 2000000;
479 #endif
480 
481  for (unsigned wait_ack = 0; wait_ack < Timeout; wait_ack++) {
482  const bool state = (can_->MSR & bxcan::MSR_INAK) != 0;
483 
484  if (state == target_state) {
485  return true;
486  }
487 
488 #if UAVCAN_STM32_NUTTX
489  ::usleep(1000);
490 #endif
491  }
492 
493  return false;
494 }
495 
496 int CanIface::init(const uavcan::uint32_t bitrate, const OperatingMode mode)
497 {
498  /*
499  * We need to silence the controller in the first order, otherwise it may interfere with the following operations.
500  */
501  {
502  CriticalSectionLocker lock;
503 
504  can_->MCR &= ~bxcan::MCR_SLEEP; // Exit sleep mode
505  can_->MCR |= bxcan::MCR_INRQ; // Request init
506 
507  can_->IER = 0; // Disable interrupts while initialization is in progress
508  }
509 
510  if (!waitMsrINakBitStateChange(true)) {
511  UAVCAN_STM32_LOG("MSR INAK not set");
513  return -ErrMsrInakNotSet;
514  }
515 
516  /*
517  * Object state - interrupts are disabled, so it's safe to modify it now
518  */
519  rx_queue_.reset();
520  error_cnt_ = 0;
521  served_aborts_cnt_ = 0;
522  uavcan::fill_n(pending_tx_, NumTxMailboxes, TxItem());
524  had_activity_ = false;
525 
526  /*
527  * CAN timings for this bitrate
528  */
529  Timings timings;
530  const int timings_res = computeTimings(bitrate, timings);
531 
532  if (timings_res < 0) {
534  return timings_res;
535  }
536 
537  UAVCAN_STM32_LOG("Timings: presc=%u sjw=%u bs1=%u bs2=%u",
538  unsigned(timings.prescaler), unsigned(timings.sjw), unsigned(timings.bs1), unsigned(timings.bs2));
539 
540  /*
541  * Hardware initialization (the hardware has already confirmed initialization mode, see above)
542  */
544 
545  can_->BTR = ((timings.sjw & 3U) << 24) |
546  ((timings.bs1 & 15U) << 16) |
547  ((timings.bs2 & 7U) << 20) |
548  (timings.prescaler & 1023U) |
549  ((mode == SilentMode) ? bxcan::BTR_SILM : 0);
550 
551  can_->IER = bxcan::IER_TMEIE | // TX mailbox empty
552  bxcan::IER_FMPIE0 | // RX FIFO 0 is not empty
553  bxcan::IER_FMPIE1; // RX FIFO 1 is not empty
554 
555  can_->MCR &= ~bxcan::MCR_INRQ; // Leave init mode
556 
557  if (!waitMsrINakBitStateChange(false)) {
558  UAVCAN_STM32_LOG("MSR INAK not cleared");
560  return -ErrMsrInakNotCleared;
561  }
562 
563  /*
564  * Default filter configuration
565  */
566  if (self_index_ == 0) {
568 
569  can_->FMR &= 0xFFFFC0F1;
570  can_->FMR |= static_cast<uavcan::uint32_t>(NumFilters) << 8; // Slave (CAN2) gets half of the filters
571 
572  can_->FFA1R = 0; // All assigned to FIFO0 by default
573  can_->FM1R = 0; // Indentifier Mask mode
574 
575 #if UAVCAN_STM32_NUM_IFACES > 1
576  can_->FS1R = 0x7ffffff; // Single 32-bit for all
577  can_->FilterRegister[0].FR1 = 0; // CAN1 accepts everything
578  can_->FilterRegister[0].FR2 = 0;
579  can_->FilterRegister[NumFilters].FR1 = 0; // CAN2 accepts everything
581  can_->FA1R = 1 | (1 << NumFilters); // One filter per each iface
582 #else
583  can_->FS1R = 0x1fff;
584  can_->FilterRegister[0].FR1 = 0;
585  can_->FilterRegister[0].FR2 = 0;
586  can_->FA1R = 1;
587 #endif
588 
589  can_->FMR &= ~bxcan::FMR_FINIT;
590  }
591 
592  return 0;
593 }
594 
595 void CanIface::handleTxMailboxInterrupt(uavcan::uint8_t mailbox_index, bool txok, const uavcan::uint64_t utc_usec)
596 {
597  UAVCAN_ASSERT(mailbox_index < NumTxMailboxes);
598 
599  had_activity_ = had_activity_ || txok;
600 
601  TxItem &txi = pending_tx_[mailbox_index];
602 
603  if (txi.loopback && txok && txi.pending) {
604  rx_queue_.push(txi.frame, utc_usec, uavcan::CanIOFlagLoopback);
605  }
606 
607  txi.pending = false;
608 }
609 
610 void CanIface::handleTxInterrupt(const uavcan::uint64_t utc_usec)
611 {
612  // TXOK == false means that there was a hardware failure
613  if (can_->TSR & bxcan::TSR_RQCP0) {
614  const bool txok = can_->TSR & bxcan::TSR_TXOK0;
616  handleTxMailboxInterrupt(0, txok, utc_usec);
617  }
618 
619  if (can_->TSR & bxcan::TSR_RQCP1) {
620  const bool txok = can_->TSR & bxcan::TSR_TXOK1;
622  handleTxMailboxInterrupt(1, txok, utc_usec);
623  }
624 
625  if (can_->TSR & bxcan::TSR_RQCP2) {
626  const bool txok = can_->TSR & bxcan::TSR_TXOK2;
628  handleTxMailboxInterrupt(2, txok, utc_usec);
629  }
630 
631  update_event_.signalFromInterrupt();
632 
634 
635 }
636 
637 void CanIface::handleRxInterrupt(uavcan::uint8_t fifo_index, uavcan::uint64_t utc_usec)
638 {
639  UAVCAN_ASSERT(fifo_index < 2);
640 
641  volatile uavcan::uint32_t *const rfr_reg = (fifo_index == 0) ? &can_->RF0R : &can_->RF1R;
642 
643  if ((*rfr_reg & bxcan::RFR_FMP_MASK) == 0) {
644  UAVCAN_ASSERT(0); // Weird, IRQ is here but no data to read
645  return;
646  }
647 
648  /*
649  * Register overflow as a hardware error
650  */
651  if ((*rfr_reg & bxcan::RFR_FOVR) != 0) {
652  error_cnt_++;
653  }
654 
655  /*
656  * Read the frame contents
657  */
658  uavcan::CanFrame frame;
659  const bxcan::RxMailboxType &rf = can_->RxMailbox[fifo_index];
660 
661  if ((rf.RIR & bxcan::RIR_IDE) == 0) {
662  frame.id = uavcan::CanFrame::MaskStdID & (rf.RIR >> 21);
663 
664  } else {
665  frame.id = uavcan::CanFrame::MaskExtID & (rf.RIR >> 3);
666  frame.id |= uavcan::CanFrame::FlagEFF;
667  }
668 
669  if ((rf.RIR & bxcan::RIR_RTR) != 0) {
670  frame.id |= uavcan::CanFrame::FlagRTR;
671  }
672 
673  frame.dlc = rf.RDTR & 15;
674 
675  frame.data[0] = uavcan::uint8_t(0xFF & (rf.RDLR >> 0));
676  frame.data[1] = uavcan::uint8_t(0xFF & (rf.RDLR >> 8));
677  frame.data[2] = uavcan::uint8_t(0xFF & (rf.RDLR >> 16));
678  frame.data[3] = uavcan::uint8_t(0xFF & (rf.RDLR >> 24));
679  frame.data[4] = uavcan::uint8_t(0xFF & (rf.RDHR >> 0));
680  frame.data[5] = uavcan::uint8_t(0xFF & (rf.RDHR >> 8));
681  frame.data[6] = uavcan::uint8_t(0xFF & (rf.RDHR >> 16));
682  frame.data[7] = uavcan::uint8_t(0xFF & (rf.RDHR >> 24));
683 
684  *rfr_reg = bxcan::RFR_RFOM | bxcan::RFR_FOVR | bxcan::RFR_FULL; // Release FIFO entry we just read
685 
686  /*
687  * Store with timeout into the FIFO buffer and signal update event
688  */
689  rx_queue_.push(frame, utc_usec, 0);
690  had_activity_ = true;
691  update_event_.signalFromInterrupt();
692 
694 
695 }
696 
698 {
699  const uavcan::uint8_t lec = uavcan::uint8_t((can_->ESR & bxcan::ESR_LEC_MASK) >> bxcan::ESR_LEC_SHIFT);
700 
701  if (lec != 0) {
702  can_->ESR = 0;
703  error_cnt_++;
704 
705  // Serving abort requests
706  for (int i = 0; i < NumTxMailboxes; i++) { // Dear compiler, may I suggest you to unroll this loop please.
707  TxItem &txi = pending_tx_[i];
708 
709  if (txi.pending && txi.abort_on_error) {
710  can_->TSR = TSR_ABRQx[i];
711  txi.pending = false;
713  }
714  }
715  }
716 }
717 
718 void CanIface::discardTimedOutTxMailboxes(uavcan::MonotonicTime current_time)
719 {
720  CriticalSectionLocker lock;
721 
722  for (int i = 0; i < NumTxMailboxes; i++) {
723  TxItem &txi = pending_tx_[i];
724 
725  if (txi.pending && txi.deadline < current_time) {
726  can_->TSR = TSR_ABRQx[i]; // Goodnight sweet transmission
727  txi.pending = false;
728  error_cnt_++;
729  }
730  }
731 }
732 
733 bool CanIface::canAcceptNewTxFrame(const uavcan::CanFrame &frame) const
734 {
735  /*
736  * We can accept more frames only if the following conditions are satisfied:
737  * - There is at least one TX mailbox free (obvious enough);
738  * - The priority of the new frame is higher than priority of all TX mailboxes.
739  */
740  {
741  static const uavcan::uint32_t TME = bxcan::TSR_TME0 | bxcan::TSR_TME1 | bxcan::TSR_TME2;
742  const uavcan::uint32_t tme = can_->TSR & TME;
743 
744  if (tme == TME) { // All TX mailboxes are free (as in freedom).
745  return true;
746  }
747 
748  if (tme == 0) { // All TX mailboxes are busy transmitting.
749  return false;
750  }
751  }
752 
753  /*
754  * The second condition requires a critical section.
755  */
756  CriticalSectionLocker lock;
757 
758  for (int mbx = 0; mbx < NumTxMailboxes; mbx++) {
759  if (pending_tx_[mbx].pending && !frame.priorityHigherThan(pending_tx_[mbx].frame)) {
760  return false; // There's a mailbox whose priority is higher or equal the priority of the new frame.
761  }
762  }
763 
764  return true; // This new frame will be added to a free TX mailbox in the next @ref send().
765 }
766 
768 {
769  CriticalSectionLocker lock;
770  return rx_queue_.getLength() == 0;
771 }
772 
773 uavcan::uint64_t CanIface::getErrorCount() const
774 {
775  CriticalSectionLocker lock;
777 }
778 
780 {
781  CriticalSectionLocker lock;
782  return rx_queue_.getLength();
783 }
784 
786 {
787  CriticalSectionLocker lock;
788  const bool ret = had_activity_;
789  had_activity_ = false;
790  return ret;
791 }
792 
793 /*
794  * CanDriver
795  */
796 uavcan::CanSelectMasks CanDriver::makeSelectMasks(const uavcan::CanFrame * (& pending_tx)[uavcan::MaxCanIfaces]) const
797 {
798  uavcan::CanSelectMasks msk;
799 
800  // Iface 0
801  msk.read = if0_.isRxBufferEmpty() ? 0 : 1;
802 
803  if (pending_tx[0] != UAVCAN_NULLPTR) {
804  msk.write = if0_.canAcceptNewTxFrame(*pending_tx[0]) ? 1 : 0;
805  }
806 
807  // Iface 1
808 #if UAVCAN_STM32_NUM_IFACES > 1
809 
810  if (!if1_.isRxBufferEmpty()) {
811  msk.read |= 1 << 1;
812  }
813 
814  if (pending_tx[1] != UAVCAN_NULLPTR) {
815  if (if1_.canAcceptNewTxFrame(*pending_tx[1])) {
816  msk.write |= 1 << 1;
817  }
818  }
819 
820 #endif
821  return msk;
822 }
823 
825 {
826 #if UAVCAN_STM32_NUM_IFACES == 1
827  return !if0_.isRxBufferEmpty();
828 #elif UAVCAN_STM32_NUM_IFACES == 2
829  return !if0_.isRxBufferEmpty() || !if1_.isRxBufferEmpty();
830 #else
831 # error UAVCAN_STM32_NUM_IFACES
832 #endif
833 }
834 
835 uavcan::int16_t CanDriver::select(uavcan::CanSelectMasks &inout_masks,
836  const uavcan::CanFrame * (& pending_tx)[uavcan::MaxCanIfaces],
837  const uavcan::MonotonicTime blocking_deadline)
838 {
839  const uavcan::CanSelectMasks in_masks = inout_masks;
840  const uavcan::MonotonicTime time = clock::getMonotonic();
841 
842  if0_.discardTimedOutTxMailboxes(time); // Check TX timeouts - this may release some TX slots
843  {
844  CriticalSectionLocker cs_locker;
845  if0_.pollErrorFlagsFromISR();
846  }
847 
848 #if UAVCAN_STM32_NUM_IFACES > 1
849  if1_.discardTimedOutTxMailboxes(time);
850  {
851  CriticalSectionLocker cs_locker;
852  if1_.pollErrorFlagsFromISR();
853  }
854 #endif
855 
856  inout_masks = makeSelectMasks(pending_tx); // Check if we already have some of the requested events
857 
858  if ((inout_masks.read & in_masks.read) != 0 ||
859  (inout_masks.write & in_masks.write) != 0) {
860  return 1;
861  }
862 
863  (void)update_event_.wait(blocking_deadline - time); // Block until timeout expires or any iface updates
864  inout_masks = makeSelectMasks(pending_tx); // Return what we got even if none of the requested events are set
865  return 1; // Return value doesn't matter as long as it is non-negative
866 }
867 
868 
870 {
871  /*
872  * CAN1, CAN2
873  */
874  {
875  CriticalSectionLocker lock;
876 #if UAVCAN_STM32_NUTTX
877  modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_CAN1EN);
878  modifyreg32(STM32_RCC_APB1RSTR, 0, RCC_APB1RSTR_CAN1RST);
879  modifyreg32(STM32_RCC_APB1RSTR, RCC_APB1RSTR_CAN1RST, 0);
880 # if UAVCAN_STM32_NUM_IFACES > 1
881  modifyreg32(STM32_RCC_APB1ENR, 0, RCC_APB1ENR_CAN2EN);
882  modifyreg32(STM32_RCC_APB1RSTR, 0, RCC_APB1RSTR_CAN2RST);
883  modifyreg32(STM32_RCC_APB1RSTR, RCC_APB1RSTR_CAN2RST, 0);
884 # endif
885 #else
886  RCC->APB1ENR |= RCC_APB1ENR_CAN1EN;
887  RCC->APB1RSTR |= RCC_APB1RSTR_CAN1RST;
888  RCC->APB1RSTR &= ~RCC_APB1RSTR_CAN1RST;
889 # if UAVCAN_STM32_NUM_IFACES > 1
890  RCC->APB1ENR |= RCC_APB1ENR_CAN2EN;
891  RCC->APB1RSTR |= RCC_APB1RSTR_CAN2RST;
892  RCC->APB1RSTR &= ~RCC_APB1RSTR_CAN2RST;
893 # endif
894 #endif
895  }
896 
897  /*
898  * IRQ
899  */
900 #if UAVCAN_STM32_NUTTX
901 # define IRQ_ATTACH(irq, handler) \
902  { \
903  const int res = irq_attach(irq, handler, NULL); \
904  (void)res; \
905  assert(res >= 0); \
906  up_enable_irq(irq); \
907  }
908  IRQ_ATTACH(STM32_IRQ_CAN1TX, can1_irq);
909  IRQ_ATTACH(STM32_IRQ_CAN1RX0, can1_irq);
910  IRQ_ATTACH(STM32_IRQ_CAN1RX1, can1_irq);
911 # if UAVCAN_STM32_NUM_IFACES > 1
912  IRQ_ATTACH(STM32_IRQ_CAN2TX, can2_irq);
913  IRQ_ATTACH(STM32_IRQ_CAN2RX0, can2_irq);
914  IRQ_ATTACH(STM32_IRQ_CAN2RX1, can2_irq);
915 # endif
916 # undef IRQ_ATTACH
917 #endif
918 }
919 
920 int CanDriver::init(const uavcan::uint32_t bitrate, const CanIface::OperatingMode mode)
921 {
922  int res = 0;
923 
924  UAVCAN_STM32_LOG("Bitrate %lu mode %d", static_cast<unsigned long>(bitrate), static_cast<int>(mode));
925 
926  static bool initialized_once = false;
927 
928  if (!initialized_once) {
929  initialized_once = true;
930  UAVCAN_STM32_LOG("First initialization");
931  initOnce();
932  }
933 
934  /*
935  * CAN1
936  */
937  UAVCAN_STM32_LOG("Initing iface 0...");
938  ifaces[0] = &if0_; // This link must be initialized first,
939  res = if0_.init(bitrate, mode); // otherwise an IRQ may fire while the interface is not linked yet;
940 
941  if (res < 0) { // a typical race condition.
942  UAVCAN_STM32_LOG("Iface 0 init failed %i", res);
943  ifaces[0] = UAVCAN_NULLPTR;
944  goto fail;
945  }
946 
947  /*
948  * CAN2
949  */
950 #if UAVCAN_STM32_NUM_IFACES > 1
951  UAVCAN_STM32_LOG("Initing iface 1...");
952  ifaces[1] = &if1_; // Same thing here.
953  res = if1_.init(bitrate, mode);
954 
955  if (res < 0) {
956  UAVCAN_STM32_LOG("Iface 1 init failed %i", res);
957  ifaces[1] = UAVCAN_NULLPTR;
958  goto fail;
959  }
960 
961 #endif
962 
963  UAVCAN_STM32_LOG("CAN drv init OK");
964  UAVCAN_ASSERT(res >= 0);
965  return res;
966 
967 fail:
968  UAVCAN_STM32_LOG("CAN drv init failed %i", res);
969  UAVCAN_ASSERT(res < 0);
970  return res;
971 }
972 
973 CanIface *CanDriver::getIface(uavcan::uint8_t iface_index)
974 {
975  if (iface_index < UAVCAN_STM32_NUM_IFACES) {
976  return ifaces[iface_index];
977  }
978 
979  return UAVCAN_NULLPTR;
980 }
981 
983 {
984  bool ret = if0_.hadActivity();
985 #if UAVCAN_STM32_NUM_IFACES > 1
986  ret |= if1_.hadActivity();
987 #endif
988  return ret;
989 }
990 
991 } // namespace uavcan_stm32
992 
993 /*
994  * Interrupt handlers
995  */
996 extern "C"
997 {
998 
999 #if UAVCAN_STM32_NUTTX
1000 
1001  static int can1_irq(const int irq, void *, void *)
1002  {
1003  if (irq == STM32_IRQ_CAN1TX) {
1004  uavcan_stm32::handleTxInterrupt(0);
1005 
1006  } else if (irq == STM32_IRQ_CAN1RX0) {
1007  uavcan_stm32::handleRxInterrupt(0, 0);
1008 
1009  } else if (irq == STM32_IRQ_CAN1RX1) {
1010  uavcan_stm32::handleRxInterrupt(0, 1);
1011 
1012  } else {
1013  PANIC();
1014  }
1015 
1016  return 0;
1017  }
1018 
1019 # if UAVCAN_STM32_NUM_IFACES > 1
1020 
1021  static int can2_irq(const int irq, void *, void *)
1022  {
1023  if (irq == STM32_IRQ_CAN2TX) {
1024  uavcan_stm32::handleTxInterrupt(1);
1025 
1026  } else if (irq == STM32_IRQ_CAN2RX0) {
1027  uavcan_stm32::handleRxInterrupt(1, 0);
1028 
1029  } else if (irq == STM32_IRQ_CAN2RX1) {
1030  uavcan_stm32::handleRxInterrupt(1, 1);
1031 
1032  } else {
1033  PANIC();
1034  }
1035 
1036  return 0;
1037  }
1038 
1039 # endif
1040 
1041 #else // UAVCAN_STM32_NUTTX
1042 
1043 #if !defined(CAN1_TX_IRQHandler) ||\
1044  !defined(CAN1_RX0_IRQHandler) ||\
1045  !defined(CAN1_RX1_IRQHandler)
1046 # error "Misconfigured build"
1047 #endif
1048 
1049  UAVCAN_STM32_IRQ_HANDLER(CAN1_TX_IRQHandler);
1050  UAVCAN_STM32_IRQ_HANDLER(CAN1_TX_IRQHandler)
1051  {
1052  UAVCAN_STM32_IRQ_PROLOGUE();
1053  uavcan_stm32::handleTxInterrupt(0);
1054  UAVCAN_STM32_IRQ_EPILOGUE();
1055  }
1056 
1057  UAVCAN_STM32_IRQ_HANDLER(CAN1_RX0_IRQHandler);
1058  UAVCAN_STM32_IRQ_HANDLER(CAN1_RX0_IRQHandler)
1059  {
1060  UAVCAN_STM32_IRQ_PROLOGUE();
1061  uavcan_stm32::handleRxInterrupt(0, 0);
1062  UAVCAN_STM32_IRQ_EPILOGUE();
1063  }
1064 
1065  UAVCAN_STM32_IRQ_HANDLER(CAN1_RX1_IRQHandler);
1066  UAVCAN_STM32_IRQ_HANDLER(CAN1_RX1_IRQHandler)
1067  {
1068  UAVCAN_STM32_IRQ_PROLOGUE();
1069  uavcan_stm32::handleRxInterrupt(0, 1);
1070  UAVCAN_STM32_IRQ_EPILOGUE();
1071  }
1072 
1073 # if UAVCAN_STM32_NUM_IFACES > 1
1074 
1075 #if !defined(CAN2_TX_IRQHandler) ||\
1076  !defined(CAN2_RX0_IRQHandler) ||\
1077  !defined(CAN2_RX1_IRQHandler)
1078 # error "Misconfigured build"
1079 #endif
1080 
1081  UAVCAN_STM32_IRQ_HANDLER(CAN2_TX_IRQHandler);
1082  UAVCAN_STM32_IRQ_HANDLER(CAN2_TX_IRQHandler)
1083  {
1084  UAVCAN_STM32_IRQ_PROLOGUE();
1085  uavcan_stm32::handleTxInterrupt(1);
1086  UAVCAN_STM32_IRQ_EPILOGUE();
1087  }
1088 
1089  UAVCAN_STM32_IRQ_HANDLER(CAN2_RX0_IRQHandler);
1090  UAVCAN_STM32_IRQ_HANDLER(CAN2_RX0_IRQHandler)
1091  {
1092  UAVCAN_STM32_IRQ_PROLOGUE();
1093  uavcan_stm32::handleRxInterrupt(1, 0);
1094  UAVCAN_STM32_IRQ_EPILOGUE();
1095  }
1096 
1097  UAVCAN_STM32_IRQ_HANDLER(CAN2_RX1_IRQHandler);
1098  UAVCAN_STM32_IRQ_HANDLER(CAN2_RX1_IRQHandler)
1099  {
1100  UAVCAN_STM32_IRQ_PROLOGUE();
1101  uavcan_stm32::handleRxInterrupt(1, 1);
1102  UAVCAN_STM32_IRQ_EPILOGUE();
1103  }
1104 
1105 # endif
1106 #endif // UAVCAN_STM32_NUTTX
1107 
1108 } // extern "C"
uavcan::uint64_t utc_usec
Definition: can.hpp:33
constexpr unsigned long IER_FMPIE0
Definition: bxcan.hpp:144
uavcan::uint32_t served_aborts_cnt_
Definition: can.hpp:115
volatile uint32_t FMR
Definition: bxcan.hpp:59
constexpr unsigned long ESR_LEC_MASK
Definition: bxcan.hpp:164
uavcan::uint8_t in_
Definition: can.hpp:52
static enum @74 state
constexpr unsigned long MCR_AWUM
Definition: bxcan.hpp:89
constexpr unsigned long TSR_ABRQ2
Definition: bxcan.hpp:123
static const uavcan::int16_t ErrInvalidBitRate
Bit rate not supported.
Definition: can.hpp:20
constexpr unsigned long TSR_ABRQ1
Definition: bxcan.hpp:118
uavcan::CanIOFlags flags
Definition: can.hpp:35
volatile uint32_t TSR
Definition: bxcan.hpp:49
static const uavcan::int16_t ErrMsrInakNotCleared
INAK bit of the MSR register is not 0.
Definition: can.hpp:24
int init(const uavcan::uint32_t bitrate, const CanIface::OperatingMode mode)
Returns zero if OK.
RxMailboxType RxMailbox[2]
Definition: bxcan.hpp:57
uavcan::uint64_t error_cnt_
Definition: can.hpp:114
uavcan::uint16_t prescaler
Definition: can.hpp:80
BusEvent & update_event_
Definition: can.hpp:116
uavcan::uint64_t getUtcUSecFromCanInterrupt()
Single CAN iface.
Definition: can.hpp:46
static const uavcan::int16_t ErrUnsupportedFrame
Frame not supported (e.g. RTR, CAN FD, etc)
Definition: can.hpp:22
constexpr unsigned long ESR_LEC_SHIFT
Definition: bxcan.hpp:163
virtual uavcan::int16_t configureFilters(const uavcan::CanFilterConfig *filter_configs, uavcan::uint16_t num_configs)
void pop(uavcan::CanFrame &out_frame, uavcan::uint64_t &out_utc_usec, uavcan::CanIOFlags &out_flags)
volatile uint32_t MSR
Definition: bxcan.hpp:48
constexpr unsigned long RIR_IDE
Definition: bxcan.hpp:238
TxItem pending_tx_[NumTxMailboxes]
Definition: can.hpp:117
constexpr unsigned long TSR_TME1
Definition: bxcan.hpp:127
static const uavcan::int16_t ErrMsrInakNotSet
INAK bit of the MSR register is not 1.
Definition: can.hpp:23
constexpr unsigned long TIR_RTR
Definition: bxcan.hpp:198
constexpr unsigned long RFR_FOVR
Definition: bxcan.hpp:138
int init(const uavcan::uint32_t bitrate, const OperatingMode mode)
Initializes the hardware CAN controller.
bool hadActivity()
Whether at least one iface had at least one successful IO since previous call of this method...
bool hasReadableInterfaces() const
Whether there&#39;s at least one interface where receive() would return a frame.
constexpr unsigned long MCR_RESET
Definition: bxcan.hpp:92
virtual uavcan::int16_t receive(uavcan::CanFrame &out_frame, uavcan::MonotonicTime &out_ts_monotonic, uavcan::UtcTime &out_ts_utc, uavcan::CanIOFlags &out_flags)
uavcan::CanSelectMasks makeSelectMasks(const uavcan::CanFrame *(&pending_tx)[uavcan::MaxCanIfaces]) const
This function returns select masks indicating which interfaces are available for read/write.
virtual uavcan::uint64_t getErrorCount() const
Total number of hardware failures and other kinds of errors (e.g.
UAVCAN_STM32_IRQ_HANDLER(CAN1_TX_IRQHandler)
constexpr unsigned long TSR_TXOK1
Definition: bxcan.hpp:115
volatile uint32_t MCR
Definition: bxcan.hpp:47
static const uavcan::int16_t ErrLogic
Internal logic error.
Definition: can.hpp:21
virtual CanIface * getIface(uavcan::uint8_t iface_index)
bool waitMsrINakBitStateChange(bool target_state)
volatile uint32_t RF1R
Definition: bxcan.hpp:51
uavcan::uint8_t bs2
Definition: can.hpp:83
bool isRxBufferEmpty() const
constexpr unsigned long TSR_TME2
Definition: bxcan.hpp:128
FilterRegisterType FilterRegister[28]
Definition: bxcan.hpp:68
constexpr unsigned long MCR_SLEEP
Definition: bxcan.hpp:85
constexpr unsigned long IER_FMPIE1
Definition: bxcan.hpp:147
constexpr unsigned long TSR_TXOK0
Definition: bxcan.hpp:110
bool hadActivity()
Whether this iface had at least one successful IO since the previous call of this method...
TxMailboxType TxMailbox[3]
Definition: bxcan.hpp:56
volatile uint32_t RF0R
Definition: bxcan.hpp:50
uavcan::MonotonicTime deadline
Definition: can.hpp:94
constexpr unsigned long IER_TMEIE
Definition: bxcan.hpp:143
bxcan::CanType *const can_
Definition: can.hpp:113
uavcan::uint8_t sjw
Definition: can.hpp:81
uavcan::uint32_t overflow_cnt_
Definition: can.hpp:55
static const uavcan::uint32_t TSR_ABRQx[NumTxMailboxes]
Definition: can.hpp:110
uavcan::uint8_t bs1
Definition: can.hpp:82
void pollErrorFlagsFromISR()
This method is used to count errors and abort transmission on error if necessary. ...
virtual uavcan::int16_t send(const uavcan::CanFrame &frame, uavcan::MonotonicTime tx_deadline, uavcan::CanIOFlags flags)
constexpr unsigned long MSR_INAK
Definition: bxcan.hpp:97
constexpr unsigned long MCR_INRQ
Definition: bxcan.hpp:84
constexpr unsigned long RFR_RFOM
Definition: bxcan.hpp:139
volatile uint32_t FM1R
Definition: bxcan.hpp:60
constexpr unsigned long TSR_RQCP0
Definition: bxcan.hpp:109
const uavcan::uint8_t capacity_
Definition: can.hpp:51
constexpr unsigned long TIR_IDE
Definition: bxcan.hpp:199
unsigned getRxQueueLength() const
Returns the number of frames pending in the RX queue.
constexpr unsigned long BTR_SILM
Definition: bxcan.hpp:189
uavcan::uint8_t len_
Definition: can.hpp:54
constexpr _Tp max(_Tp a, _Tp b)
Definition: Limits.hpp:60
uavcan::uint8_t peak_tx_mailbox_index_
Definition: can.hpp:118
void handleTxInterrupt(uavcan::uint64_t utc_usec)
unsigned getLength() const
Definition: can.hpp:74
volatile uint32_t IER
Definition: bxcan.hpp:52
void handleTxMailboxInterrupt(uavcan::uint8_t mailbox_index, bool txok, uavcan::uint64_t utc_usec)
uavcan::MonotonicTime getMonotonic()
Returns current monotonic time since the moment when clock::init() was called.
volatile uint32_t BTR
Definition: bxcan.hpp:54
void handleRxInterrupt(uavcan::uint8_t fifo_index, uavcan::uint64_t utc_usec)
void discardTimedOutTxMailboxes(uavcan::MonotonicTime current_time)
#define IRQ_ATTACH(irq, handler)
constexpr unsigned long TSR_RQCP1
Definition: bxcan.hpp:114
uavcan::uint32_t getOverflowCount() const
Definition: can.hpp:76
int computeTimings(uavcan::uint32_t target_bitrate, Timings &out_timings)
uavcan::CanFrame frame
Definition: can.hpp:34
constexpr unsigned long TIR_TXRQ
Definition: bxcan.hpp:197
constexpr unsigned long RFR_FULL
Definition: bxcan.hpp:137
constexpr unsigned long RIR_RTR
Definition: bxcan.hpp:237
volatile uint32_t FA1R
Definition: bxcan.hpp:66
constexpr unsigned long TSR_TME0
Definition: bxcan.hpp:126
bool canAcceptNewTxFrame(const uavcan::CanFrame &frame) const
static const uavcan::int16_t ErrFilterNumConfigs
Number of filters is more than supported.
Definition: can.hpp:26
volatile uint32_t ESR
Definition: bxcan.hpp:53
volatile uint32_t FFA1R
Definition: bxcan.hpp:64
volatile uint32_t FS1R
Definition: bxcan.hpp:62
mode
Definition: vtol_type.h:76
uavcan::uint8_t out_
Definition: can.hpp:53
constexpr unsigned long FMR_FINIT
Definition: bxcan.hpp:277
constexpr unsigned long RFR_FMP_MASK
Definition: bxcan.hpp:136
virtual uavcan::int16_t select(uavcan::CanSelectMasks &inout_masks, const uavcan::CanFrame *(&pending_tx)[uavcan::MaxCanIfaces], uavcan::MonotonicTime blocking_deadline)
constexpr unsigned long MCR_ABOM
Definition: bxcan.hpp:90
uavcan::CanFrame frame
Definition: can.hpp:95
constexpr unsigned long TSR_TXOK2
Definition: bxcan.hpp:120
#define UAVCAN_STM32_LOG(...)
Debug output.
Definition: internal.hpp:27
constexpr unsigned long TSR_RQCP2
Definition: bxcan.hpp:119
constexpr unsigned long TSR_ABRQ0
Definition: bxcan.hpp:113
void push(const uavcan::CanFrame &frame, const uint64_t &utc_usec, uavcan::CanIOFlags flags)
const uavcan::uint8_t self_index_
Definition: can.hpp:119
CanRxItem *const buf_
Definition: can.hpp:50