Руслан Урядинский
/
libuavcan
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependents: UAVCAN UAVCAN_Subscriber
libuavcan_drivers/lpc11c24/driver/src/can.cpp@0:dfe6edabb8ec, 2018-04-14 (annotated)
- Committer:
- RuslanUrya
- Date:
- Sat Apr 14 10:25:32 2018 +0000
- Revision:
- 0:dfe6edabb8ec
Initial commit
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| RuslanUrya | 0:dfe6edabb8ec | 1 | /* |
| RuslanUrya | 0:dfe6edabb8ec | 2 | * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com> |
| RuslanUrya | 0:dfe6edabb8ec | 3 | */ |
| RuslanUrya | 0:dfe6edabb8ec | 4 | |
| RuslanUrya | 0:dfe6edabb8ec | 5 | #include <uavcan_lpc11c24/can.hpp> |
| RuslanUrya | 0:dfe6edabb8ec | 6 | #include <uavcan_lpc11c24/clock.hpp> |
| RuslanUrya | 0:dfe6edabb8ec | 7 | #include <uavcan/util/templates.hpp> |
| RuslanUrya | 0:dfe6edabb8ec | 8 | #include <chip.h> |
| RuslanUrya | 0:dfe6edabb8ec | 9 | #include "c_can.hpp" |
| RuslanUrya | 0:dfe6edabb8ec | 10 | #include "internal.hpp" |
| RuslanUrya | 0:dfe6edabb8ec | 11 | |
| RuslanUrya | 0:dfe6edabb8ec | 12 | /** |
| RuslanUrya | 0:dfe6edabb8ec | 13 | * The default value should be OK for any use case. |
| RuslanUrya | 0:dfe6edabb8ec | 14 | */ |
| RuslanUrya | 0:dfe6edabb8ec | 15 | #ifndef UAVCAN_LPC11C24_RX_QUEUE_LEN |
| RuslanUrya | 0:dfe6edabb8ec | 16 | # define UAVCAN_LPC11C24_RX_QUEUE_LEN 8 |
| RuslanUrya | 0:dfe6edabb8ec | 17 | #endif |
| RuslanUrya | 0:dfe6edabb8ec | 18 | |
| RuslanUrya | 0:dfe6edabb8ec | 19 | #if UAVCAN_LPC11C24_RX_QUEUE_LEN > 254 |
| RuslanUrya | 0:dfe6edabb8ec | 20 | # error UAVCAN_LPC11C24_RX_QUEUE_LEN is too large |
| RuslanUrya | 0:dfe6edabb8ec | 21 | #endif |
| RuslanUrya | 0:dfe6edabb8ec | 22 | |
| RuslanUrya | 0:dfe6edabb8ec | 23 | extern "C" void canRxCallback(std::uint8_t msg_obj_num); |
| RuslanUrya | 0:dfe6edabb8ec | 24 | extern "C" void canTxCallback(std::uint8_t msg_obj_num); |
| RuslanUrya | 0:dfe6edabb8ec | 25 | extern "C" void canErrorCallback(std::uint32_t error_info); |
| RuslanUrya | 0:dfe6edabb8ec | 26 | |
| RuslanUrya | 0:dfe6edabb8ec | 27 | namespace uavcan_lpc11c24 |
| RuslanUrya | 0:dfe6edabb8ec | 28 | { |
| RuslanUrya | 0:dfe6edabb8ec | 29 | namespace |
| RuslanUrya | 0:dfe6edabb8ec | 30 | { |
| RuslanUrya | 0:dfe6edabb8ec | 31 | /** |
| RuslanUrya | 0:dfe6edabb8ec | 32 | * Hardware message objects are allocated as follows: |
| RuslanUrya | 0:dfe6edabb8ec | 33 | * - 1 - Single TX object |
| RuslanUrya | 0:dfe6edabb8ec | 34 | * - 2..32 - RX objects |
| RuslanUrya | 0:dfe6edabb8ec | 35 | * TX priority is defined by the message object number, not by the CAN ID (chapter 16.7.3.5 of the user manual), |
| RuslanUrya | 0:dfe6edabb8ec | 36 | * hence we can't use more than one object because that would cause priority inversion on long transfers. |
| RuslanUrya | 0:dfe6edabb8ec | 37 | */ |
| RuslanUrya | 0:dfe6edabb8ec | 38 | constexpr unsigned NumberOfMessageObjects = 32; |
| RuslanUrya | 0:dfe6edabb8ec | 39 | constexpr unsigned NumberOfTxMessageObjects = 1; |
| RuslanUrya | 0:dfe6edabb8ec | 40 | constexpr unsigned NumberOfRxMessageObjects = NumberOfMessageObjects - NumberOfTxMessageObjects; |
| RuslanUrya | 0:dfe6edabb8ec | 41 | constexpr unsigned TxMessageObjectNumber = 1; |
| RuslanUrya | 0:dfe6edabb8ec | 42 | |
| RuslanUrya | 0:dfe6edabb8ec | 43 | /** |
| RuslanUrya | 0:dfe6edabb8ec | 44 | * Total number of CAN errors. |
| RuslanUrya | 0:dfe6edabb8ec | 45 | * Does not overflow. |
| RuslanUrya | 0:dfe6edabb8ec | 46 | */ |
| RuslanUrya | 0:dfe6edabb8ec | 47 | volatile std::uint32_t error_cnt = 0; |
| RuslanUrya | 0:dfe6edabb8ec | 48 | |
| RuslanUrya | 0:dfe6edabb8ec | 49 | /** |
| RuslanUrya | 0:dfe6edabb8ec | 50 | * False if there's no pending TX frame, i.e. write is possible. |
| RuslanUrya | 0:dfe6edabb8ec | 51 | */ |
| RuslanUrya | 0:dfe6edabb8ec | 52 | volatile bool tx_pending = false; |
| RuslanUrya | 0:dfe6edabb8ec | 53 | |
| RuslanUrya | 0:dfe6edabb8ec | 54 | /** |
| RuslanUrya | 0:dfe6edabb8ec | 55 | * Currently pending frame must be aborted on first error. |
| RuslanUrya | 0:dfe6edabb8ec | 56 | */ |
| RuslanUrya | 0:dfe6edabb8ec | 57 | volatile bool tx_abort_on_error = false; |
| RuslanUrya | 0:dfe6edabb8ec | 58 | |
| RuslanUrya | 0:dfe6edabb8ec | 59 | /** |
| RuslanUrya | 0:dfe6edabb8ec | 60 | * Gets updated every time the CAN IRQ handler is being called. |
| RuslanUrya | 0:dfe6edabb8ec | 61 | */ |
| RuslanUrya | 0:dfe6edabb8ec | 62 | volatile std::uint64_t last_irq_utc_timestamp = 0; |
| RuslanUrya | 0:dfe6edabb8ec | 63 | |
| RuslanUrya | 0:dfe6edabb8ec | 64 | /** |
| RuslanUrya | 0:dfe6edabb8ec | 65 | * Set by the driver on every successful TX or RX; reset by the application. |
| RuslanUrya | 0:dfe6edabb8ec | 66 | */ |
| RuslanUrya | 0:dfe6edabb8ec | 67 | volatile bool had_activity = false; |
| RuslanUrya | 0:dfe6edabb8ec | 68 | |
| RuslanUrya | 0:dfe6edabb8ec | 69 | /** |
| RuslanUrya | 0:dfe6edabb8ec | 70 | * After a received message gets extracted from C_CAN, it will be stored in the RX queue until libuavcan |
| RuslanUrya | 0:dfe6edabb8ec | 71 | * reads it via select()/receive() calls. |
| RuslanUrya | 0:dfe6edabb8ec | 72 | */ |
| RuslanUrya | 0:dfe6edabb8ec | 73 | class RxQueue |
| RuslanUrya | 0:dfe6edabb8ec | 74 | { |
| RuslanUrya | 0:dfe6edabb8ec | 75 | struct Item |
| RuslanUrya | 0:dfe6edabb8ec | 76 | { |
| RuslanUrya | 0:dfe6edabb8ec | 77 | std::uint64_t utc_usec = 0; |
| RuslanUrya | 0:dfe6edabb8ec | 78 | uavcan::CanFrame frame; |
| RuslanUrya | 0:dfe6edabb8ec | 79 | }; |
| RuslanUrya | 0:dfe6edabb8ec | 80 | |
| RuslanUrya | 0:dfe6edabb8ec | 81 | Item buf_[UAVCAN_LPC11C24_RX_QUEUE_LEN]; |
| RuslanUrya | 0:dfe6edabb8ec | 82 | std::uint32_t overflow_cnt_ = 0; |
| RuslanUrya | 0:dfe6edabb8ec | 83 | std::uint8_t in_ = 0; |
| RuslanUrya | 0:dfe6edabb8ec | 84 | std::uint8_t out_ = 0; |
| RuslanUrya | 0:dfe6edabb8ec | 85 | std::uint8_t len_ = 0; |
| RuslanUrya | 0:dfe6edabb8ec | 86 | |
| RuslanUrya | 0:dfe6edabb8ec | 87 | public: |
| RuslanUrya | 0:dfe6edabb8ec | 88 | void push(const uavcan::CanFrame& frame, const volatile std::uint64_t& utc_usec) |
| RuslanUrya | 0:dfe6edabb8ec | 89 | { |
| RuslanUrya | 0:dfe6edabb8ec | 90 | buf_[in_].frame = frame; |
| RuslanUrya | 0:dfe6edabb8ec | 91 | buf_[in_].utc_usec = utc_usec; |
| RuslanUrya | 0:dfe6edabb8ec | 92 | in_++; |
| RuslanUrya | 0:dfe6edabb8ec | 93 | if (in_ >= UAVCAN_LPC11C24_RX_QUEUE_LEN) |
| RuslanUrya | 0:dfe6edabb8ec | 94 | { |
| RuslanUrya | 0:dfe6edabb8ec | 95 | in_ = 0; |
| RuslanUrya | 0:dfe6edabb8ec | 96 | } |
| RuslanUrya | 0:dfe6edabb8ec | 97 | len_++; |
| RuslanUrya | 0:dfe6edabb8ec | 98 | if (len_ > UAVCAN_LPC11C24_RX_QUEUE_LEN) |
| RuslanUrya | 0:dfe6edabb8ec | 99 | { |
| RuslanUrya | 0:dfe6edabb8ec | 100 | len_ = UAVCAN_LPC11C24_RX_QUEUE_LEN; |
| RuslanUrya | 0:dfe6edabb8ec | 101 | if (overflow_cnt_ < 0xFFFFFFFF) |
| RuslanUrya | 0:dfe6edabb8ec | 102 | { |
| RuslanUrya | 0:dfe6edabb8ec | 103 | overflow_cnt_++; |
| RuslanUrya | 0:dfe6edabb8ec | 104 | } |
| RuslanUrya | 0:dfe6edabb8ec | 105 | out_++; |
| RuslanUrya | 0:dfe6edabb8ec | 106 | if (out_ >= UAVCAN_LPC11C24_RX_QUEUE_LEN) |
| RuslanUrya | 0:dfe6edabb8ec | 107 | { |
| RuslanUrya | 0:dfe6edabb8ec | 108 | out_ = 0; |
| RuslanUrya | 0:dfe6edabb8ec | 109 | } |
| RuslanUrya | 0:dfe6edabb8ec | 110 | } |
| RuslanUrya | 0:dfe6edabb8ec | 111 | } |
| RuslanUrya | 0:dfe6edabb8ec | 112 | |
| RuslanUrya | 0:dfe6edabb8ec | 113 | void pop(uavcan::CanFrame& out_frame, std::uint64_t& out_utc_usec) |
| RuslanUrya | 0:dfe6edabb8ec | 114 | { |
| RuslanUrya | 0:dfe6edabb8ec | 115 | if (len_ > 0) |
| RuslanUrya | 0:dfe6edabb8ec | 116 | { |
| RuslanUrya | 0:dfe6edabb8ec | 117 | out_frame = buf_[out_].frame; |
| RuslanUrya | 0:dfe6edabb8ec | 118 | out_utc_usec = buf_[out_].utc_usec; |
| RuslanUrya | 0:dfe6edabb8ec | 119 | out_++; |
| RuslanUrya | 0:dfe6edabb8ec | 120 | if (out_ >= UAVCAN_LPC11C24_RX_QUEUE_LEN) |
| RuslanUrya | 0:dfe6edabb8ec | 121 | { |
| RuslanUrya | 0:dfe6edabb8ec | 122 | out_ = 0; |
| RuslanUrya | 0:dfe6edabb8ec | 123 | } |
| RuslanUrya | 0:dfe6edabb8ec | 124 | len_--; |
| RuslanUrya | 0:dfe6edabb8ec | 125 | } |
| RuslanUrya | 0:dfe6edabb8ec | 126 | } |
| RuslanUrya | 0:dfe6edabb8ec | 127 | |
| RuslanUrya | 0:dfe6edabb8ec | 128 | unsigned getLength() const { return len_; } |
| RuslanUrya | 0:dfe6edabb8ec | 129 | |
| RuslanUrya | 0:dfe6edabb8ec | 130 | std::uint32_t getOverflowCount() const { return overflow_cnt_; } |
| RuslanUrya | 0:dfe6edabb8ec | 131 | }; |
| RuslanUrya | 0:dfe6edabb8ec | 132 | |
| RuslanUrya | 0:dfe6edabb8ec | 133 | RxQueue rx_queue; |
| RuslanUrya | 0:dfe6edabb8ec | 134 | |
| RuslanUrya | 0:dfe6edabb8ec | 135 | |
| RuslanUrya | 0:dfe6edabb8ec | 136 | struct BitTimingSettings |
| RuslanUrya | 0:dfe6edabb8ec | 137 | { |
| RuslanUrya | 0:dfe6edabb8ec | 138 | std::uint32_t canclkdiv; |
| RuslanUrya | 0:dfe6edabb8ec | 139 | std::uint32_t canbtr; |
| RuslanUrya | 0:dfe6edabb8ec | 140 | |
| RuslanUrya | 0:dfe6edabb8ec | 141 | bool isValid() const { return canbtr != 0; } |
| RuslanUrya | 0:dfe6edabb8ec | 142 | }; |
| RuslanUrya | 0:dfe6edabb8ec | 143 | |
| RuslanUrya | 0:dfe6edabb8ec | 144 | /** |
| RuslanUrya | 0:dfe6edabb8ec | 145 | * http://www.bittiming.can-wiki.info |
| RuslanUrya | 0:dfe6edabb8ec | 146 | */ |
| RuslanUrya | 0:dfe6edabb8ec | 147 | BitTimingSettings computeBitTimings(std::uint32_t bitrate) |
| RuslanUrya | 0:dfe6edabb8ec | 148 | { |
| RuslanUrya | 0:dfe6edabb8ec | 149 | if (Chip_Clock_GetSystemClockRate() == 48000000) // 48 MHz is optimal for CAN timings |
| RuslanUrya | 0:dfe6edabb8ec | 150 | { |
| RuslanUrya | 0:dfe6edabb8ec | 151 | switch (bitrate) |
| RuslanUrya | 0:dfe6edabb8ec | 152 | { |
| RuslanUrya | 0:dfe6edabb8ec | 153 | case 1000000: return BitTimingSettings{ 0, 0x0505 }; // 8 quanta, 87.5% |
| RuslanUrya | 0:dfe6edabb8ec | 154 | case 500000: return BitTimingSettings{ 0, 0x1c05 }; // 16 quanta, 87.5% |
| RuslanUrya | 0:dfe6edabb8ec | 155 | case 250000: return BitTimingSettings{ 0, 0x1c0b }; // 16 quanta, 87.5% |
| RuslanUrya | 0:dfe6edabb8ec | 156 | case 125000: return BitTimingSettings{ 0, 0x1c17 }; // 16 quanta, 87.5% |
| RuslanUrya | 0:dfe6edabb8ec | 157 | case 100000: return BitTimingSettings{ 0, 0x1c1d }; // 16 quanta, 87.5% |
| RuslanUrya | 0:dfe6edabb8ec | 158 | default: return BitTimingSettings{ 0, 0 }; |
| RuslanUrya | 0:dfe6edabb8ec | 159 | } |
| RuslanUrya | 0:dfe6edabb8ec | 160 | } |
| RuslanUrya | 0:dfe6edabb8ec | 161 | else |
| RuslanUrya | 0:dfe6edabb8ec | 162 | { |
| RuslanUrya | 0:dfe6edabb8ec | 163 | return BitTimingSettings{ 0, 0 }; |
| RuslanUrya | 0:dfe6edabb8ec | 164 | } |
| RuslanUrya | 0:dfe6edabb8ec | 165 | } |
| RuslanUrya | 0:dfe6edabb8ec | 166 | |
| RuslanUrya | 0:dfe6edabb8ec | 167 | } // namespace |
| RuslanUrya | 0:dfe6edabb8ec | 168 | |
| RuslanUrya | 0:dfe6edabb8ec | 169 | CanDriver CanDriver::self; |
| RuslanUrya | 0:dfe6edabb8ec | 170 | |
| RuslanUrya | 0:dfe6edabb8ec | 171 | uavcan::uint32_t CanDriver::detectBitRate(void (*idle_callback)()) |
| RuslanUrya | 0:dfe6edabb8ec | 172 | { |
| RuslanUrya | 0:dfe6edabb8ec | 173 | static constexpr uavcan::uint32_t BitRatesToTry[] = |
| RuslanUrya | 0:dfe6edabb8ec | 174 | { |
| RuslanUrya | 0:dfe6edabb8ec | 175 | 1000000, |
| RuslanUrya | 0:dfe6edabb8ec | 176 | 500000, |
| RuslanUrya | 0:dfe6edabb8ec | 177 | 250000, |
| RuslanUrya | 0:dfe6edabb8ec | 178 | 125000, |
| RuslanUrya | 0:dfe6edabb8ec | 179 | 100000 |
| RuslanUrya | 0:dfe6edabb8ec | 180 | }; |
| RuslanUrya | 0:dfe6edabb8ec | 181 | |
| RuslanUrya | 0:dfe6edabb8ec | 182 | const auto ListeningDuration = uavcan::MonotonicDuration::fromMSec(1050); |
| RuslanUrya | 0:dfe6edabb8ec | 183 | |
| RuslanUrya | 0:dfe6edabb8ec | 184 | NVIC_DisableIRQ(CAN_IRQn); |
| RuslanUrya | 0:dfe6edabb8ec | 185 | Chip_Clock_EnablePeriphClock(SYSCTL_CLOCK_CAN); |
| RuslanUrya | 0:dfe6edabb8ec | 186 | |
| RuslanUrya | 0:dfe6edabb8ec | 187 | for (auto bitrate : BitRatesToTry) |
| RuslanUrya | 0:dfe6edabb8ec | 188 | { |
| RuslanUrya | 0:dfe6edabb8ec | 189 | // Computing bit timings |
| RuslanUrya | 0:dfe6edabb8ec | 190 | const auto bit_timings = computeBitTimings(bitrate); |
| RuslanUrya | 0:dfe6edabb8ec | 191 | if (!bit_timings.isValid()) |
| RuslanUrya | 0:dfe6edabb8ec | 192 | { |
| RuslanUrya | 0:dfe6edabb8ec | 193 | return 0; |
| RuslanUrya | 0:dfe6edabb8ec | 194 | } |
| RuslanUrya | 0:dfe6edabb8ec | 195 | |
| RuslanUrya | 0:dfe6edabb8ec | 196 | // Configuring the CAN controller |
| RuslanUrya | 0:dfe6edabb8ec | 197 | { |
| RuslanUrya | 0:dfe6edabb8ec | 198 | CriticalSectionLocker locker; |
| RuslanUrya | 0:dfe6edabb8ec | 199 | |
| RuslanUrya | 0:dfe6edabb8ec | 200 | LPC_SYSCTL->PRESETCTRL |= (1U << RESET_CAN0); |
| RuslanUrya | 0:dfe6edabb8ec | 201 | |
| RuslanUrya | 0:dfe6edabb8ec | 202 | // Entering initialization mode |
| RuslanUrya | 0:dfe6edabb8ec | 203 | c_can::CAN.CNTL = c_can::CNTL_INIT | c_can::CNTL_CCE; |
| RuslanUrya | 0:dfe6edabb8ec | 204 | |
| RuslanUrya | 0:dfe6edabb8ec | 205 | while ((c_can::CAN.CNTL & c_can::CNTL_INIT) == 0) |
| RuslanUrya | 0:dfe6edabb8ec | 206 | { |
| RuslanUrya | 0:dfe6edabb8ec | 207 | ; // Nothing to do |
| RuslanUrya | 0:dfe6edabb8ec | 208 | } |
| RuslanUrya | 0:dfe6edabb8ec | 209 | |
| RuslanUrya | 0:dfe6edabb8ec | 210 | // Configuring bit rate |
| RuslanUrya | 0:dfe6edabb8ec | 211 | c_can::CAN.CLKDIV = bit_timings.canclkdiv; |
| RuslanUrya | 0:dfe6edabb8ec | 212 | c_can::CAN.BT = bit_timings.canbtr; |
| RuslanUrya | 0:dfe6edabb8ec | 213 | c_can::CAN.BRPE = 0; |
| RuslanUrya | 0:dfe6edabb8ec | 214 | |
| RuslanUrya | 0:dfe6edabb8ec | 215 | // Configuring silent mode |
| RuslanUrya | 0:dfe6edabb8ec | 216 | c_can::CAN.CNTL |= c_can::CNTL_TEST; |
| RuslanUrya | 0:dfe6edabb8ec | 217 | c_can::CAN.TEST = c_can::TEST_SILENT; |
| RuslanUrya | 0:dfe6edabb8ec | 218 | |
| RuslanUrya | 0:dfe6edabb8ec | 219 | // Configuring status monitor |
| RuslanUrya | 0:dfe6edabb8ec | 220 | c_can::CAN.STAT = (unsigned(c_can::StatLec::Unused) << c_can::STAT_LEC_SHIFT); |
| RuslanUrya | 0:dfe6edabb8ec | 221 | |
| RuslanUrya | 0:dfe6edabb8ec | 222 | // Leaving initialization mode |
| RuslanUrya | 0:dfe6edabb8ec | 223 | c_can::CAN.CNTL &= ~(c_can::CNTL_INIT | c_can::CNTL_CCE); |
| RuslanUrya | 0:dfe6edabb8ec | 224 | |
| RuslanUrya | 0:dfe6edabb8ec | 225 | while ((c_can::CAN.CNTL & c_can::CNTL_INIT) != 0) |
| RuslanUrya | 0:dfe6edabb8ec | 226 | { |
| RuslanUrya | 0:dfe6edabb8ec | 227 | ; // Nothing to do |
| RuslanUrya | 0:dfe6edabb8ec | 228 | } |
| RuslanUrya | 0:dfe6edabb8ec | 229 | } |
| RuslanUrya | 0:dfe6edabb8ec | 230 | |
| RuslanUrya | 0:dfe6edabb8ec | 231 | // Listening |
| RuslanUrya | 0:dfe6edabb8ec | 232 | const auto deadline = clock::getMonotonic() + ListeningDuration; |
| RuslanUrya | 0:dfe6edabb8ec | 233 | bool match_detected = false; |
| RuslanUrya | 0:dfe6edabb8ec | 234 | while (clock::getMonotonic() < deadline) |
| RuslanUrya | 0:dfe6edabb8ec | 235 | { |
| RuslanUrya | 0:dfe6edabb8ec | 236 | if (idle_callback != nullptr) |
| RuslanUrya | 0:dfe6edabb8ec | 237 | { |
| RuslanUrya | 0:dfe6edabb8ec | 238 | idle_callback(); |
| RuslanUrya | 0:dfe6edabb8ec | 239 | } |
| RuslanUrya | 0:dfe6edabb8ec | 240 | |
| RuslanUrya | 0:dfe6edabb8ec | 241 | const auto LastErrorCode = (c_can::CAN.STAT >> c_can::STAT_LEC_SHIFT) & c_can::STAT_LEC_MASK; |
| RuslanUrya | 0:dfe6edabb8ec | 242 | |
| RuslanUrya | 0:dfe6edabb8ec | 243 | if (LastErrorCode == unsigned(c_can::StatLec::NoError)) |
| RuslanUrya | 0:dfe6edabb8ec | 244 | { |
| RuslanUrya | 0:dfe6edabb8ec | 245 | match_detected = true; |
| RuslanUrya | 0:dfe6edabb8ec | 246 | break; |
| RuslanUrya | 0:dfe6edabb8ec | 247 | } |
| RuslanUrya | 0:dfe6edabb8ec | 248 | } |
| RuslanUrya | 0:dfe6edabb8ec | 249 | |
| RuslanUrya | 0:dfe6edabb8ec | 250 | // De-configuring the CAN controller back to reset state |
| RuslanUrya | 0:dfe6edabb8ec | 251 | { |
| RuslanUrya | 0:dfe6edabb8ec | 252 | CriticalSectionLocker locker; |
| RuslanUrya | 0:dfe6edabb8ec | 253 | |
| RuslanUrya | 0:dfe6edabb8ec | 254 | c_can::CAN.CNTL = c_can::CNTL_INIT; |
| RuslanUrya | 0:dfe6edabb8ec | 255 | |
| RuslanUrya | 0:dfe6edabb8ec | 256 | while ((c_can::CAN.CNTL & c_can::CNTL_INIT) == 0) |
| RuslanUrya | 0:dfe6edabb8ec | 257 | { |
| RuslanUrya | 0:dfe6edabb8ec | 258 | ; // Nothing to do |
| RuslanUrya | 0:dfe6edabb8ec | 259 | } |
| RuslanUrya | 0:dfe6edabb8ec | 260 | |
| RuslanUrya | 0:dfe6edabb8ec | 261 | LPC_SYSCTL->PRESETCTRL &= ~(1U << RESET_CAN0); |
| RuslanUrya | 0:dfe6edabb8ec | 262 | } |
| RuslanUrya | 0:dfe6edabb8ec | 263 | |
| RuslanUrya | 0:dfe6edabb8ec | 264 | // Termination condition |
| RuslanUrya | 0:dfe6edabb8ec | 265 | if (match_detected) |
| RuslanUrya | 0:dfe6edabb8ec | 266 | { |
| RuslanUrya | 0:dfe6edabb8ec | 267 | return bitrate; |
| RuslanUrya | 0:dfe6edabb8ec | 268 | } |
| RuslanUrya | 0:dfe6edabb8ec | 269 | } |
| RuslanUrya | 0:dfe6edabb8ec | 270 | |
| RuslanUrya | 0:dfe6edabb8ec | 271 | return 0; // No match |
| RuslanUrya | 0:dfe6edabb8ec | 272 | } |
| RuslanUrya | 0:dfe6edabb8ec | 273 | |
| RuslanUrya | 0:dfe6edabb8ec | 274 | int CanDriver::init(uavcan::uint32_t bitrate) |
| RuslanUrya | 0:dfe6edabb8ec | 275 | { |
| RuslanUrya | 0:dfe6edabb8ec | 276 | { |
| RuslanUrya | 0:dfe6edabb8ec | 277 | auto bit_timings = computeBitTimings(bitrate); |
| RuslanUrya | 0:dfe6edabb8ec | 278 | if (!bit_timings.isValid()) |
| RuslanUrya | 0:dfe6edabb8ec | 279 | { |
| RuslanUrya | 0:dfe6edabb8ec | 280 | return -1; |
| RuslanUrya | 0:dfe6edabb8ec | 281 | } |
| RuslanUrya | 0:dfe6edabb8ec | 282 | |
| RuslanUrya | 0:dfe6edabb8ec | 283 | CriticalSectionLocker locker; |
| RuslanUrya | 0:dfe6edabb8ec | 284 | |
| RuslanUrya | 0:dfe6edabb8ec | 285 | error_cnt = 0; |
| RuslanUrya | 0:dfe6edabb8ec | 286 | tx_abort_on_error = false; |
| RuslanUrya | 0:dfe6edabb8ec | 287 | tx_pending = false; |
| RuslanUrya | 0:dfe6edabb8ec | 288 | last_irq_utc_timestamp = 0; |
| RuslanUrya | 0:dfe6edabb8ec | 289 | had_activity = false; |
| RuslanUrya | 0:dfe6edabb8ec | 290 | |
| RuslanUrya | 0:dfe6edabb8ec | 291 | /* |
| RuslanUrya | 0:dfe6edabb8ec | 292 | * C_CAN init |
| RuslanUrya | 0:dfe6edabb8ec | 293 | */ |
| RuslanUrya | 0:dfe6edabb8ec | 294 | Chip_Clock_EnablePeriphClock(SYSCTL_CLOCK_CAN); |
| RuslanUrya | 0:dfe6edabb8ec | 295 | |
| RuslanUrya | 0:dfe6edabb8ec | 296 | LPC_CCAN_API->init_can(reinterpret_cast<std::uint32_t*>(&bit_timings), true); |
| RuslanUrya | 0:dfe6edabb8ec | 297 | |
| RuslanUrya | 0:dfe6edabb8ec | 298 | static CCAN_CALLBACKS_T ccan_callbacks = |
| RuslanUrya | 0:dfe6edabb8ec | 299 | { |
| RuslanUrya | 0:dfe6edabb8ec | 300 | canRxCallback, |
| RuslanUrya | 0:dfe6edabb8ec | 301 | canTxCallback, |
| RuslanUrya | 0:dfe6edabb8ec | 302 | canErrorCallback, |
| RuslanUrya | 0:dfe6edabb8ec | 303 | nullptr, |
| RuslanUrya | 0:dfe6edabb8ec | 304 | nullptr, |
| RuslanUrya | 0:dfe6edabb8ec | 305 | nullptr, |
| RuslanUrya | 0:dfe6edabb8ec | 306 | nullptr, |
| RuslanUrya | 0:dfe6edabb8ec | 307 | nullptr |
| RuslanUrya | 0:dfe6edabb8ec | 308 | }; |
| RuslanUrya | 0:dfe6edabb8ec | 309 | LPC_CCAN_API->config_calb(&ccan_callbacks); |
| RuslanUrya | 0:dfe6edabb8ec | 310 | |
| RuslanUrya | 0:dfe6edabb8ec | 311 | /* |
| RuslanUrya | 0:dfe6edabb8ec | 312 | * Interrupts |
| RuslanUrya | 0:dfe6edabb8ec | 313 | */ |
| RuslanUrya | 0:dfe6edabb8ec | 314 | c_can::CAN.CNTL |= c_can::CNTL_SIE; // This is necessary for transmission aborts on error |
| RuslanUrya | 0:dfe6edabb8ec | 315 | |
| RuslanUrya | 0:dfe6edabb8ec | 316 | NVIC_EnableIRQ(CAN_IRQn); |
| RuslanUrya | 0:dfe6edabb8ec | 317 | } |
| RuslanUrya | 0:dfe6edabb8ec | 318 | |
| RuslanUrya | 0:dfe6edabb8ec | 319 | /* |
| RuslanUrya | 0:dfe6edabb8ec | 320 | * Applying default filter configuration (accept all) |
| RuslanUrya | 0:dfe6edabb8ec | 321 | */ |
| RuslanUrya | 0:dfe6edabb8ec | 322 | if (configureFilters(nullptr, 0) < 0) |
| RuslanUrya | 0:dfe6edabb8ec | 323 | { |
| RuslanUrya | 0:dfe6edabb8ec | 324 | return -1; |
| RuslanUrya | 0:dfe6edabb8ec | 325 | } |
| RuslanUrya | 0:dfe6edabb8ec | 326 | |
| RuslanUrya | 0:dfe6edabb8ec | 327 | return 0; |
| RuslanUrya | 0:dfe6edabb8ec | 328 | } |
| RuslanUrya | 0:dfe6edabb8ec | 329 | |
| RuslanUrya | 0:dfe6edabb8ec | 330 | bool CanDriver::hasReadyRx() const |
| RuslanUrya | 0:dfe6edabb8ec | 331 | { |
| RuslanUrya | 0:dfe6edabb8ec | 332 | CriticalSectionLocker locker; |
| RuslanUrya | 0:dfe6edabb8ec | 333 | return rx_queue.getLength() > 0; |
| RuslanUrya | 0:dfe6edabb8ec | 334 | } |
| RuslanUrya | 0:dfe6edabb8ec | 335 | |
| RuslanUrya | 0:dfe6edabb8ec | 336 | bool CanDriver::hasEmptyTx() const |
| RuslanUrya | 0:dfe6edabb8ec | 337 | { |
| RuslanUrya | 0:dfe6edabb8ec | 338 | CriticalSectionLocker locker; |
| RuslanUrya | 0:dfe6edabb8ec | 339 | return !tx_pending; |
| RuslanUrya | 0:dfe6edabb8ec | 340 | } |
| RuslanUrya | 0:dfe6edabb8ec | 341 | |
| RuslanUrya | 0:dfe6edabb8ec | 342 | bool CanDriver::hadActivity() |
| RuslanUrya | 0:dfe6edabb8ec | 343 | { |
| RuslanUrya | 0:dfe6edabb8ec | 344 | CriticalSectionLocker locker; |
| RuslanUrya | 0:dfe6edabb8ec | 345 | const bool ret = had_activity; |
| RuslanUrya | 0:dfe6edabb8ec | 346 | had_activity = false; |
| RuslanUrya | 0:dfe6edabb8ec | 347 | return ret; |
| RuslanUrya | 0:dfe6edabb8ec | 348 | } |
| RuslanUrya | 0:dfe6edabb8ec | 349 | |
| RuslanUrya | 0:dfe6edabb8ec | 350 | uavcan::uint32_t CanDriver::getRxQueueOverflowCount() const |
| RuslanUrya | 0:dfe6edabb8ec | 351 | { |
| RuslanUrya | 0:dfe6edabb8ec | 352 | CriticalSectionLocker locker; |
| RuslanUrya | 0:dfe6edabb8ec | 353 | return rx_queue.getOverflowCount(); |
| RuslanUrya | 0:dfe6edabb8ec | 354 | } |
| RuslanUrya | 0:dfe6edabb8ec | 355 | |
| RuslanUrya | 0:dfe6edabb8ec | 356 | bool CanDriver::isInBusOffState() const |
| RuslanUrya | 0:dfe6edabb8ec | 357 | { |
| RuslanUrya | 0:dfe6edabb8ec | 358 | return (c_can::CAN.STAT & c_can::STAT_BOFF) != 0; |
| RuslanUrya | 0:dfe6edabb8ec | 359 | } |
| RuslanUrya | 0:dfe6edabb8ec | 360 | |
| RuslanUrya | 0:dfe6edabb8ec | 361 | uavcan::int16_t CanDriver::send(const uavcan::CanFrame& frame, uavcan::MonotonicTime tx_deadline, |
| RuslanUrya | 0:dfe6edabb8ec | 362 | uavcan::CanIOFlags flags) |
| RuslanUrya | 0:dfe6edabb8ec | 363 | { |
| RuslanUrya | 0:dfe6edabb8ec | 364 | if (frame.isErrorFrame() || |
| RuslanUrya | 0:dfe6edabb8ec | 365 | frame.dlc > 8 || |
| RuslanUrya | 0:dfe6edabb8ec | 366 | (flags & uavcan::CanIOFlagLoopback) != 0) // TX timestamping is not supported by this driver. |
| RuslanUrya | 0:dfe6edabb8ec | 367 | { |
| RuslanUrya | 0:dfe6edabb8ec | 368 | return -1; |
| RuslanUrya | 0:dfe6edabb8ec | 369 | } |
| RuslanUrya | 0:dfe6edabb8ec | 370 | |
| RuslanUrya | 0:dfe6edabb8ec | 371 | /* |
| RuslanUrya | 0:dfe6edabb8ec | 372 | * Frame conversion |
| RuslanUrya | 0:dfe6edabb8ec | 373 | */ |
| RuslanUrya | 0:dfe6edabb8ec | 374 | CCAN_MSG_OBJ_T msgobj = CCAN_MSG_OBJ_T(); |
| RuslanUrya | 0:dfe6edabb8ec | 375 | msgobj.mode_id = frame.id & uavcan::CanFrame::MaskExtID; |
| RuslanUrya | 0:dfe6edabb8ec | 376 | if (frame.isExtended()) |
| RuslanUrya | 0:dfe6edabb8ec | 377 | { |
| RuslanUrya | 0:dfe6edabb8ec | 378 | msgobj.mode_id |= CAN_MSGOBJ_EXT; |
| RuslanUrya | 0:dfe6edabb8ec | 379 | } |
| RuslanUrya | 0:dfe6edabb8ec | 380 | if (frame.isRemoteTransmissionRequest()) |
| RuslanUrya | 0:dfe6edabb8ec | 381 | { |
| RuslanUrya | 0:dfe6edabb8ec | 382 | msgobj.mode_id |= CAN_MSGOBJ_RTR; |
| RuslanUrya | 0:dfe6edabb8ec | 383 | } |
| RuslanUrya | 0:dfe6edabb8ec | 384 | msgobj.dlc = frame.dlc; |
| RuslanUrya | 0:dfe6edabb8ec | 385 | uavcan::copy(frame.data, frame.data + frame.dlc, msgobj.data); |
| RuslanUrya | 0:dfe6edabb8ec | 386 | |
| RuslanUrya | 0:dfe6edabb8ec | 387 | /* |
| RuslanUrya | 0:dfe6edabb8ec | 388 | * Transmission |
| RuslanUrya | 0:dfe6edabb8ec | 389 | */ |
| RuslanUrya | 0:dfe6edabb8ec | 390 | (void)tx_deadline; // TX timeouts are not supported by this driver yet (and hardly going to be). |
| RuslanUrya | 0:dfe6edabb8ec | 391 | |
| RuslanUrya | 0:dfe6edabb8ec | 392 | CriticalSectionLocker locker; |
| RuslanUrya | 0:dfe6edabb8ec | 393 | |
| RuslanUrya | 0:dfe6edabb8ec | 394 | if (!tx_pending) |
| RuslanUrya | 0:dfe6edabb8ec | 395 | { |
| RuslanUrya | 0:dfe6edabb8ec | 396 | tx_pending = true; // Mark as pending - will be released in TX callback |
| RuslanUrya | 0:dfe6edabb8ec | 397 | tx_abort_on_error = (flags & uavcan::CanIOFlagAbortOnError) != 0; |
| RuslanUrya | 0:dfe6edabb8ec | 398 | msgobj.msgobj = TxMessageObjectNumber; |
| RuslanUrya | 0:dfe6edabb8ec | 399 | LPC_CCAN_API->can_transmit(&msgobj); |
| RuslanUrya | 0:dfe6edabb8ec | 400 | return 1; |
| RuslanUrya | 0:dfe6edabb8ec | 401 | } |
| RuslanUrya | 0:dfe6edabb8ec | 402 | return 0; |
| RuslanUrya | 0:dfe6edabb8ec | 403 | } |
| RuslanUrya | 0:dfe6edabb8ec | 404 | |
| RuslanUrya | 0:dfe6edabb8ec | 405 | uavcan::int16_t CanDriver::receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic, |
| RuslanUrya | 0:dfe6edabb8ec | 406 | uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags) |
| RuslanUrya | 0:dfe6edabb8ec | 407 | { |
| RuslanUrya | 0:dfe6edabb8ec | 408 | out_ts_monotonic = clock::getMonotonic(); |
| RuslanUrya | 0:dfe6edabb8ec | 409 | out_flags = 0; // We don't support any IO flags |
| RuslanUrya | 0:dfe6edabb8ec | 410 | |
| RuslanUrya | 0:dfe6edabb8ec | 411 | CriticalSectionLocker locker; |
| RuslanUrya | 0:dfe6edabb8ec | 412 | if (rx_queue.getLength() == 0) |
| RuslanUrya | 0:dfe6edabb8ec | 413 | { |
| RuslanUrya | 0:dfe6edabb8ec | 414 | return 0; |
| RuslanUrya | 0:dfe6edabb8ec | 415 | } |
| RuslanUrya | 0:dfe6edabb8ec | 416 | std::uint64_t ts_utc = 0; |
| RuslanUrya | 0:dfe6edabb8ec | 417 | rx_queue.pop(out_frame, ts_utc); |
| RuslanUrya | 0:dfe6edabb8ec | 418 | out_ts_utc = uavcan::UtcTime::fromUSec(ts_utc); |
| RuslanUrya | 0:dfe6edabb8ec | 419 | return 1; |
| RuslanUrya | 0:dfe6edabb8ec | 420 | } |
| RuslanUrya | 0:dfe6edabb8ec | 421 | |
| RuslanUrya | 0:dfe6edabb8ec | 422 | uavcan::int16_t CanDriver::select(uavcan::CanSelectMasks& inout_masks, |
| RuslanUrya | 0:dfe6edabb8ec | 423 | const uavcan::CanFrame* (&)[uavcan::MaxCanIfaces], |
| RuslanUrya | 0:dfe6edabb8ec | 424 | uavcan::MonotonicTime blocking_deadline) |
| RuslanUrya | 0:dfe6edabb8ec | 425 | { |
| RuslanUrya | 0:dfe6edabb8ec | 426 | const bool bus_off = isInBusOffState(); |
| RuslanUrya | 0:dfe6edabb8ec | 427 | if (bus_off) // Recover automatically on bus-off |
| RuslanUrya | 0:dfe6edabb8ec | 428 | { |
| RuslanUrya | 0:dfe6edabb8ec | 429 | CriticalSectionLocker locker; |
| RuslanUrya | 0:dfe6edabb8ec | 430 | if ((c_can::CAN.CNTL & c_can::CNTL_INIT) != 0) |
| RuslanUrya | 0:dfe6edabb8ec | 431 | { |
| RuslanUrya | 0:dfe6edabb8ec | 432 | c_can::CAN.CNTL &= ~c_can::CNTL_INIT; |
| RuslanUrya | 0:dfe6edabb8ec | 433 | } |
| RuslanUrya | 0:dfe6edabb8ec | 434 | } |
| RuslanUrya | 0:dfe6edabb8ec | 435 | |
| RuslanUrya | 0:dfe6edabb8ec | 436 | const bool noblock = ((inout_masks.read == 1) && hasReadyRx()) || |
| RuslanUrya | 0:dfe6edabb8ec | 437 | ((inout_masks.write == 1) && hasEmptyTx()); |
| RuslanUrya | 0:dfe6edabb8ec | 438 | |
| RuslanUrya | 0:dfe6edabb8ec | 439 | if (!noblock && (clock::getMonotonic() > blocking_deadline)) |
| RuslanUrya | 0:dfe6edabb8ec | 440 | { |
| RuslanUrya | 0:dfe6edabb8ec | 441 | #if defined(UAVCAN_LPC11C24_USE_WFE) && UAVCAN_LPC11C24_USE_WFE |
| RuslanUrya | 0:dfe6edabb8ec | 442 | /* |
| RuslanUrya | 0:dfe6edabb8ec | 443 | * It's not cool (literally) to burn cycles in a busyloop, and we have no OS to pass control to other |
| RuslanUrya | 0:dfe6edabb8ec | 444 | * tasks, thus solution is to halt the core until a hardware event occurs - e.g. clock timer overflow. |
| RuslanUrya | 0:dfe6edabb8ec | 445 | * Upon such event the select() call will return, even if no requested IO operations became available. |
| RuslanUrya | 0:dfe6edabb8ec | 446 | * It's OK to do that, libuavcan can handle such behavior. |
| RuslanUrya | 0:dfe6edabb8ec | 447 | * |
| RuslanUrya | 0:dfe6edabb8ec | 448 | * Note that it is not possible to precisely control the sleep duration with WFE, since we can't predict when |
| RuslanUrya | 0:dfe6edabb8ec | 449 | * the next hardware event occurs. Worst case conditions: |
| RuslanUrya | 0:dfe6edabb8ec | 450 | * - WFE gets executed right after the clock timer interrupt; |
| RuslanUrya | 0:dfe6edabb8ec | 451 | * - CAN bus is completely silent (no traffic); |
| RuslanUrya | 0:dfe6edabb8ec | 452 | * - User's application has no interrupts and generates no hardware events. |
| RuslanUrya | 0:dfe6edabb8ec | 453 | * In such scenario execution will stuck here for one period of the clock timer interrupt, even if |
| RuslanUrya | 0:dfe6edabb8ec | 454 | * blocking_deadline expires sooner. |
| RuslanUrya | 0:dfe6edabb8ec | 455 | * If the user's application requires higher timing precision, an extra dummy IRQ can be added just to |
| RuslanUrya | 0:dfe6edabb8ec | 456 | * break WFE every once in a while. |
| RuslanUrya | 0:dfe6edabb8ec | 457 | */ |
| RuslanUrya | 0:dfe6edabb8ec | 458 | __WFE(); |
| RuslanUrya | 0:dfe6edabb8ec | 459 | #endif |
| RuslanUrya | 0:dfe6edabb8ec | 460 | } |
| RuslanUrya | 0:dfe6edabb8ec | 461 | |
| RuslanUrya | 0:dfe6edabb8ec | 462 | inout_masks.read = hasReadyRx() ? 1 : 0; |
| RuslanUrya | 0:dfe6edabb8ec | 463 | |
| RuslanUrya | 0:dfe6edabb8ec | 464 | inout_masks.write = (hasEmptyTx() && !bus_off) ? 1 : 0; // Disable write while in bus-off |
| RuslanUrya | 0:dfe6edabb8ec | 465 | |
| RuslanUrya | 0:dfe6edabb8ec | 466 | return 0; // Return value doesn't matter as long as it is non-negative |
| RuslanUrya | 0:dfe6edabb8ec | 467 | } |
| RuslanUrya | 0:dfe6edabb8ec | 468 | |
| RuslanUrya | 0:dfe6edabb8ec | 469 | uavcan::int16_t CanDriver::configureFilters(const uavcan::CanFilterConfig* filter_configs, |
| RuslanUrya | 0:dfe6edabb8ec | 470 | uavcan::uint16_t num_configs) |
| RuslanUrya | 0:dfe6edabb8ec | 471 | { |
| RuslanUrya | 0:dfe6edabb8ec | 472 | CriticalSectionLocker locker; |
| RuslanUrya | 0:dfe6edabb8ec | 473 | |
| RuslanUrya | 0:dfe6edabb8ec | 474 | /* |
| RuslanUrya | 0:dfe6edabb8ec | 475 | * If C_CAN is active (INIT=0) and the CAN bus has intensive traffic, RX object configuration may fail. |
| RuslanUrya | 0:dfe6edabb8ec | 476 | * The solution is to disable the controller while configuration is in progress. |
| RuslanUrya | 0:dfe6edabb8ec | 477 | * The documentation, as always, doesn't bother to mention this detail. Shame on you, NXP. |
| RuslanUrya | 0:dfe6edabb8ec | 478 | */ |
| RuslanUrya | 0:dfe6edabb8ec | 479 | struct RAIIDisabler |
| RuslanUrya | 0:dfe6edabb8ec | 480 | { |
| RuslanUrya | 0:dfe6edabb8ec | 481 | RAIIDisabler() |
| RuslanUrya | 0:dfe6edabb8ec | 482 | { |
| RuslanUrya | 0:dfe6edabb8ec | 483 | c_can::CAN.CNTL |= c_can::CNTL_INIT; |
| RuslanUrya | 0:dfe6edabb8ec | 484 | } |
| RuslanUrya | 0:dfe6edabb8ec | 485 | ~RAIIDisabler() |
| RuslanUrya | 0:dfe6edabb8ec | 486 | { |
| RuslanUrya | 0:dfe6edabb8ec | 487 | c_can::CAN.CNTL &= ~c_can::CNTL_INIT; |
| RuslanUrya | 0:dfe6edabb8ec | 488 | } |
| RuslanUrya | 0:dfe6edabb8ec | 489 | } can_disabler; // Must be instantiated AFTER the critical section locker |
| RuslanUrya | 0:dfe6edabb8ec | 490 | |
| RuslanUrya | 0:dfe6edabb8ec | 491 | if (num_configs == 0) |
| RuslanUrya | 0:dfe6edabb8ec | 492 | { |
| RuslanUrya | 0:dfe6edabb8ec | 493 | auto msg_obj = CCAN_MSG_OBJ_T(); |
| RuslanUrya | 0:dfe6edabb8ec | 494 | msg_obj.msgobj = NumberOfTxMessageObjects + 1; |
| RuslanUrya | 0:dfe6edabb8ec | 495 | LPC_CCAN_API->config_rxmsgobj(&msg_obj); // all STD frames |
| RuslanUrya | 0:dfe6edabb8ec | 496 | |
| RuslanUrya | 0:dfe6edabb8ec | 497 | msg_obj.mode_id = CAN_MSGOBJ_EXT; |
| RuslanUrya | 0:dfe6edabb8ec | 498 | msg_obj.msgobj = NumberOfTxMessageObjects + 2; |
| RuslanUrya | 0:dfe6edabb8ec | 499 | LPC_CCAN_API->config_rxmsgobj(&msg_obj); // all EXT frames |
| RuslanUrya | 0:dfe6edabb8ec | 500 | } |
| RuslanUrya | 0:dfe6edabb8ec | 501 | else if (num_configs <= NumberOfRxMessageObjects) |
| RuslanUrya | 0:dfe6edabb8ec | 502 | { |
| RuslanUrya | 0:dfe6edabb8ec | 503 | // Making sure the configs use only EXT frames; otherwise we can't accept them |
| RuslanUrya | 0:dfe6edabb8ec | 504 | for (unsigned i = 0; i < num_configs; i++) |
| RuslanUrya | 0:dfe6edabb8ec | 505 | { |
| RuslanUrya | 0:dfe6edabb8ec | 506 | auto& f = filter_configs[i]; |
| RuslanUrya | 0:dfe6edabb8ec | 507 | if ((f.id & f.mask & uavcan::CanFrame::FlagEFF) == 0) |
| RuslanUrya | 0:dfe6edabb8ec | 508 | { |
| RuslanUrya | 0:dfe6edabb8ec | 509 | return -1; |
| RuslanUrya | 0:dfe6edabb8ec | 510 | } |
| RuslanUrya | 0:dfe6edabb8ec | 511 | } |
| RuslanUrya | 0:dfe6edabb8ec | 512 | |
| RuslanUrya | 0:dfe6edabb8ec | 513 | // Installing the configuration |
| RuslanUrya | 0:dfe6edabb8ec | 514 | for (unsigned i = 0; i < NumberOfRxMessageObjects; i++) |
| RuslanUrya | 0:dfe6edabb8ec | 515 | { |
| RuslanUrya | 0:dfe6edabb8ec | 516 | auto msg_obj = CCAN_MSG_OBJ_T(); |
| RuslanUrya | 0:dfe6edabb8ec | 517 | msg_obj.msgobj = std::uint8_t(i + 1U + NumberOfTxMessageObjects); // Message objects are numbered from 1 |
| RuslanUrya | 0:dfe6edabb8ec | 518 | |
| RuslanUrya | 0:dfe6edabb8ec | 519 | if (i < num_configs) |
| RuslanUrya | 0:dfe6edabb8ec | 520 | { |
| RuslanUrya | 0:dfe6edabb8ec | 521 | msg_obj.mode_id = (filter_configs[i].id & uavcan::CanFrame::MaskExtID) | CAN_MSGOBJ_EXT; // Only EXT |
| RuslanUrya | 0:dfe6edabb8ec | 522 | msg_obj.mask = filter_configs[i].mask & uavcan::CanFrame::MaskExtID; |
| RuslanUrya | 0:dfe6edabb8ec | 523 | } |
| RuslanUrya | 0:dfe6edabb8ec | 524 | else |
| RuslanUrya | 0:dfe6edabb8ec | 525 | { |
| RuslanUrya | 0:dfe6edabb8ec | 526 | msg_obj.mode_id = CAN_MSGOBJ_RTR; // Using this configuration to disable the object |
| RuslanUrya | 0:dfe6edabb8ec | 527 | msg_obj.mask = uavcan::CanFrame::MaskStdID; |
| RuslanUrya | 0:dfe6edabb8ec | 528 | } |
| RuslanUrya | 0:dfe6edabb8ec | 529 | |
| RuslanUrya | 0:dfe6edabb8ec | 530 | LPC_CCAN_API->config_rxmsgobj(&msg_obj); |
| RuslanUrya | 0:dfe6edabb8ec | 531 | } |
| RuslanUrya | 0:dfe6edabb8ec | 532 | } |
| RuslanUrya | 0:dfe6edabb8ec | 533 | else |
| RuslanUrya | 0:dfe6edabb8ec | 534 | { |
| RuslanUrya | 0:dfe6edabb8ec | 535 | return -1; |
| RuslanUrya | 0:dfe6edabb8ec | 536 | } |
| RuslanUrya | 0:dfe6edabb8ec | 537 | |
| RuslanUrya | 0:dfe6edabb8ec | 538 | return 0; |
| RuslanUrya | 0:dfe6edabb8ec | 539 | } |
| RuslanUrya | 0:dfe6edabb8ec | 540 | |
| RuslanUrya | 0:dfe6edabb8ec | 541 | uavcan::uint64_t CanDriver::getErrorCount() const |
| RuslanUrya | 0:dfe6edabb8ec | 542 | { |
| RuslanUrya | 0:dfe6edabb8ec | 543 | CriticalSectionLocker locker; |
| RuslanUrya | 0:dfe6edabb8ec | 544 | return std::uint64_t(error_cnt) + std::uint64_t(rx_queue.getOverflowCount()); |
| RuslanUrya | 0:dfe6edabb8ec | 545 | } |
| RuslanUrya | 0:dfe6edabb8ec | 546 | |
| RuslanUrya | 0:dfe6edabb8ec | 547 | uavcan::uint16_t CanDriver::getNumFilters() const |
| RuslanUrya | 0:dfe6edabb8ec | 548 | { |
| RuslanUrya | 0:dfe6edabb8ec | 549 | return NumberOfRxMessageObjects; |
| RuslanUrya | 0:dfe6edabb8ec | 550 | } |
| RuslanUrya | 0:dfe6edabb8ec | 551 | |
| RuslanUrya | 0:dfe6edabb8ec | 552 | uavcan::ICanIface* CanDriver::getIface(uavcan::uint8_t iface_index) |
| RuslanUrya | 0:dfe6edabb8ec | 553 | { |
| RuslanUrya | 0:dfe6edabb8ec | 554 | return (iface_index == 0) ? this : nullptr; |
| RuslanUrya | 0:dfe6edabb8ec | 555 | } |
| RuslanUrya | 0:dfe6edabb8ec | 556 | |
| RuslanUrya | 0:dfe6edabb8ec | 557 | uavcan::uint8_t CanDriver::getNumIfaces() const |
| RuslanUrya | 0:dfe6edabb8ec | 558 | { |
| RuslanUrya | 0:dfe6edabb8ec | 559 | return 1; |
| RuslanUrya | 0:dfe6edabb8ec | 560 | } |
| RuslanUrya | 0:dfe6edabb8ec | 561 | |
| RuslanUrya | 0:dfe6edabb8ec | 562 | } |
| RuslanUrya | 0:dfe6edabb8ec | 563 | |
| RuslanUrya | 0:dfe6edabb8ec | 564 | /* |
| RuslanUrya | 0:dfe6edabb8ec | 565 | * C_CAN handlers |
| RuslanUrya | 0:dfe6edabb8ec | 566 | */ |
| RuslanUrya | 0:dfe6edabb8ec | 567 | extern "C" |
| RuslanUrya | 0:dfe6edabb8ec | 568 | { |
| RuslanUrya | 0:dfe6edabb8ec | 569 | |
| RuslanUrya | 0:dfe6edabb8ec | 570 | void canRxCallback(std::uint8_t msg_obj_num) |
| RuslanUrya | 0:dfe6edabb8ec | 571 | { |
| RuslanUrya | 0:dfe6edabb8ec | 572 | using namespace uavcan_lpc11c24; |
| RuslanUrya | 0:dfe6edabb8ec | 573 | |
| RuslanUrya | 0:dfe6edabb8ec | 574 | auto msg_obj = CCAN_MSG_OBJ_T(); |
| RuslanUrya | 0:dfe6edabb8ec | 575 | msg_obj.msgobj = msg_obj_num; |
| RuslanUrya | 0:dfe6edabb8ec | 576 | LPC_CCAN_API->can_receive(&msg_obj); |
| RuslanUrya | 0:dfe6edabb8ec | 577 | |
| RuslanUrya | 0:dfe6edabb8ec | 578 | uavcan::CanFrame frame; |
| RuslanUrya | 0:dfe6edabb8ec | 579 | |
| RuslanUrya | 0:dfe6edabb8ec | 580 | // CAN ID, EXT or not |
| RuslanUrya | 0:dfe6edabb8ec | 581 | if (msg_obj.mode_id & CAN_MSGOBJ_EXT) |
| RuslanUrya | 0:dfe6edabb8ec | 582 | { |
| RuslanUrya | 0:dfe6edabb8ec | 583 | frame.id = msg_obj.mode_id & uavcan::CanFrame::MaskExtID; |
| RuslanUrya | 0:dfe6edabb8ec | 584 | frame.id |= uavcan::CanFrame::FlagEFF; |
| RuslanUrya | 0:dfe6edabb8ec | 585 | } |
| RuslanUrya | 0:dfe6edabb8ec | 586 | else |
| RuslanUrya | 0:dfe6edabb8ec | 587 | { |
| RuslanUrya | 0:dfe6edabb8ec | 588 | frame.id = msg_obj.mode_id & uavcan::CanFrame::MaskStdID; |
| RuslanUrya | 0:dfe6edabb8ec | 589 | } |
| RuslanUrya | 0:dfe6edabb8ec | 590 | |
| RuslanUrya | 0:dfe6edabb8ec | 591 | // RTR |
| RuslanUrya | 0:dfe6edabb8ec | 592 | if (msg_obj.mode_id & CAN_MSGOBJ_RTR) |
| RuslanUrya | 0:dfe6edabb8ec | 593 | { |
| RuslanUrya | 0:dfe6edabb8ec | 594 | frame.id |= uavcan::CanFrame::FlagRTR; |
| RuslanUrya | 0:dfe6edabb8ec | 595 | } |
| RuslanUrya | 0:dfe6edabb8ec | 596 | |
| RuslanUrya | 0:dfe6edabb8ec | 597 | // Payload |
| RuslanUrya | 0:dfe6edabb8ec | 598 | frame.dlc = msg_obj.dlc; |
| RuslanUrya | 0:dfe6edabb8ec | 599 | uavcan::copy(msg_obj.data, msg_obj.data + msg_obj.dlc, frame.data); |
| RuslanUrya | 0:dfe6edabb8ec | 600 | |
| RuslanUrya | 0:dfe6edabb8ec | 601 | rx_queue.push(frame, last_irq_utc_timestamp); |
| RuslanUrya | 0:dfe6edabb8ec | 602 | had_activity = true; |
| RuslanUrya | 0:dfe6edabb8ec | 603 | } |
| RuslanUrya | 0:dfe6edabb8ec | 604 | |
| RuslanUrya | 0:dfe6edabb8ec | 605 | void canTxCallback(std::uint8_t msg_obj_num) |
| RuslanUrya | 0:dfe6edabb8ec | 606 | { |
| RuslanUrya | 0:dfe6edabb8ec | 607 | using namespace uavcan_lpc11c24; |
| RuslanUrya | 0:dfe6edabb8ec | 608 | |
| RuslanUrya | 0:dfe6edabb8ec | 609 | (void)msg_obj_num; |
| RuslanUrya | 0:dfe6edabb8ec | 610 | |
| RuslanUrya | 0:dfe6edabb8ec | 611 | tx_pending = false; |
| RuslanUrya | 0:dfe6edabb8ec | 612 | had_activity = true; |
| RuslanUrya | 0:dfe6edabb8ec | 613 | } |
| RuslanUrya | 0:dfe6edabb8ec | 614 | |
| RuslanUrya | 0:dfe6edabb8ec | 615 | void canErrorCallback(std::uint32_t error_info) |
| RuslanUrya | 0:dfe6edabb8ec | 616 | { |
| RuslanUrya | 0:dfe6edabb8ec | 617 | using namespace uavcan_lpc11c24; |
| RuslanUrya | 0:dfe6edabb8ec | 618 | |
| RuslanUrya | 0:dfe6edabb8ec | 619 | // Updating the error counter |
| RuslanUrya | 0:dfe6edabb8ec | 620 | if ((error_info != 0) && (error_cnt < 0xFFFFFFFFUL)) |
| RuslanUrya | 0:dfe6edabb8ec | 621 | { |
| RuslanUrya | 0:dfe6edabb8ec | 622 | error_cnt++; |
| RuslanUrya | 0:dfe6edabb8ec | 623 | } |
| RuslanUrya | 0:dfe6edabb8ec | 624 | |
| RuslanUrya | 0:dfe6edabb8ec | 625 | // Serving abort requests |
| RuslanUrya | 0:dfe6edabb8ec | 626 | if (tx_pending && tx_abort_on_error) |
| RuslanUrya | 0:dfe6edabb8ec | 627 | { |
| RuslanUrya | 0:dfe6edabb8ec | 628 | tx_pending = false; |
| RuslanUrya | 0:dfe6edabb8ec | 629 | tx_abort_on_error = false; |
| RuslanUrya | 0:dfe6edabb8ec | 630 | |
| RuslanUrya | 0:dfe6edabb8ec | 631 | // Using the first interface, because this approach seems to be compliant with the BASIC mode (just in case) |
| RuslanUrya | 0:dfe6edabb8ec | 632 | c_can::CAN.IF[0].CMDREQ = TxMessageObjectNumber; |
| RuslanUrya | 0:dfe6edabb8ec | 633 | c_can::CAN.IF[0].CMDMSK.W = c_can::IF_CMDMSK_W_WR_RD; // Clearing IF_CMDMSK_W_TXRQST |
| RuslanUrya | 0:dfe6edabb8ec | 634 | c_can::CAN.IF[0].MCTRL &= ~c_can::IF_MCTRL_TXRQST; // Clearing IF_MCTRL_TXRQST |
| RuslanUrya | 0:dfe6edabb8ec | 635 | } |
| RuslanUrya | 0:dfe6edabb8ec | 636 | } |
| RuslanUrya | 0:dfe6edabb8ec | 637 | |
| RuslanUrya | 0:dfe6edabb8ec | 638 | void CAN_IRQHandler(); |
| RuslanUrya | 0:dfe6edabb8ec | 639 | |
| RuslanUrya | 0:dfe6edabb8ec | 640 | void CAN_IRQHandler() |
| RuslanUrya | 0:dfe6edabb8ec | 641 | { |
| RuslanUrya | 0:dfe6edabb8ec | 642 | using namespace uavcan_lpc11c24; |
| RuslanUrya | 0:dfe6edabb8ec | 643 | |
| RuslanUrya | 0:dfe6edabb8ec | 644 | last_irq_utc_timestamp = clock::getUtcUSecFromCanInterrupt(); |
| RuslanUrya | 0:dfe6edabb8ec | 645 | |
| RuslanUrya | 0:dfe6edabb8ec | 646 | LPC_CCAN_API->isr(); |
| RuslanUrya | 0:dfe6edabb8ec | 647 | } |
| RuslanUrya | 0:dfe6edabb8ec | 648 | |
| RuslanUrya | 0:dfe6edabb8ec | 649 | } |