diff --git a/libraries/AP_HAL_ChibiOS/CAN.h b/libraries/AP_HAL_ChibiOS/CAN.h index 04d49cbf57..d46075c857 100644 --- a/libraries/AP_HAL_ChibiOS/CAN.h +++ b/libraries/AP_HAL_ChibiOS/CAN.h @@ -26,9 +26,9 @@ #include #include -#include -#include -#include +#include "CANThread.h" +#include "CANClock.h" +#include "CANIface.h" #define MAX_NUMBER_OF_CAN_INTERFACES 2 #define MAX_NUMBER_OF_CAN_DRIVERS 2 @@ -66,7 +66,7 @@ public: private: bool initialized_; - uavcan_stm32::CanInitHelper can_helper; + ChibiOS_CAN::CanInitHelper can_helper; }; } diff --git a/libraries/AP_HAL_ChibiOS/CANClock.cpp b/libraries/AP_HAL_ChibiOS/CANClock.cpp new file mode 100644 index 0000000000..5c31711c2c --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/CANClock.cpp @@ -0,0 +1,408 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2014 Pavel Kirienko + * + * Permission is hereby granted, free of charge, to any person obtaining a copy of + * this software and associated documentation files (the "Software"), to deal in + * the Software without restriction, including without limitation the rights to + * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of + * the Software, and to permit persons to whom the Software is furnished to do so, + * subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS + * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR + * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER + * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN + * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + */ + +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Modified for Ardupilot by Siddharth Bharat Purohit + */ + +#include "CANClock.h" +#include "CANThread.h" +#include "CANInternal.h" + +#ifndef UAVCAN_STM32_TIMER_NUMBER +#define UAVCAN_STM32_TIMER_NUMBER 0 +#endif + +#if UAVCAN_STM32_TIMER_NUMBER + +#include +#include + +/* + * Timer instance + */ +# if (CH_KERNEL_MAJOR == 2) +# define TIMX UAVCAN_STM32_GLUE2(TIM, UAVCAN_STM32_TIMER_NUMBER) +# define TIMX_IRQn UAVCAN_STM32_GLUE3(TIM, UAVCAN_STM32_TIMER_NUMBER, _IRQn) +# define TIMX_INPUT_CLOCK STM32_TIMCLK1 +# endif + +# if (CH_KERNEL_MAJOR == 3 || CH_KERNEL_MAJOR == 4) +# define TIMX UAVCAN_STM32_GLUE2(STM32_TIM, UAVCAN_STM32_TIMER_NUMBER) +# define TIMX_IRQn UAVCAN_STM32_GLUE3(STM32_TIM, UAVCAN_STM32_TIMER_NUMBER, _NUMBER) +# define TIMX_IRQHandler UAVCAN_STM32_GLUE3(STM32_TIM, UAVCAN_STM32_TIMER_NUMBER, _HANDLER) +# define TIMX_INPUT_CLOCK STM32_TIMCLK1 +# else +# define TIMX_IRQHandler UAVCAN_STM32_GLUE3(TIM, UAVCAN_STM32_TIMER_NUMBER, _IRQHandler) +# endif + +# if UAVCAN_STM32_TIMER_NUMBER >= 2 && UAVCAN_STM32_TIMER_NUMBER <= 7 +# define TIMX_RCC_ENR RCC->APB1ENR +# define TIMX_RCC_RSTR RCC->APB1RSTR +# define TIMX_RCC_ENR_MASK UAVCAN_STM32_GLUE3(RCC_APB1ENR_TIM, UAVCAN_STM32_TIMER_NUMBER, EN) +# define TIMX_RCC_RSTR_MASK UAVCAN_STM32_GLUE3(RCC_APB1RSTR_TIM, UAVCAN_STM32_TIMER_NUMBER, RST) +# else +# error "This UAVCAN_STM32_TIMER_NUMBER is not supported yet" +# endif + +# if (TIMX_INPUT_CLOCK % 1000000) != 0 +# error "No way, timer clock must be divisible to 1e6. FIXME!" +# endif + +extern "C" UAVCAN_STM32_IRQ_HANDLER(TIMX_IRQHandler); + +namespace ChibiOS_CAN { +namespace clock { +namespace { + +const uavcan::uint32_t USecPerOverflow = 65536; + +Mutex mutex; + +bool initialized = false; + +bool utc_set = false; +bool utc_locked = false; +uavcan::uint32_t utc_jump_cnt = 0; +UtcSyncParams utc_sync_params; +float utc_prev_adj = 0; +float utc_rel_rate_ppm = 0; +float utc_rel_rate_error_integral = 0; +uavcan::int32_t utc_accumulated_correction_nsec = 0; +uavcan::int32_t utc_correction_nsec_per_overflow = 0; +uavcan::MonotonicTime prev_utc_adj_at; + +uavcan::uint64_t time_mono = 0; +uavcan::uint64_t time_utc = 0; + +} + +void init() +{ + CriticalSectionLocker lock; + if (initialized) { + return; + } + initialized = true; + + + // Power-on and reset + TIMX_RCC_ENR |= TIMX_RCC_ENR_MASK; + TIMX_RCC_RSTR |= TIMX_RCC_RSTR_MASK; + TIMX_RCC_RSTR &= ~TIMX_RCC_RSTR_MASK; + + // Enable IRQ + nvicEnableVector(TIMX_IRQn, UAVCAN_STM32_IRQ_PRIORITY_MASK); + +# if (TIMX_INPUT_CLOCK % 1000000) != 0 +# error "No way, timer clock must be divisible to 1e6. FIXME!" +# endif + + // Start the timer + TIMX->ARR = 0xFFFF; + TIMX->PSC = (TIMX_INPUT_CLOCK / 1000000) - 1; // 1 tick == 1 microsecond + TIMX->CR1 = TIM_CR1_URS; + TIMX->SR = 0; + TIMX->EGR = TIM_EGR_UG; // Reload immediately + TIMX->DIER = TIM_DIER_UIE; + TIMX->CR1 = TIM_CR1_CEN; // Start + +} + +void setUtc(uavcan::UtcTime time) +{ + MutexLocker mlocker(mutex); + UAVCAN_ASSERT(initialized); + + { + CriticalSectionLocker locker; + time_utc = time.toUSec(); + } + + utc_set = true; + utc_locked = false; + utc_jump_cnt++; + utc_prev_adj = 0; + utc_rel_rate_ppm = 0; +} + +static uavcan::uint64_t sampleUtcFromCriticalSection() +{ + UAVCAN_ASSERT(initialized); + UAVCAN_ASSERT(TIMX->DIER & TIM_DIER_UIE); + + volatile uavcan::uint64_t time = time_utc; + volatile uavcan::uint32_t cnt = TIMX->CNT; + + if (TIMX->SR & TIM_SR_UIF) { + cnt = TIMX->CNT; + const uavcan::int32_t add = uavcan::int32_t(USecPerOverflow) + + (utc_accumulated_correction_nsec + utc_correction_nsec_per_overflow) / 1000; + time = uavcan::uint64_t(uavcan::int64_t(time) + add); + } + return time + cnt; +} + +uavcan::uint64_t getUtcUSecFromCanInterrupt() +{ + return utc_set ? sampleUtcFromCriticalSection() : 0; +} + +uavcan::MonotonicTime getMonotonic() +{ + uavcan::uint64_t usec = 0; + // Scope Critical section + { + CriticalSectionLocker locker; + + volatile uavcan::uint64_t time = time_mono; + + + volatile uavcan::uint32_t cnt = TIMX->CNT; + if (TIMX->SR & TIM_SR_UIF) { + cnt = TIMX->CNT; + time += USecPerOverflow; + } + usec = time + cnt; + +# ifndef NDEBUG + static uavcan::uint64_t prev_usec = 0; // Self-test + UAVCAN_ASSERT(prev_usec <= usec); + (void)prev_usec; + prev_usec = usec; +# endif + } // End Scope Critical section + + return uavcan::MonotonicTime::fromUSec(usec); +} + +uavcan::UtcTime getUtc() +{ + if (utc_set) { + uavcan::uint64_t usec = 0; + { + CriticalSectionLocker locker; + usec = sampleUtcFromCriticalSection(); + } + return uavcan::UtcTime::fromUSec(usec); + } + return uavcan::UtcTime(); +} + +static float lowpass(float xold, float xnew, float corner, float dt) +{ + const float tau = 1.F / corner; + return (dt * xnew + tau * xold) / (dt + tau); +} + +static void updateRatePID(uavcan::UtcDuration adjustment) +{ + const uavcan::MonotonicTime ts = getMonotonic(); + const float dt = float((ts - prev_utc_adj_at).toUSec()) / 1e6F; + prev_utc_adj_at = ts; + const float adj_usec = float(adjustment.toUSec()); + + /* + * Target relative rate in PPM + * Positive to go faster + */ + const float target_rel_rate_ppm = adj_usec * utc_sync_params.offset_p; + + /* + * Current relative rate in PPM + * Positive if the local clock is faster + */ + const float new_rel_rate_ppm = (utc_prev_adj - adj_usec) / dt; // rate error in [usec/sec], which is PPM + utc_prev_adj = adj_usec; + utc_rel_rate_ppm = lowpass(utc_rel_rate_ppm, new_rel_rate_ppm, utc_sync_params.rate_error_corner_freq, dt); + + const float rel_rate_error = target_rel_rate_ppm - utc_rel_rate_ppm; + + if (dt > 10) { + utc_rel_rate_error_integral = 0; + } + else { + utc_rel_rate_error_integral += rel_rate_error * dt * utc_sync_params.rate_i; + utc_rel_rate_error_integral = + uavcan::max(utc_rel_rate_error_integral, -utc_sync_params.max_rate_correction_ppm); + utc_rel_rate_error_integral = + uavcan::min(utc_rel_rate_error_integral, utc_sync_params.max_rate_correction_ppm); + } + + /* + * Rate controller + */ + float total_rate_correction_ppm = rel_rate_error + utc_rel_rate_error_integral; + total_rate_correction_ppm = uavcan::max(total_rate_correction_ppm, -utc_sync_params.max_rate_correction_ppm); + total_rate_correction_ppm = uavcan::min(total_rate_correction_ppm, utc_sync_params.max_rate_correction_ppm); + + utc_correction_nsec_per_overflow = uavcan::int32_t((USecPerOverflow * 1000) * (total_rate_correction_ppm / 1e6F)); + + // syslog("$ adj=%f rel_rate=%f rel_rate_eint=%f tgt_rel_rate=%f ppm=%f\n", + // adj_usec, utc_rel_rate_ppm, utc_rel_rate_error_integral, target_rel_rate_ppm, + // total_rate_correction_ppm); +} + +void adjustUtc(uavcan::UtcDuration adjustment) +{ + MutexLocker mlocker(mutex); + UAVCAN_ASSERT(initialized); + + if (adjustment.getAbs() > utc_sync_params.min_jump || !utc_set) { + const uavcan::int64_t adj_usec = adjustment.toUSec(); + + { + CriticalSectionLocker locker; + if ((adj_usec < 0) && uavcan::uint64_t(-adj_usec) > time_utc) { + time_utc = 1; + } + else { + time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + adj_usec); + } + } + + utc_set = true; + utc_locked = false; + utc_jump_cnt++; + utc_prev_adj = 0; + utc_rel_rate_ppm = 0; + } + else { + updateRatePID(adjustment); + + if (!utc_locked) { + utc_locked = + (std::abs(utc_rel_rate_ppm) < utc_sync_params.lock_thres_rate_ppm) && + (std::abs(utc_prev_adj) < utc_sync_params.lock_thres_offset.toUSec()); + } + } +} + +float getUtcRateCorrectionPPM() +{ + MutexLocker mlocker(mutex); + const float rate_correction_mult = float(utc_correction_nsec_per_overflow) / float(USecPerOverflow * 1000); + return 1e6F * rate_correction_mult; +} + +uavcan::uint32_t getUtcJumpCount() +{ + MutexLocker mlocker(mutex); + return utc_jump_cnt; +} + +bool isUtcLocked() +{ + MutexLocker mlocker(mutex); + return utc_locked; +} + +UtcSyncParams getUtcSyncParams() +{ + MutexLocker mlocker(mutex); + return utc_sync_params; +} + +void setUtcSyncParams(const UtcSyncParams& params) +{ + MutexLocker mlocker(mutex); + // Add some sanity check + utc_sync_params = params; +} + +} // namespace clock + +SystemClock& SystemClock::instance() +{ + static union SystemClockStorage { + uavcan::uint8_t buffer[sizeof(SystemClock)]; + long long _aligner_1; + long double _aligner_2; + } storage; + + SystemClock* const ptr = reinterpret_cast(storage.buffer); + + if (!clock::initialized) { + MutexLocker mlocker(clock::mutex); + clock::init(); + new (ptr)SystemClock(); + } + return *ptr; +} + +} // namespace uavcan_stm32 + + +/** + * Timer interrupt handler + */ + +extern "C" +UAVCAN_STM32_IRQ_HANDLER(TIMX_IRQHandler) +{ + UAVCAN_STM32_IRQ_PROLOGUE(); + + TIMX->SR = 0; + + using namespace uavcan_stm32::clock; + UAVCAN_ASSERT(initialized); + + time_mono += USecPerOverflow; + + if (utc_set) { + time_utc += USecPerOverflow; + utc_accumulated_correction_nsec += utc_correction_nsec_per_overflow; + if (std::abs(utc_accumulated_correction_nsec) >= 1000) { + time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + utc_accumulated_correction_nsec / 1000); + utc_accumulated_correction_nsec %= 1000; + } + + // Correction decay - 1 nsec per 65536 usec + if (utc_correction_nsec_per_overflow > 0) { + utc_correction_nsec_per_overflow--; + } + else if (utc_correction_nsec_per_overflow < 0) { + utc_correction_nsec_per_overflow++; + } + else { + ; // Zero + } + } + + UAVCAN_STM32_IRQ_EPILOGUE(); +} + +#endif diff --git a/libraries/AP_HAL_ChibiOS/CANClock.h b/libraries/AP_HAL_ChibiOS/CANClock.h new file mode 100644 index 0000000000..cd8ec9db27 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/CANClock.h @@ -0,0 +1,161 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2014 Pavel Kirienko + * + * Permission is hereby granted, free of charge, to any person obtaining a copy of + * this software and associated documentation files (the "Software"), to deal in + * the Software without restriction, including without limitation the rights to + * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of + * the Software, and to permit persons to whom the Software is furnished to do so, + * subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS + * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR + * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER + * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN + * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + */ + +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Modified for Ardupilot by Siddharth Bharat Purohit + */ + +#pragma once + +#include + +namespace ChibiOS_CAN { + +namespace clock { +/** + * Starts the clock. + * Can be called multiple times, only the first call will be effective. + */ +void init(); + +/** + * Returns current monotonic time since the moment when clock::init() was called. + * This function is thread safe. + */ +uavcan::MonotonicTime getMonotonic(); + +/** + * Sets the driver's notion of the system UTC. It should be called + * at startup and any time the system clock is updated from an + * external source that is not the UAVCAN Timesync master. + * This function is thread safe. + */ +void setUtc(uavcan::UtcTime time); + +/** + * Returns UTC time if it has been set, otherwise returns zero time. + * This function is thread safe. + */ +uavcan::UtcTime getUtc(); + +/** + * Performs UTC phase and frequency adjustment. + * The UTC time will be zero until first adjustment has been performed. + * This function is thread safe. + */ +void adjustUtc(uavcan::UtcDuration adjustment); + +/** + * UTC clock synchronization parameters + */ +struct UtcSyncParams { + float offset_p; ///< PPM per one usec error + float rate_i; ///< PPM per one PPM error for second + float rate_error_corner_freq; + float max_rate_correction_ppm; + float lock_thres_rate_ppm; + uavcan::UtcDuration lock_thres_offset; + uavcan::UtcDuration min_jump; ///< Min error to jump rather than change rate + + UtcSyncParams() + : offset_p(0.01F) + , rate_i(0.02F) + , rate_error_corner_freq(0.01F) + , max_rate_correction_ppm(300.0F) + , lock_thres_rate_ppm(2.0F) + , lock_thres_offset(uavcan::UtcDuration::fromMSec(4)) + , min_jump(uavcan::UtcDuration::fromMSec(10)) + { } +}; + +/** + * Clock rate error. + * Positive if the hardware timer is slower than reference time. + * This function is thread safe. + */ +float getUtcRateCorrectionPPM(); + +/** + * Number of non-gradual adjustments performed so far. + * Ideally should be zero. + * This function is thread safe. + */ +uavcan::uint32_t getUtcJumpCount(); + +/** + * Whether UTC is synchronized and locked. + * This function is thread safe. + */ +bool isUtcLocked(); + +/** + * UTC sync params get/set. + * Both functions are thread safe. + */ +UtcSyncParams getUtcSyncParams(); +void setUtcSyncParams(const UtcSyncParams& params); + +} + +/** + * Adapter for uavcan::ISystemClock. + */ +class SystemClock : public uavcan::ISystemClock, uavcan::Noncopyable { + SystemClock() { } + + virtual void adjustUtc(uavcan::UtcDuration adjustment) + { + clock::adjustUtc(adjustment); + } + +public: + virtual uavcan::MonotonicTime getMonotonic() const + { + return clock::getMonotonic(); + } + virtual uavcan::UtcTime getUtc() const + { + return clock::getUtc(); + } + + /** + * Calls clock::init() as needed. + * This function is thread safe. + */ + static SystemClock& instance(); +}; + +} diff --git a/libraries/AP_HAL_ChibiOS/CANIface.h b/libraries/AP_HAL_ChibiOS/CANIface.h new file mode 100644 index 0000000000..a42b741c28 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/CANIface.h @@ -0,0 +1,442 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2014 Pavel Kirienko + * + * Permission is hereby granted, free of charge, to any person obtaining a copy of + * this software and associated documentation files (the "Software"), to deal in + * the Software without restriction, including without limitation the rights to + * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of + * the Software, and to permit persons to whom the Software is furnished to do so, + * subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS + * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR + * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER + * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN + * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + */ + +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Code by Siddharth Bharat Purohit + */ + +#pragma once + +#include "CANThread.h" +#include "CANIface.h" +#include "bxcan.hpp" + +class SLCANRouter; + +namespace ChibiOS_CAN { +/** + * Driver error codes. + * These values can be returned from driver functions negated. + */ +//static const uavcan::int16_t ErrUnknown = 1000; ///< Reserved for future use +static const uavcan::int16_t ErrNotImplemented = 1001; ///< Feature not implemented +static const uavcan::int16_t ErrInvalidBitRate = 1002; ///< Bit rate not supported +static const uavcan::int16_t ErrLogic = 1003; ///< Internal logic error +static const uavcan::int16_t ErrUnsupportedFrame = 1004; ///< Frame not supported (e.g. RTR, CAN FD, etc) +static const uavcan::int16_t ErrMsrInakNotSet = 1005; ///< INAK bit of the MSR register is not 1 +static const uavcan::int16_t ErrMsrInakNotCleared = 1006; ///< INAK bit of the MSR register is not 0 +static const uavcan::int16_t ErrBitRateNotDetected = 1007; ///< Auto bit rate detection could not be finished +static const uavcan::int16_t ErrFilterNumConfigs = 1008; ///< Number of filters is more than supported + +/** + * RX queue item. + * The application shall not use this directly. + */ +struct CanRxItem { + uavcan::uint64_t utc_usec; + uavcan::CanFrame frame; + uavcan::CanIOFlags flags; + CanRxItem() + : utc_usec(0) + , flags(0) + { } +}; + +/** + * Single CAN iface. + * The application shall not use this directly. + */ +class CanIface : public uavcan::ICanIface, uavcan::Noncopyable { + friend class ::SLCANRouter; + class RxQueue { + CanRxItem* const buf_; + const uavcan::uint8_t capacity_; + uavcan::uint8_t in_; + uavcan::uint8_t out_; + uavcan::uint8_t len_; + uavcan::uint32_t overflow_cnt_; + + void registerOverflow(); + + public: + RxQueue(CanRxItem* buf, uavcan::uint8_t capacity) + : buf_(buf) + , capacity_(capacity) + , in_(0) + , out_(0) + , len_(0) + , overflow_cnt_(0) + { } + + void push(const uavcan::CanFrame& frame, const uint64_t& utc_usec, uavcan::CanIOFlags flags); + void pop(uavcan::CanFrame& out_frame, uavcan::uint64_t& out_utc_usec, uavcan::CanIOFlags& out_flags); + + void reset(); + + unsigned getLength() const + { + return len_; + } + + uavcan::uint32_t getOverflowCount() const + { + return overflow_cnt_; + } + }; + + struct Timings { + uavcan::uint16_t prescaler; + uavcan::uint8_t sjw; + uavcan::uint8_t bs1; + uavcan::uint8_t bs2; + + Timings() + : prescaler(0) + , sjw(0) + , bs1(0) + , bs2(0) + { } + }; + + struct TxItem { + uavcan::MonotonicTime deadline; + uavcan::CanFrame frame; + bool pending; + bool loopback; + bool abort_on_error; + + TxItem() + : pending(false) + , loopback(false) + , abort_on_error(false) + { } + }; + + enum { NumTxMailboxes = 3 }; + enum { NumFilters = 14 }; + + static const uavcan::uint32_t TSR_ABRQx[NumTxMailboxes]; + + RxQueue rx_queue_; + bxcan::CanType* const can_; + uavcan::uint64_t error_cnt_; + uavcan::uint32_t served_aborts_cnt_; + BusEvent& update_event_; + TxItem pending_tx_[NumTxMailboxes]; + uavcan::uint8_t peak_tx_mailbox_index_; + const uavcan::uint8_t self_index_; + bool had_activity_; + + int computeTimings(uavcan::uint32_t target_bitrate, Timings& out_timings); + + virtual uavcan::int16_t send(const uavcan::CanFrame& frame, uavcan::MonotonicTime tx_deadline, + uavcan::CanIOFlags flags); + + virtual uavcan::int16_t receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic, + uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags); + + virtual uavcan::int16_t configureFilters(const uavcan::CanFilterConfig* filter_configs, + uavcan::uint16_t num_configs); + + virtual uavcan::uint16_t getNumFilters() const + { + return NumFilters; + } + + void handleTxMailboxInterrupt(uavcan::uint8_t mailbox_index, bool txok, uavcan::uint64_t utc_usec); + + bool waitMsrINakBitStateChange(bool target_state); + +public: + enum { MaxRxQueueCapacity = 254 }; + + enum OperatingMode { + NormalMode, + SilentMode + }; + + CanIface(bxcan::CanType* can, BusEvent& update_event, uavcan::uint8_t self_index, + CanRxItem* rx_queue_buffer, uavcan::uint8_t rx_queue_capacity) + : rx_queue_(rx_queue_buffer, rx_queue_capacity) + , can_(can) + , error_cnt_(0) + , served_aborts_cnt_(0) + , update_event_(update_event) + , peak_tx_mailbox_index_(0) + , self_index_(self_index) + , had_activity_(false) + { + UAVCAN_ASSERT(self_index_ < UAVCAN_STM32_NUM_IFACES); + } + + /** + * Initializes the hardware CAN controller. + * Assumes: + * - Iface clock is enabled + * - Iface has been resetted via RCC + * - Caller will configure NVIC by itself + */ + int init(const uavcan::uint32_t bitrate, const OperatingMode mode); + + void handleTxInterrupt(uavcan::uint64_t utc_usec); + void handleRxInterrupt(uavcan::uint8_t fifo_index, uavcan::uint64_t utc_usec); + + /** + * This method is used to count errors and abort transmission on error if necessary. + * This functionality used to be implemented in the SCE interrupt handler, but that approach was + * generating too much processing overhead, especially on disconnected interfaces. + * + * Should be called from RX ISR, TX ISR, and select(); interrupts must be enabled. + */ + void pollErrorFlagsFromISR(); + + void discardTimedOutTxMailboxes(uavcan::MonotonicTime current_time); + + bool canAcceptNewTxFrame(const uavcan::CanFrame& frame) const; + bool isRxBufferEmpty() const; + + /** + * Number of RX frames lost due to queue overflow. + * This is an atomic read, it doesn't require a critical section. + */ + uavcan::uint32_t getRxQueueOverflowCount() const + { + return rx_queue_.getOverflowCount(); + } + + /** + * Total number of hardware failures and other kinds of errors (e.g. queue overruns). + * May increase continuously if the interface is not connected to the bus. + */ + virtual uavcan::uint64_t getErrorCount() const; + + /** + * Number of times the driver exercised library's requirement to abort transmission on first error. + * This is an atomic read, it doesn't require a critical section. + * See @ref uavcan::CanIOFlagAbortOnError. + */ + uavcan::uint32_t getVoluntaryTxAbortCount() const + { + return served_aborts_cnt_; + } + + /** + * Returns the number of frames pending in the RX queue. + * This is intended for debug use only. + */ + unsigned getRxQueueLength() const; + + /** + * Whether this iface had at least one successful IO since the previous call of this method. + * This is designed for use with iface activity LEDs. + */ + bool hadActivity(); + + /** + * Peak number of TX mailboxes used concurrently since initialization. + * Range is [1, 3]. + * Value of 3 suggests that priority inversion could be taking place. + */ + uavcan::uint8_t getPeakNumTxMailboxesUsed() const + { + return uavcan::uint8_t(peak_tx_mailbox_index_ + 1); + } +}; + +/** + * CAN driver, incorporates all available CAN ifaces. + * Please avoid direct use, prefer @ref CanInitHelper instead. + */ +class CanDriver : public uavcan::ICanDriver, uavcan::Noncopyable { + BusEvent update_event_; + CanIface if0_; +#if UAVCAN_STM32_NUM_IFACES > 1 + CanIface if1_; +#endif + bool initialized_by_me_[UAVCAN_STM32_NUM_IFACES]; + uavcan::uint8_t num_ifaces_; + uavcan::uint8_t if_int_to_gl_index_[UAVCAN_STM32_NUM_IFACES]; + + + virtual uavcan::int16_t select(uavcan::CanSelectMasks& inout_masks, + const uavcan::CanFrame* (& pending_tx)[uavcan::MaxCanIfaces], + uavcan::MonotonicTime blocking_deadline); + + static void initOnce(); + static void initOnce(uavcan::uint8_t can_number, bool enable_irqs); + +public: + template + CanDriver(CanRxItem(&rx_queue_storage)[UAVCAN_STM32_NUM_IFACES][RxQueueCapacity]) + : update_event_(*this) + , if0_(bxcan::Can[0], update_event_, 0, rx_queue_storage[0], RxQueueCapacity) +#if UAVCAN_STM32_NUM_IFACES > 1 + , if1_(bxcan::Can[1], update_event_, 1, rx_queue_storage[1], RxQueueCapacity) +#endif + { + uavcan::StaticAssert<(RxQueueCapacity <= CanIface::MaxRxQueueCapacity)>::check(); + } + + /** + * This function returns select masks indicating which interfaces are available for read/write. + */ + uavcan::CanSelectMasks makeSelectMasks(const uavcan::CanFrame* (& pending_tx)[uavcan::MaxCanIfaces]) const; + + BusEvent* getUpdateEvent() + { + return &update_event_; + } + /** + * Whether there's at least one interface where receive() would return a frame. + */ + bool hasReadableInterfaces() const; + + /** + * Returns zero if OK. + * Returns negative value if failed (e.g. invalid bitrate). + */ + int init(const uavcan::uint32_t bitrate, const CanIface::OperatingMode mode); + int init(const uavcan::uint32_t bitrate, const CanIface::OperatingMode mode, uavcan::uint8_t can_number); + + virtual CanIface* getIface(uavcan::uint8_t iface_index); + + virtual uavcan::uint8_t getNumIfaces() const + { + return num_ifaces_; + } + + /** + * Whether at least one iface had at least one successful IO since previous call of this method. + * This is designed for use with iface activity LEDs. + */ + bool hadActivity(); +}; + +/** + * Helper class. + * Normally only this class should be used by the application. + * 145 usec per Extended CAN frame @ 1 Mbps, e.g. 32 RX slots * 145 usec --> 4.6 msec before RX queue overruns. + */ +template +class CanInitHelper { + CanRxItem queue_storage_[UAVCAN_STM32_NUM_IFACES][RxQueueCapacity]; + +public: + enum { BitRateAutoDetect = 0 }; + + CanDriver driver; + + CanInitHelper() : + driver(queue_storage_) + { } + + /** + * This overload simply configures the provided bitrate. + * Auto bit rate detection will not be performed. + * Bitrate value must be positive. + * @return Negative value on error; non-negative on success. Refer to constants Err*. + */ + int init(uavcan::uint32_t bitrate) + { + return driver.init(bitrate, CanIface::NormalMode); + } + + int init(const uavcan::uint32_t bitrate, const CanIface::OperatingMode mode, uavcan::uint8_t can_number) + { + return driver.init(bitrate, mode, can_number); + } + + /** + * This function can either initialize the driver at a fixed bit rate, or it can perform + * automatic bit rate detection. For theory please refer to the CiA application note #801. + * + * @param delay_callable A callable entity that suspends execution for strictly more than one second. + * The callable entity will be invoked without arguments. + * @ref getRecommendedListeningDelay(). + * + * @param inout_bitrate Fixed bit rate or zero. Zero invokes the bit rate detection process. + * If auto detection was used, the function will update the argument + * with established bit rate. In case of an error the value will be undefined. + * + * @return Negative value on error; non-negative on success. Refer to constants Err*. + */ + template + int init(DelayCallable delay_callable, uavcan::uint32_t& inout_bitrate = BitRateAutoDetect) + { + if (inout_bitrate > 0) { + return driver.init(inout_bitrate, CanIface::NormalMode); + } + else { + static const uavcan::uint32_t StandardBitRates[] = { + 1000000, + 500000, + 250000, + 125000 + }; + + for (uavcan::uint8_t br = 0; br < sizeof(StandardBitRates) / sizeof(StandardBitRates[0]); br++) { + inout_bitrate = StandardBitRates[br]; + + const int res = driver.init(inout_bitrate, CanIface::SilentMode); + + delay_callable(); + + if (res >= 0) { + for (uavcan::uint8_t iface = 0; iface < driver.getNumIfaces(); iface++) { + if (!driver.getIface(iface)->isRxBufferEmpty()) { + // Re-initializing in normal mode + return driver.init(inout_bitrate, CanIface::NormalMode); + } + } + } + } + + return -ErrBitRateNotDetected; + } + } + + /** + * Use this value for listening delay during automatic bit rate detection. + */ + static uavcan::MonotonicDuration getRecommendedListeningDelay() + { + return uavcan::MonotonicDuration::fromMSec(1050); + } +}; + +} + +#include "CANSerialRouter.h" diff --git a/libraries/AP_HAL_ChibiOS/CANInternal.h b/libraries/AP_HAL_ChibiOS/CANInternal.h new file mode 100644 index 0000000000..aeec9c3741 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/CANInternal.h @@ -0,0 +1,99 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2014 Pavel Kirienko + * + * Permission is hereby granted, free of charge, to any person obtaining a copy of + * this software and associated documentation files (the "Software"), to deal in + * the Software without restriction, including without limitation the rights to + * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of + * the Software, and to permit persons to whom the Software is furnished to do so, + * subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS + * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR + * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER + * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN + * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + */ + +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Code by Siddharth Bharat Purohit + */ + +#pragma once + +#include +#include +/** + * Debug output + */ +#ifndef UAVCAN_STM32_LOG +# if 0 +# define UAVCAN_STM32_LOG(fmt, ...) syslog("uavcan_stm32: " fmt "\n", ##__VA_ARGS__) +# else +# define UAVCAN_STM32_LOG(...) ((void)0) +# endif +#endif + +/** + * IRQ handler macros + */ +# define UAVCAN_STM32_IRQ_HANDLER(id) CH_IRQ_HANDLER(id) +# define UAVCAN_STM32_IRQ_PROLOGUE() CH_IRQ_PROLOGUE() +# define UAVCAN_STM32_IRQ_EPILOGUE() CH_IRQ_EPILOGUE() + +/** + * Priority mask for timer and CAN interrupts. + */ +# ifndef UAVCAN_STM32_IRQ_PRIORITY_MASK +# if (CH_KERNEL_MAJOR == 2) +# define UAVCAN_STM32_IRQ_PRIORITY_MASK CORTEX_PRIORITY_MASK(CORTEX_MAX_KERNEL_PRIORITY) +# else // ChibiOS 3+ +# define UAVCAN_STM32_IRQ_PRIORITY_MASK CORTEX_MAX_KERNEL_PRIORITY +# endif +# endif + +/** + * Glue macros + */ +#define UAVCAN_STM32_GLUE2_(A, B) A##B +#define UAVCAN_STM32_GLUE2(A, B) UAVCAN_STM32_GLUE2_(A, B) + +#define UAVCAN_STM32_GLUE3_(A, B, C) A##B##C +#define UAVCAN_STM32_GLUE3(A, B, C) UAVCAN_STM32_GLUE3_(A, B, C) + +namespace ChibiOS_CAN { + +struct CriticalSectionLocker { + CriticalSectionLocker() + { + chSysSuspend(); + } + ~CriticalSectionLocker() + { + chSysEnable(); + } +}; + +namespace clock { +uint64_t getUtcUSecFromCanInterrupt(); +} +} diff --git a/libraries/AP_HAL_ChibiOS/CAN.cpp b/libraries/AP_HAL_ChibiOS/CANManager.cpp similarity index 86% rename from libraries/AP_HAL_ChibiOS/CAN.cpp rename to libraries/AP_HAL_ChibiOS/CANManager.cpp index fd6a19aa0c..bceee26a4b 100644 --- a/libraries/AP_HAL_ChibiOS/CAN.cpp +++ b/libraries/AP_HAL_ChibiOS/CANManager.cpp @@ -11,7 +11,7 @@ * * You should have received a copy of the GNU General Public License along * with this program. If not, see . - * + * * Code by Andrew Tridgell and Siddharth Bharat Purohit * Based on stm32 can driver by Pavel Kirienko */ @@ -20,14 +20,13 @@ #if HAL_WITH_UAVCAN -#include +#include "CANInternal.h" +#include "CANClock.h" -using namespace ChibiOS; -using namespace uavcan_stm32; extern const AP_HAL::HAL& hal; - -namespace uavcan_stm32 { +using namespace ChibiOS; +namespace ChibiOS_CAN { uint64_t clock::getUtcUSecFromCanInterrupt() { @@ -43,7 +42,7 @@ uavcan::MonotonicTime clock::getMonotonic() bool CANManager::begin(uint32_t bitrate, uint8_t can_number) { - return (can_helper.init(bitrate, CanIface::OperatingMode::NormalMode, can_number) == 0); + return (can_helper.init(bitrate, ChibiOS_CAN::CanIface::OperatingMode::NormalMode, can_number) == 0); } bool CANManager::is_initialized() diff --git a/libraries/AP_HAL_ChibiOS/CANThread.cpp b/libraries/AP_HAL_ChibiOS/CANThread.cpp new file mode 100644 index 0000000000..d1532f2955 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/CANThread.cpp @@ -0,0 +1,117 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2014 Pavel Kirienko + * + * Permission is hereby granted, free of charge, to any person obtaining a copy of + * this software and associated documentation files (the "Software"), to deal in + * the Software without restriction, including without limitation the rights to + * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of + * the Software, and to permit persons to whom the Software is furnished to do so, + * subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS + * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR + * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER + * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN + * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + */ + +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Modified for Ardupilot by Siddharth Bharat Purohit + */ + +#include "CANThread.h" +#include "CANClock.h" +#include "CANIface.h" +#include "CANInternal.h" + + +namespace ChibiOS_CAN { +/* + * BusEvent + */ +bool BusEvent::wait(uavcan::MonotonicDuration duration) +{ + // set maximum time to allow for 16 bit timers running at 1MHz + static const uavcan::int64_t MaxDelayUSec = 0x000FFFF; + + const uavcan::int64_t usec = duration.toUSec(); + msg_t ret = msg_t(); + + if (usec <= 0) { +# if (CH_KERNEL_MAJOR == 2) + ret = sem_.waitTimeout(TIME_IMMEDIATE); +# else // ChibiOS 3+ + ret = sem_.wait(TIME_IMMEDIATE); +# endif + } + else { +# if (CH_KERNEL_MAJOR == 2) + ret = sem_.waitTimeout((usec > MaxDelayUSec) ? US2ST(MaxDelayUSec) : US2ST(usec)); +# elif (CH_KERNEL_MAJOR >= 5) + ret = sem_.wait((usec > MaxDelayUSec) ? chTimeUS2I(MaxDelayUSec) : chTimeUS2I(usec)); +# else // ChibiOS 3+ + ret = sem_.wait((usec > MaxDelayUSec) ? US2ST(MaxDelayUSec) : US2ST(usec)); +# endif + } +# if (CH_KERNEL_MAJOR == 2) + return ret == RDY_OK; +# else // ChibiOS 3+ + return ret == MSG_OK; +# endif +} + +void BusEvent::signal() +{ + sem_.signal(); +} + +void BusEvent::signalFromInterrupt() +{ +# if (CH_KERNEL_MAJOR == 2) + chSysLockFromIsr(); + sem_.signalI(); + chSysUnlockFromIsr(); +# else // ChibiOS 3+ + chSysLockFromISR(); + sem_.signalI(); + chSysUnlockFromISR(); +# endif +} + +/* + * Mutex + */ +void Mutex::lock() +{ + mtx_.lock(); +} + +void Mutex::unlock() +{ +# if (CH_KERNEL_MAJOR == 2) + chibios_rt::BaseThread::unlockMutex(); +# else // ChibiOS 3+ + mtx_.unlock(); +# endif +} + +} diff --git a/libraries/AP_HAL_ChibiOS/CANThread.h b/libraries/AP_HAL_ChibiOS/CANThread.h new file mode 100644 index 0000000000..def2752aa2 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/CANThread.h @@ -0,0 +1,91 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2014 Pavel Kirienko + * + * Permission is hereby granted, free of charge, to any person obtaining a copy of + * this software and associated documentation files (the "Software"), to deal in + * the Software without restriction, including without limitation the rights to + * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of + * the Software, and to permit persons to whom the Software is furnished to do so, + * subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS + * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR + * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER + * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN + * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + */ + +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Modified for Ardupilot by Siddharth Bharat Purohit + */ + +#pragma once + +# include + +#include + +namespace ChibiOS_CAN { + +class CanDriver; + +class BusEvent { + chibios_rt::CounterSemaphore sem_; + +public: + BusEvent(CanDriver& can_driver) + : sem_(0) + { + (void)can_driver; + } + + bool wait(uavcan::MonotonicDuration duration); + + void signal(); + + void signalFromInterrupt(); +}; + +class Mutex { + chibios_rt::Mutex mtx_; + +public: + void lock(); + void unlock(); +}; + +class MutexLocker { + Mutex& mutex_; + +public: + MutexLocker(Mutex& mutex) + : mutex_(mutex) + { + mutex_.lock(); + } + ~MutexLocker() + { + mutex_.unlock(); + } +}; + +} diff --git a/libraries/AP_HAL_ChibiOS/CanIface.cpp b/libraries/AP_HAL_ChibiOS/CanIface.cpp new file mode 100644 index 0000000000..601f938be8 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/CanIface.cpp @@ -0,0 +1,1244 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2014 Pavel Kirienko + * + * Permission is hereby granted, free of charge, to any person obtaining a copy of + * this software and associated documentation files (the "Software"), to deal in + * the Software without restriction, including without limitation the rights to + * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of + * the Software, and to permit persons to whom the Software is furnished to do so, + * subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS + * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR + * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER + * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN + * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + */ + +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Code by Siddharth Bharat Purohit + */ + +#include +#include +#include "CANIface.h" +#include "CANClock.h" +#include "CANInternal.h" +#include "CANSerialRouter.h" +#include +# include + +#if CH_KERNEL_MAJOR == 2 +# if !(defined(STM32F10X_CL) || defined(STM32F2XX) || defined(STM32F3XX) || defined(STM32F4XX)) +// IRQ numbers +# define CAN1_RX0_IRQn USB_LP_CAN1_RX0_IRQn +# define CAN1_TX_IRQn USB_HP_CAN1_TX_IRQn +// IRQ vectors +# if !defined(CAN1_RX0_IRQHandler) || !defined(CAN1_TX_IRQHandler) +# define CAN1_TX_IRQHandler USB_HP_CAN1_TX_IRQHandler +# define CAN1_RX0_IRQHandler USB_LP_CAN1_RX0_IRQHandler +# endif +# endif +#endif + +#if (CH_KERNEL_MAJOR == 3 || CH_KERNEL_MAJOR == 4 || CH_KERNEL_MAJOR == 5) +#define CAN1_TX_IRQHandler STM32_CAN1_TX_HANDLER +#define CAN1_RX0_IRQHandler STM32_CAN1_RX0_HANDLER +#define CAN1_RX1_IRQHandler STM32_CAN1_RX1_HANDLER +#define CAN2_TX_IRQHandler STM32_CAN2_TX_HANDLER +#define CAN2_RX0_IRQHandler STM32_CAN2_RX0_HANDLER +#define CAN2_RX1_IRQHandler STM32_CAN2_RX1_HANDLER +#endif + + +/* STM32F3's only CAN inteface does not have a number. */ +#if defined(STM32F3XX) +#define RCC_APB1ENR_CAN1EN RCC_APB1ENR_CANEN +#define RCC_APB1RSTR_CAN1RST RCC_APB1RSTR_CANRST +#define CAN1_TX_IRQn CAN_TX_IRQn +#define CAN1_RX0_IRQn CAN_RX0_IRQn +#define CAN1_RX1_IRQn CAN_RX1_IRQn +#define CAN1_TX_IRQHandler CAN_TX_IRQHandler +#define CAN1_RX0_IRQHandler CAN_RX0_IRQHandler +#define CAN1_RX1_IRQHandler CAN_RX1_IRQHandler +#endif + + +namespace ChibiOS_CAN +{ +namespace +{ + +CanIface* ifaces[UAVCAN_STM32_NUM_IFACES] = +{ + UAVCAN_NULLPTR +#if UAVCAN_STM32_NUM_IFACES > 1 + , UAVCAN_NULLPTR +#endif +}; + +inline void handleTxInterrupt(uavcan::uint8_t iface_index) +{ + UAVCAN_ASSERT(iface_index < UAVCAN_STM32_NUM_IFACES); + uavcan::uint64_t utc_usec = clock::getUtcUSecFromCanInterrupt(); + if (utc_usec > 0) + { + utc_usec--; + } + if (ifaces[iface_index] != UAVCAN_NULLPTR) + { + ifaces[iface_index]->handleTxInterrupt(utc_usec); + } + else + { + UAVCAN_ASSERT(0); + } +} + +inline void handleRxInterrupt(uavcan::uint8_t iface_index, uavcan::uint8_t fifo_index) +{ + UAVCAN_ASSERT(iface_index < UAVCAN_STM32_NUM_IFACES); + uavcan::uint64_t utc_usec = clock::getUtcUSecFromCanInterrupt(); + if (utc_usec > 0) + { + utc_usec--; + } + if (ifaces[iface_index] != UAVCAN_NULLPTR) + { + ifaces[iface_index]->handleRxInterrupt(fifo_index, utc_usec); + } + else + { + UAVCAN_ASSERT(0); + } +} + +} // namespace + +/* + * CanIface::RxQueue + */ +void CanIface::RxQueue::registerOverflow() +{ + if (overflow_cnt_ < 0xFFFFFFFF) + { + overflow_cnt_++; + } +} + +void CanIface::RxQueue::push(const uavcan::CanFrame& frame, const uint64_t& utc_usec, uavcan::CanIOFlags flags) +{ + buf_[in_].frame = frame; + buf_[in_].utc_usec = utc_usec; + buf_[in_].flags = flags; + in_++; + if (in_ >= capacity_) + { + in_ = 0; + } + len_++; + if (len_ > capacity_) + { + len_ = capacity_; + registerOverflow(); + out_++; + if (out_ >= capacity_) + { + out_ = 0; + } + } +} + +void CanIface::RxQueue::pop(uavcan::CanFrame& out_frame, uavcan::uint64_t& out_utc_usec, uavcan::CanIOFlags& out_flags) +{ + if (len_ > 0) + { + out_frame = buf_[out_].frame; + out_utc_usec = buf_[out_].utc_usec; + out_flags = buf_[out_].flags; + out_++; + if (out_ >= capacity_) + { + out_ = 0; + } + len_--; + } + else { UAVCAN_ASSERT(0); } +} + +void CanIface::RxQueue::reset() +{ + in_ = 0; + out_ = 0; + len_ = 0; + overflow_cnt_ = 0; +} + +/* + * CanIface + */ +const uavcan::uint32_t CanIface::TSR_ABRQx[CanIface::NumTxMailboxes] = +{ + bxcan::TSR_ABRQ0, + bxcan::TSR_ABRQ1, + bxcan::TSR_ABRQ2 +}; + +int CanIface::computeTimings(const uavcan::uint32_t target_bitrate, Timings& out_timings) +{ + if (target_bitrate < 1) + { + return -ErrInvalidBitRate; + } + + /* + * Hardware configuration + */ + const uavcan::uint32_t pclk = STM32_PCLK1; + + static const int MaxBS1 = 16; + static const int MaxBS2 = 8; + + /* + * Ref. "Automatic Baudrate Detection in CANopen Networks", U. Koppe, MicroControl GmbH & Co. KG + * CAN in Automation, 2003 + * + * According to the source, optimal quanta per bit are: + * Bitrate Optimal Maximum + * 1000 kbps 8 10 + * 500 kbps 16 17 + * 250 kbps 16 17 + * 125 kbps 16 17 + */ + const int max_quanta_per_bit = (target_bitrate >= 1000000) ? 10 : 17; + + UAVCAN_ASSERT(max_quanta_per_bit <= (MaxBS1 + MaxBS2)); + + static const int MaxSamplePointLocation = 900; + + /* + * Computing (prescaler * BS): + * BITRATE = 1 / (PRESCALER * (1 / PCLK) * (1 + BS1 + BS2)) -- See the Reference Manual + * BITRATE = PCLK / (PRESCALER * (1 + BS1 + BS2)) -- Simplified + * let: + * BS = 1 + BS1 + BS2 -- Number of time quanta per bit + * PRESCALER_BS = PRESCALER * BS + * ==> + * PRESCALER_BS = PCLK / BITRATE + */ + const uavcan::uint32_t prescaler_bs = pclk / target_bitrate; + + /* + * Searching for such prescaler value so that the number of quanta per bit is highest. + */ + uavcan::uint8_t bs1_bs2_sum = uavcan::uint8_t(max_quanta_per_bit - 1); + + while ((prescaler_bs % (1 + bs1_bs2_sum)) != 0) + { + if (bs1_bs2_sum <= 2) + { + return -ErrInvalidBitRate; // No solution + } + bs1_bs2_sum--; + } + + const uavcan::uint32_t prescaler = prescaler_bs / (1 + bs1_bs2_sum); + if ((prescaler < 1U) || (prescaler > 1024U)) + { + return -ErrInvalidBitRate; // No solution + } + + /* + * Now we have a constraint: (BS1 + BS2) == bs1_bs2_sum. + * We need to find the values so that the sample point is as close as possible to the optimal value. + * + * Solve[(1 + bs1)/(1 + bs1 + bs2) == 7/8, bs2] (* Where 7/8 is 0.875, the recommended sample point location *) + * {{bs2 -> (1 + bs1)/7}} + * + * Hence: + * bs2 = (1 + bs1) / 7 + * bs1 = (7 * bs1_bs2_sum - 1) / 8 + * + * Sample point location can be computed as follows: + * Sample point location = (1 + bs1) / (1 + bs1 + bs2) + * + * Since the optimal solution is so close to the maximum, we prepare two solutions, and then pick the best one: + * - With rounding to nearest + * - With rounding to zero + */ + struct BsPair + { + uavcan::uint8_t bs1; + uavcan::uint8_t bs2; + uavcan::uint16_t sample_point_permill; + + BsPair() : + bs1(0), + bs2(0), + sample_point_permill(0) + { } + + BsPair(uavcan::uint8_t bs1_bs2_sum, uavcan::uint8_t arg_bs1) : + bs1(arg_bs1), + bs2(uavcan::uint8_t(bs1_bs2_sum - bs1)), + sample_point_permill(uavcan::uint16_t(1000 * (1 + bs1) / (1 + bs1 + bs2))) + { + UAVCAN_ASSERT(bs1_bs2_sum > arg_bs1); + } + + bool isValid() const { return (bs1 >= 1) && (bs1 <= MaxBS1) && (bs2 >= 1) && (bs2 <= MaxBS2); } + }; + + // First attempt with rounding to nearest + BsPair solution(bs1_bs2_sum, uavcan::uint8_t(((7 * bs1_bs2_sum - 1) + 4) / 8)); + + if (solution.sample_point_permill > MaxSamplePointLocation) + { + // Second attempt with rounding to zero + solution = BsPair(bs1_bs2_sum, uavcan::uint8_t((7 * bs1_bs2_sum - 1) / 8)); + } + + /* + * Final validation + * Helpful Python: + * def sample_point_from_btr(x): + * assert 0b0011110010000000111111000000000 & x == 0 + * ts2,ts1,brp = (x>>20)&7, (x>>16)&15, x&511 + * return (1+ts1+1)/(1+ts1+1+ts2+1) + * + */ + if ((target_bitrate != (pclk / (prescaler * (1 + solution.bs1 + solution.bs2)))) || !solution.isValid()) + { + UAVCAN_ASSERT(0); + return -ErrLogic; + } + + UAVCAN_STM32_LOG("Timings: quanta/bit: %d, sample point location: %.1f%%", + int(1 + solution.bs1 + solution.bs2), float(solution.sample_point_permill) / 10.F); + + out_timings.prescaler = uavcan::uint16_t(prescaler - 1U); + out_timings.sjw = 0; // Which means one + out_timings.bs1 = uavcan::uint8_t(solution.bs1 - 1); + out_timings.bs2 = uavcan::uint8_t(solution.bs2 - 1); + return 0; +} + +uavcan::int16_t CanIface::send(const uavcan::CanFrame& frame, uavcan::MonotonicTime tx_deadline, + uavcan::CanIOFlags flags) +{ + if (frame.isErrorFrame() || frame.dlc > 8) + { + return -ErrUnsupportedFrame; + } + + /* + * Normally we should perform the same check as in @ref canAcceptNewTxFrame(), because + * it is possible that the highest-priority frame between select() and send() could have been + * replaced with a lower priority one due to TX timeout. But we don't do this check because: + * + * - It is a highly unlikely scenario. + * + * - Frames do not timeout on a properly functioning bus. Since frames do not timeout, the new + * frame can only have higher priority, which doesn't break the logic. + * + * - If high-priority frames are timing out in the TX queue, there's probably a lot of other + * issues to take care of before this one becomes relevant. + * + * - It takes CPU time. Not just CPU time, but critical section time, which is expensive. + */ + CriticalSectionLocker lock; + + /* + * Seeking for an empty slot + */ + uavcan::uint8_t txmailbox = 0xFF; + if ((can_->TSR & bxcan::TSR_TME0) == bxcan::TSR_TME0) + { + txmailbox = 0; + } + else if ((can_->TSR & bxcan::TSR_TME1) == bxcan::TSR_TME1) + { + txmailbox = 1; + } + else if ((can_->TSR & bxcan::TSR_TME2) == bxcan::TSR_TME2) + { + txmailbox = 2; + } + else + { + return 0; // No transmission for you. + } + + peak_tx_mailbox_index_ = uavcan::max(peak_tx_mailbox_index_, txmailbox); // Statistics + + /* + * Setting up the mailbox + */ + bxcan::TxMailboxType& mb = can_->TxMailbox[txmailbox]; + if (frame.isExtended()) + { + mb.TIR = ((frame.id & uavcan::CanFrame::MaskExtID) << 3) | bxcan::TIR_IDE; + } + else + { + mb.TIR = ((frame.id & uavcan::CanFrame::MaskStdID) << 21); + } + + if (frame.isRemoteTransmissionRequest()) + { + mb.TIR |= bxcan::TIR_RTR; + } + + mb.TDTR = frame.dlc; + + mb.TDHR = (uavcan::uint32_t(frame.data[7]) << 24) | + (uavcan::uint32_t(frame.data[6]) << 16) | + (uavcan::uint32_t(frame.data[5]) << 8) | + (uavcan::uint32_t(frame.data[4]) << 0); + mb.TDLR = (uavcan::uint32_t(frame.data[3]) << 24) | + (uavcan::uint32_t(frame.data[2]) << 16) | + (uavcan::uint32_t(frame.data[1]) << 8) | + (uavcan::uint32_t(frame.data[0]) << 0); + + mb.TIR |= bxcan::TIR_TXRQ; // Go. + + /* + * Registering the pending transmission so we can track its deadline and loopback it as needed + */ + TxItem& txi = pending_tx_[txmailbox]; + txi.deadline = tx_deadline; + txi.frame = frame; + txi.loopback = (flags & uavcan::CanIOFlagLoopback) != 0; + txi.abort_on_error = (flags & uavcan::CanIOFlagAbortOnError) != 0; + txi.pending = true; + return 1; +} + +uavcan::int16_t CanIface::receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic, + uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags) +{ + out_ts_monotonic = clock::getMonotonic(); // High precision is not required for monotonic timestamps + uavcan::uint64_t utc_usec = 0; + { + CriticalSectionLocker lock; + if (rx_queue_.getLength() == 0) + { + return 0; + } + rx_queue_.pop(out_frame, utc_usec, out_flags); + } + out_ts_utc = uavcan::UtcTime::fromUSec(utc_usec); + return 1; +} + +uavcan::int16_t CanIface::configureFilters(const uavcan::CanFilterConfig* filter_configs, + uavcan::uint16_t num_configs) +{ + if (num_configs <= NumFilters) + { + CriticalSectionLocker lock; + + can_->FMR |= bxcan::FMR_FINIT; + + // Slave (CAN2) gets half of the filters + can_->FMR &= ~0x00003F00UL; + can_->FMR |= static_cast(NumFilters) << 8; + + can_->FFA1R = 0x0AAAAAAA; // FIFO's are interleaved between filters + can_->FM1R = 0; // Identifier Mask mode + can_->FS1R = 0x7ffffff; // Single 32-bit for all + + const uint8_t filter_start_index = (self_index_ == 0) ? 0 : NumFilters; + + if (num_configs == 0) + { + can_->FilterRegister[filter_start_index].FR1 = 0; + can_->FilterRegister[filter_start_index].FR2 = 0; + can_->FA1R = 1 << filter_start_index; + } + else + { + for (uint8_t i = 0; i < NumFilters; i++) + { + if (i < num_configs) + { + uint32_t id = 0; + uint32_t mask = 0; + + const uavcan::CanFilterConfig* const cfg = filter_configs + i; + + if ((cfg->id & uavcan::CanFrame::FlagEFF) || !(cfg->mask & uavcan::CanFrame::FlagEFF)) + { + id = (cfg->id & uavcan::CanFrame::MaskExtID) << 3; + mask = (cfg->mask & uavcan::CanFrame::MaskExtID) << 3; + id |= bxcan::RIR_IDE; + } + else + { + id = (cfg->id & uavcan::CanFrame::MaskStdID) << 21; // Regular std frames, nothing fancy. + mask = (cfg->mask & uavcan::CanFrame::MaskStdID) << 21; // Boring. + } + + if (cfg->id & uavcan::CanFrame::FlagRTR) + { + id |= bxcan::RIR_RTR; + } + + if (cfg->mask & uavcan::CanFrame::FlagEFF) + { + mask |= bxcan::RIR_IDE; + } + + if (cfg->mask & uavcan::CanFrame::FlagRTR) + { + mask |= bxcan::RIR_RTR; + } + + can_->FilterRegister[filter_start_index + i].FR1 = id; + can_->FilterRegister[filter_start_index + i].FR2 = mask; + + can_->FA1R |= (1 << (filter_start_index + i)); + } + else + { + can_->FA1R &= ~(1 << (filter_start_index + i)); + } + } + } + + can_->FMR &= ~bxcan::FMR_FINIT; + + return 0; + } + + return -ErrFilterNumConfigs; +} + +bool CanIface::waitMsrINakBitStateChange(bool target_state) +{ + const unsigned Timeout = 1000; + for (unsigned wait_ack = 0; wait_ack < Timeout; wait_ack++) + { + const bool state = (can_->MSR & bxcan::MSR_INAK) != 0; + if (state == target_state) + { + return true; + } +#if CH_KERNEL_MAJOR >= 5 + ::chThdSleep(chTimeMS2I(1)); +#else + ::chThdSleep(MS2ST(1)); +#endif + } + return false; +} + +int CanIface::init(const uavcan::uint32_t bitrate, const OperatingMode mode) +{ + /* + * We need to silence the controller in the first order, otherwise it may interfere with the following operations. + */ + { + CriticalSectionLocker lock; + + can_->MCR &= ~bxcan::MCR_SLEEP; // Exit sleep mode + can_->MCR |= bxcan::MCR_INRQ; // Request init + + can_->IER = 0; // Disable interrupts while initialization is in progress + } + + if (!waitMsrINakBitStateChange(true)) + { + UAVCAN_STM32_LOG("MSR INAK not set"); + can_->MCR = bxcan::MCR_RESET; + return -ErrMsrInakNotSet; + } + + /* + * Object state - interrupts are disabled, so it's safe to modify it now + */ + rx_queue_.reset(); + error_cnt_ = 0; + served_aborts_cnt_ = 0; + uavcan::fill_n(pending_tx_, NumTxMailboxes, TxItem()); + peak_tx_mailbox_index_ = 0; + had_activity_ = false; + + /* + * CAN timings for this bitrate + */ + Timings timings; + const int timings_res = computeTimings(bitrate, timings); + if (timings_res < 0) + { + can_->MCR = bxcan::MCR_RESET; + return timings_res; + } + UAVCAN_STM32_LOG("Timings: presc=%u sjw=%u bs1=%u bs2=%u", + unsigned(timings.prescaler), unsigned(timings.sjw), unsigned(timings.bs1), unsigned(timings.bs2)); + + /* + * Hardware initialization (the hardware has already confirmed initialization mode, see above) + */ + can_->MCR = bxcan::MCR_ABOM | bxcan::MCR_AWUM | bxcan::MCR_INRQ; // RM page 648 + + can_->BTR = ((timings.sjw & 3U) << 24) | + ((timings.bs1 & 15U) << 16) | + ((timings.bs2 & 7U) << 20) | + (timings.prescaler & 1023U) | + ((mode == SilentMode) ? bxcan::BTR_SILM : 0); + + can_->IER = bxcan::IER_TMEIE | // TX mailbox empty + bxcan::IER_FMPIE0 | // RX FIFO 0 is not empty + bxcan::IER_FMPIE1; // RX FIFO 1 is not empty + + can_->MCR &= ~bxcan::MCR_INRQ; // Leave init mode + + if (!waitMsrINakBitStateChange(false)) + { + UAVCAN_STM32_LOG("MSR INAK not cleared"); + can_->MCR = bxcan::MCR_RESET; + return -ErrMsrInakNotCleared; + } + + /* + * Default filter configuration + */ + if (self_index_ == 0) + { + can_->FMR |= bxcan::FMR_FINIT; + + can_->FMR &= 0xFFFFC0F1; + can_->FMR |= static_cast(NumFilters) << 8; // Slave (CAN2) gets half of the filters + + can_->FFA1R = 0; // All assigned to FIFO0 by default + can_->FM1R = 0; // Indentifier Mask mode + +#if UAVCAN_STM32_NUM_IFACES > 1 + can_->FS1R = 0x7ffffff; // Single 32-bit for all + can_->FilterRegister[0].FR1 = 0; // CAN1 accepts everything + can_->FilterRegister[0].FR2 = 0; + can_->FilterRegister[NumFilters].FR1 = 0; // CAN2 accepts everything + can_->FilterRegister[NumFilters].FR2 = 0; + can_->FA1R = 1 | (1 << NumFilters); // One filter per each iface +#else + can_->FS1R = 0x1fff; + can_->FilterRegister[0].FR1 = 0; + can_->FilterRegister[0].FR2 = 0; + can_->FA1R = 1; +#endif + + can_->FMR &= ~bxcan::FMR_FINIT; + } + + return 0; +} + +void CanIface::handleTxMailboxInterrupt(uavcan::uint8_t mailbox_index, bool txok, const uavcan::uint64_t utc_usec) +{ + UAVCAN_ASSERT(mailbox_index < NumTxMailboxes); + + had_activity_ = had_activity_ || txok; + + TxItem& txi = pending_tx_[mailbox_index]; + + if (txi.loopback && txok && txi.pending) + { + rx_queue_.push(txi.frame, utc_usec, uavcan::CanIOFlagLoopback); + } + + txi.pending = false; +} + +void CanIface::handleTxInterrupt(const uavcan::uint64_t utc_usec) +{ + // TXOK == false means that there was a hardware failure + if (can_->TSR & bxcan::TSR_RQCP0) + { + const bool txok = can_->TSR & bxcan::TSR_TXOK0; + can_->TSR = bxcan::TSR_RQCP0; + handleTxMailboxInterrupt(0, txok, utc_usec); + } + if (can_->TSR & bxcan::TSR_RQCP1) + { + const bool txok = can_->TSR & bxcan::TSR_TXOK1; + can_->TSR = bxcan::TSR_RQCP1; + handleTxMailboxInterrupt(1, txok, utc_usec); + } + if (can_->TSR & bxcan::TSR_RQCP2) + { + const bool txok = can_->TSR & bxcan::TSR_TXOK2; + can_->TSR = bxcan::TSR_RQCP2; + handleTxMailboxInterrupt(2, txok, utc_usec); + } + update_event_.signalFromInterrupt(); + + pollErrorFlagsFromISR(); + + #if UAVCAN_STM32_FREERTOS + update_event_.yieldFromISR(); + #endif +} + +void CanIface::handleRxInterrupt(uavcan::uint8_t fifo_index, uavcan::uint64_t utc_usec) +{ + UAVCAN_ASSERT(fifo_index < 2); + + volatile uavcan::uint32_t* const rfr_reg = (fifo_index == 0) ? &can_->RF0R : &can_->RF1R; + if ((*rfr_reg & bxcan::RFR_FMP_MASK) == 0) + { + UAVCAN_ASSERT(0); // Weird, IRQ is here but no data to read + return; + } + + /* + * Register overflow as a hardware error + */ + if ((*rfr_reg & bxcan::RFR_FOVR) != 0) + { + error_cnt_++; + } + + /* + * Read the frame contents + */ + uavcan::CanFrame frame; + const bxcan::RxMailboxType& rf = can_->RxMailbox[fifo_index]; + + if ((rf.RIR & bxcan::RIR_IDE) == 0) + { + frame.id = uavcan::CanFrame::MaskStdID & (rf.RIR >> 21); + } + else + { + frame.id = uavcan::CanFrame::MaskExtID & (rf.RIR >> 3); + frame.id |= uavcan::CanFrame::FlagEFF; + } + + if ((rf.RIR & bxcan::RIR_RTR) != 0) + { + frame.id |= uavcan::CanFrame::FlagRTR; + } + + frame.dlc = rf.RDTR & 15; + + frame.data[0] = uavcan::uint8_t(0xFF & (rf.RDLR >> 0)); + frame.data[1] = uavcan::uint8_t(0xFF & (rf.RDLR >> 8)); + frame.data[2] = uavcan::uint8_t(0xFF & (rf.RDLR >> 16)); + frame.data[3] = uavcan::uint8_t(0xFF & (rf.RDLR >> 24)); + frame.data[4] = uavcan::uint8_t(0xFF & (rf.RDHR >> 0)); + frame.data[5] = uavcan::uint8_t(0xFF & (rf.RDHR >> 8)); + frame.data[6] = uavcan::uint8_t(0xFF & (rf.RDHR >> 16)); + frame.data[7] = uavcan::uint8_t(0xFF & (rf.RDHR >> 24)); + + *rfr_reg = bxcan::RFR_RFOM | bxcan::RFR_FOVR | bxcan::RFR_FULL; // Release FIFO entry we just read + + /* + * Store with timeout into the FIFO buffer and signal update event + */ + rx_queue_.push(frame, utc_usec, 0); + slcan_router().route_frame_to_slcan(this, frame, utc_usec); + + had_activity_ = true; + update_event_.signalFromInterrupt(); + + pollErrorFlagsFromISR(); + + #if UAVCAN_STM32_FREERTOS + update_event_.yieldFromISR(); + #endif +} + +void CanIface::pollErrorFlagsFromISR() +{ + const uavcan::uint8_t lec = uavcan::uint8_t((can_->ESR & bxcan::ESR_LEC_MASK) >> bxcan::ESR_LEC_SHIFT); + if (lec != 0) + { + can_->ESR = 0; + error_cnt_++; + + // Serving abort requests + for (int i = 0; i < NumTxMailboxes; i++) // Dear compiler, may I suggest you to unroll this loop please. + { + TxItem& txi = pending_tx_[i]; + if (txi.pending && txi.abort_on_error) + { + can_->TSR = TSR_ABRQx[i]; + txi.pending = false; + served_aborts_cnt_++; + } + } + } +} + +void CanIface::discardTimedOutTxMailboxes(uavcan::MonotonicTime current_time) +{ + CriticalSectionLocker lock; + for (int i = 0; i < NumTxMailboxes; i++) + { + TxItem& txi = pending_tx_[i]; + if (txi.pending && txi.deadline < current_time) + { + can_->TSR = TSR_ABRQx[i]; // Goodnight sweet transmission + txi.pending = false; + error_cnt_++; + } + } +} + +bool CanIface::canAcceptNewTxFrame(const uavcan::CanFrame& frame) const +{ + /* + * We can accept more frames only if the following conditions are satisfied: + * - There is at least one TX mailbox free (obvious enough); + * - The priority of the new frame is higher than priority of all TX mailboxes. + */ + { + static const uavcan::uint32_t TME = bxcan::TSR_TME0 | bxcan::TSR_TME1 | bxcan::TSR_TME2; + const uavcan::uint32_t tme = can_->TSR & TME; + + if (tme == TME) // All TX mailboxes are free (as in freedom). + { + return true; + } + + if (tme == 0) // All TX mailboxes are busy transmitting. + { + return false; + } + } + + /* + * The second condition requires a critical section. + */ + CriticalSectionLocker lock; + + for (int mbx = 0; mbx < NumTxMailboxes; mbx++) + { + if (pending_tx_[mbx].pending && !frame.priorityHigherThan(pending_tx_[mbx].frame)) + { + return false; // There's a mailbox whose priority is higher or equal the priority of the new frame. + } + } + + return true; // This new frame will be added to a free TX mailbox in the next @ref send(). +} + +bool CanIface::isRxBufferEmpty() const +{ + CriticalSectionLocker lock; + return rx_queue_.getLength() == 0; +} + +uavcan::uint64_t CanIface::getErrorCount() const +{ + CriticalSectionLocker lock; + return error_cnt_ + rx_queue_.getOverflowCount(); +} + +unsigned CanIface::getRxQueueLength() const +{ + CriticalSectionLocker lock; + return rx_queue_.getLength(); +} + +bool CanIface::hadActivity() +{ + CriticalSectionLocker lock; + const bool ret = had_activity_; + had_activity_ = false; + return ret; +} + +/* + * CanDriver + */ +uavcan::CanSelectMasks CanDriver::makeSelectMasks(const uavcan::CanFrame* (& pending_tx)[uavcan::MaxCanIfaces]) const +{ + uavcan::CanSelectMasks msk; + + for (uavcan::uint8_t i = 0; i < num_ifaces_; i++) { + CanIface* iface = ifaces[if_int_to_gl_index_[i]]; + msk.read |= (iface->isRxBufferEmpty() ? 0 : 1) << i; + + if (pending_tx[i] != UAVCAN_NULLPTR) + { + msk.write |= (iface->canAcceptNewTxFrame(*pending_tx[i]) ? 1 : 0) << i; + } + } + + return msk; +} + +bool CanDriver::hasReadableInterfaces() const +{ + for (uavcan::uint8_t i = 0; i < num_ifaces_; i++) { + if (!ifaces[if_int_to_gl_index_[i]]->isRxBufferEmpty()) { + return true; + } + } + + return false; +} + +uavcan::int16_t CanDriver::select(uavcan::CanSelectMasks& inout_masks, + const uavcan::CanFrame* (& pending_tx)[uavcan::MaxCanIfaces], + const uavcan::MonotonicTime blocking_deadline) +{ + const uavcan::CanSelectMasks in_masks = inout_masks; + const uavcan::MonotonicTime time = clock::getMonotonic(); + + for (uavcan::uint8_t i = 0; i < num_ifaces_; i++) { + CanIface* iface = ifaces[if_int_to_gl_index_[i]]; + iface->discardTimedOutTxMailboxes(time); // Check TX timeouts - this may release some TX slots + { + CriticalSectionLocker cs_locker; + iface->pollErrorFlagsFromISR(); + } + } + + inout_masks = makeSelectMasks(pending_tx); // Check if we already have some of the requested events + if ((inout_masks.read & in_masks.read) != 0 || + (inout_masks.write & in_masks.write) != 0) + { + return 1; + } + + (void)update_event_.wait(blocking_deadline - time); // Block until timeout expires or any iface updates + inout_masks = makeSelectMasks(pending_tx); // Return what we got even if none of the requested events are set + return 1; // Return value doesn't matter as long as it is non-negative +} + + +#if UAVCAN_STM32_BAREMETAL || UAVCAN_STM32_FREERTOS + +static void nvicEnableVector(IRQn_Type irq, uint8_t prio) +{ + #if !defined (USE_HAL_DRIVER) + NVIC_InitTypeDef NVIC_InitStructure; + NVIC_InitStructure.NVIC_IRQChannel = irq; + NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = prio; + NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; + NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; + NVIC_Init(&NVIC_InitStructure); + #else + HAL_NVIC_SetPriority(irq, prio, 0); + HAL_NVIC_EnableIRQ(irq); + #endif +} + +#endif + +void CanDriver::initOnce() +{ + /* + * CAN1, CAN2 + */ + { + CriticalSectionLocker lock; + RCC->APB1ENR |= RCC_APB1ENR_CAN1EN; + RCC->APB1RSTR |= RCC_APB1RSTR_CAN1RST; + RCC->APB1RSTR &= ~RCC_APB1RSTR_CAN1RST; +# if UAVCAN_STM32_NUM_IFACES > 1 + RCC->APB1ENR |= RCC_APB1ENR_CAN2EN; + RCC->APB1RSTR |= RCC_APB1RSTR_CAN2RST; + RCC->APB1RSTR &= ~RCC_APB1RSTR_CAN2RST; +# endif + } + + /* + * IRQ + */ + { + CriticalSectionLocker lock; + nvicEnableVector(CAN1_TX_IRQn, UAVCAN_STM32_IRQ_PRIORITY_MASK); + nvicEnableVector(CAN1_RX0_IRQn, UAVCAN_STM32_IRQ_PRIORITY_MASK); + nvicEnableVector(CAN1_RX1_IRQn, UAVCAN_STM32_IRQ_PRIORITY_MASK); +# if UAVCAN_STM32_NUM_IFACES > 1 + nvicEnableVector(CAN2_TX_IRQn, UAVCAN_STM32_IRQ_PRIORITY_MASK); + nvicEnableVector(CAN2_RX0_IRQn, UAVCAN_STM32_IRQ_PRIORITY_MASK); + nvicEnableVector(CAN2_RX1_IRQn, UAVCAN_STM32_IRQ_PRIORITY_MASK); +# endif + } +} + +int CanDriver::init(const uavcan::uint32_t bitrate, const CanIface::OperatingMode mode) +{ + int res = 0; + + UAVCAN_STM32_LOG("Bitrate %lu mode %d", static_cast(bitrate), static_cast(mode)); + + static bool initialized_once = false; + if (!initialized_once) + { + initialized_once = true; + UAVCAN_STM32_LOG("First initialization"); + initOnce(); + } + + /* + * CAN1 + */ + UAVCAN_STM32_LOG("Initing iface 0..."); + ifaces[0] = &if0_; // This link must be initialized first, + res = if0_.init(bitrate, mode); // otherwise an IRQ may fire while the interface is not linked yet; + if (res < 0) // a typical race condition. + { + UAVCAN_STM32_LOG("Iface 0 init failed %i", res); + ifaces[0] = UAVCAN_NULLPTR; + goto fail; + } + + /* + * CAN2 + */ +#if UAVCAN_STM32_NUM_IFACES > 1 + UAVCAN_STM32_LOG("Initing iface 1..."); + ifaces[1] = &if1_; // Same thing here. + res = if1_.init(bitrate, mode); + if (res < 0) + { + UAVCAN_STM32_LOG("Iface 1 init failed %i", res); + ifaces[1] = UAVCAN_NULLPTR; + goto fail; + } +#endif + + UAVCAN_STM32_LOG("CAN drv init OK"); + UAVCAN_ASSERT(res >= 0); + return res; + +fail: + UAVCAN_STM32_LOG("CAN drv init failed %i", res); + UAVCAN_ASSERT(res < 0); + return res; +} + +void CanDriver::initOnce(uavcan::uint8_t can_number, bool enable_irqs) +{ + /* + * CAN1, CAN2 + */ + { + CriticalSectionLocker lock; + if (can_number == 0) { + RCC->APB1ENR |= RCC_APB1ENR_CAN1EN; + RCC->APB1RSTR |= RCC_APB1RSTR_CAN1RST; + RCC->APB1RSTR &= ~RCC_APB1RSTR_CAN1RST; + } +# if UAVCAN_STM32_NUM_IFACES > 1 + else if (can_number == 1) { + RCC->APB1ENR |= RCC_APB1ENR_CAN2EN; + RCC->APB1RSTR |= RCC_APB1RSTR_CAN2RST; + RCC->APB1RSTR &= ~RCC_APB1RSTR_CAN2RST; + } +# endif + } + + if (!enable_irqs) { + return; + } + /* + * IRQ + */ + { + CriticalSectionLocker lock; + if (can_number == 0) { + nvicEnableVector(CAN1_TX_IRQn, UAVCAN_STM32_IRQ_PRIORITY_MASK); + nvicEnableVector(CAN1_RX0_IRQn, UAVCAN_STM32_IRQ_PRIORITY_MASK); + nvicEnableVector(CAN1_RX1_IRQn, UAVCAN_STM32_IRQ_PRIORITY_MASK); + } +# if UAVCAN_STM32_NUM_IFACES > 1 + else if (can_number == 1) { + nvicEnableVector(CAN2_TX_IRQn, UAVCAN_STM32_IRQ_PRIORITY_MASK); + nvicEnableVector(CAN2_RX0_IRQn, UAVCAN_STM32_IRQ_PRIORITY_MASK); + nvicEnableVector(CAN2_RX1_IRQn, UAVCAN_STM32_IRQ_PRIORITY_MASK); + } +# endif + } +} + +int CanDriver::init(const uavcan::uint32_t bitrate, const CanIface::OperatingMode mode, uavcan::uint8_t can_number) +{ + int res = 0; + + UAVCAN_STM32_LOG("Bitrate %lu mode %d", static_cast(bitrate), static_cast(mode)); + if (can_number > UAVCAN_STM32_NUM_IFACES) { + res = -1; + goto fail; + } + static bool initialized_once[UAVCAN_STM32_NUM_IFACES] = {false}; + + if (!initialized_once[can_number]) { + initialized_once[can_number] = true; + initialized_by_me_[can_number] = true; + + if (can_number == 1 && !initialized_once[0]) { + UAVCAN_STM32_LOG("Iface 0 is not initialized yet but we need it for Iface 1, trying to init it"); + UAVCAN_STM32_LOG("Enabling CAN iface 0"); + initOnce(0, false); + UAVCAN_STM32_LOG("Initing iface 0..."); + res = if0_.init(bitrate, mode); + + if (res < 0) { + UAVCAN_STM32_LOG("Iface 0 init failed %i", res); + goto fail; + } + } + + UAVCAN_STM32_LOG("Enabling CAN iface %d", can_number); + initOnce(can_number, true); + } else if (!initialized_by_me_[can_number]) { + UAVCAN_STM32_LOG("CAN iface %d initialized in another CANDriver!", can_number); + res = -2; + goto fail; + } + + if (can_number == 0) { + /* + * CAN1 + */ + UAVCAN_STM32_LOG("Initing iface 0..."); + ifaces[0] = &if0_; // This link must be initialized first, + res = if0_.init(bitrate, mode); // otherwise an IRQ may fire while the interface is not linked yet; + if (res < 0) // a typical race condition. + { + UAVCAN_STM32_LOG("Iface 0 init failed %i", res); + ifaces[0] = UAVCAN_NULLPTR; + goto fail; + } + } else if (can_number == 1) { + /* + * CAN2 + */ + #if UAVCAN_STM32_NUM_IFACES > 1 + UAVCAN_STM32_LOG("Initing iface 1..."); + ifaces[1] = &if1_; // Same thing here. + res = if1_.init(bitrate, mode); + if (res < 0) + { + UAVCAN_STM32_LOG("Iface 1 init failed %i", res); + ifaces[1] = UAVCAN_NULLPTR; + goto fail; + } + #endif + } + + if_int_to_gl_index_[num_ifaces_++] = can_number; + + UAVCAN_STM32_LOG("CAN drv init OK"); + UAVCAN_ASSERT(res >= 0); + return res; + +fail: + UAVCAN_STM32_LOG("CAN drv init failed %i", res); + UAVCAN_ASSERT(res < 0); + return res; +} + +CanIface* CanDriver::getIface(uavcan::uint8_t iface_index) +{ + if (iface_index < num_ifaces_) + { + return ifaces[if_int_to_gl_index_[iface_index]]; + } + return UAVCAN_NULLPTR; +} + +bool CanDriver::hadActivity() +{ + for (uavcan::uint8_t i = 0; i < num_ifaces_; i++) { + if (ifaces[if_int_to_gl_index_[i]]->hadActivity()) { + return true; + } + } + + return false; +} + +} // namespace uavcan_stm32 + +/* + * Interrupt handlers + */ +extern "C" +{ + +UAVCAN_STM32_IRQ_HANDLER(CAN1_TX_IRQHandler); +UAVCAN_STM32_IRQ_HANDLER(CAN1_TX_IRQHandler) +{ + UAVCAN_STM32_IRQ_PROLOGUE(); + ChibiOS_CAN::handleTxInterrupt(0); + UAVCAN_STM32_IRQ_EPILOGUE(); +} + +UAVCAN_STM32_IRQ_HANDLER(CAN1_RX0_IRQHandler); +UAVCAN_STM32_IRQ_HANDLER(CAN1_RX0_IRQHandler) +{ + UAVCAN_STM32_IRQ_PROLOGUE(); + ChibiOS_CAN::handleRxInterrupt(0, 0); + UAVCAN_STM32_IRQ_EPILOGUE(); +} + +UAVCAN_STM32_IRQ_HANDLER(CAN1_RX1_IRQHandler); +UAVCAN_STM32_IRQ_HANDLER(CAN1_RX1_IRQHandler) +{ + UAVCAN_STM32_IRQ_PROLOGUE(); + ChibiOS_CAN::handleRxInterrupt(0, 1); + UAVCAN_STM32_IRQ_EPILOGUE(); +} + +# if UAVCAN_STM32_NUM_IFACES > 1 + +#if !defined(CAN2_TX_IRQHandler) ||\ + !defined(CAN2_RX0_IRQHandler) ||\ + !defined(CAN2_RX1_IRQHandler) +# error "Misconfigured build" +#endif + +UAVCAN_STM32_IRQ_HANDLER(CAN2_TX_IRQHandler); +UAVCAN_STM32_IRQ_HANDLER(CAN2_TX_IRQHandler) +{ + UAVCAN_STM32_IRQ_PROLOGUE(); + ChibiOS_CAN::handleTxInterrupt(1); + UAVCAN_STM32_IRQ_EPILOGUE(); +} + +UAVCAN_STM32_IRQ_HANDLER(CAN2_RX0_IRQHandler); +UAVCAN_STM32_IRQ_HANDLER(CAN2_RX0_IRQHandler) +{ + UAVCAN_STM32_IRQ_PROLOGUE(); + ChibiOS_CAN::handleRxInterrupt(1, 0); + UAVCAN_STM32_IRQ_EPILOGUE(); +} + +UAVCAN_STM32_IRQ_HANDLER(CAN2_RX1_IRQHandler); +UAVCAN_STM32_IRQ_HANDLER(CAN2_RX1_IRQHandler) +{ + UAVCAN_STM32_IRQ_PROLOGUE(); + ChibiOS_CAN::handleRxInterrupt(1, 1); + UAVCAN_STM32_IRQ_EPILOGUE(); +} + +# endif + +} // extern "C" diff --git a/libraries/AP_HAL_ChibiOS/bxcan.hpp b/libraries/AP_HAL_ChibiOS/bxcan.hpp new file mode 100644 index 0000000000..75830cc0a9 --- /dev/null +++ b/libraries/AP_HAL_ChibiOS/bxcan.hpp @@ -0,0 +1,324 @@ +/* + * The MIT License (MIT) + * + * Copyright (c) 2014 Pavel Kirienko + * + * Permission is hereby granted, free of charge, to any person obtaining a copy of + * this software and associated documentation files (the "Software"), to deal in + * the Software without restriction, including without limitation the rights to + * use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of + * the Software, and to permit persons to whom the Software is furnished to do so, + * subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS + * FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR + * COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER + * IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN + * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. + */ + +/* + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + * + * Modified for Ardupilot by Siddharth Bharat Purohit + */ + +#pragma once + +#include + +#include +#include + +#ifndef UAVCAN_CPP_VERSION +# error UAVCAN_CPP_VERSION +#endif + +#if UAVCAN_CPP_VERSION < UAVCAN_CPP11 +// #undef'ed at the end of this file +# define constexpr const +#endif + +namespace ChibiOS_CAN +{ +namespace bxcan +{ + +struct TxMailboxType +{ + volatile uint32_t TIR; + volatile uint32_t TDTR; + volatile uint32_t TDLR; + volatile uint32_t TDHR; +}; + +struct RxMailboxType +{ + volatile uint32_t RIR; + volatile uint32_t RDTR; + volatile uint32_t RDLR; + volatile uint32_t RDHR; +}; + +struct FilterRegisterType +{ + volatile uint32_t FR1; + volatile uint32_t FR2; +}; + +struct CanType +{ + volatile uint32_t MCR; /*!< CAN master control register, Address offset: 0x00 */ + volatile uint32_t MSR; /*!< CAN master status register, Address offset: 0x04 */ + volatile uint32_t TSR; /*!< CAN transmit status register, Address offset: 0x08 */ + volatile uint32_t RF0R; /*!< CAN receive FIFO 0 register, Address offset: 0x0C */ + volatile uint32_t RF1R; /*!< CAN receive FIFO 1 register, Address offset: 0x10 */ + volatile uint32_t IER; /*!< CAN interrupt enable register, Address offset: 0x14 */ + volatile uint32_t ESR; /*!< CAN error status register, Address offset: 0x18 */ + volatile uint32_t BTR; /*!< CAN bit timing register, Address offset: 0x1C */ + uint32_t RESERVED0[88]; /*!< Reserved, 0x020 - 0x17F */ + TxMailboxType TxMailbox[3]; /*!< CAN Tx MailBox, Address offset: 0x180 - 0x1AC */ + RxMailboxType RxMailbox[2]; /*!< CAN FIFO MailBox, Address offset: 0x1B0 - 0x1CC */ + uint32_t RESERVED1[12]; /*!< Reserved, 0x1D0 - 0x1FF */ + volatile uint32_t FMR; /*!< CAN filter master register, Address offset: 0x200 */ + volatile uint32_t FM1R; /*!< CAN filter mode register, Address offset: 0x204 */ + uint32_t RESERVED2; /*!< Reserved, 0x208 */ + volatile uint32_t FS1R; /*!< CAN filter scale register, Address offset: 0x20C */ + uint32_t RESERVED3; /*!< Reserved, 0x210 */ + volatile uint32_t FFA1R; /*!< CAN filter FIFO assignment register, Address offset: 0x214 */ + uint32_t RESERVED4; /*!< Reserved, 0x218 */ + volatile uint32_t FA1R; /*!< CAN filter activation register, Address offset: 0x21C */ + uint32_t RESERVED5[8]; /*!< Reserved, 0x220-0x23F */ + FilterRegisterType FilterRegister[28]; /*!< CAN Filter Register, Address offset: 0x240-0x31C */ +}; + +/** + * CANx register sets + */ +CanType* const Can[UAVCAN_STM32_NUM_IFACES] = +{ + reinterpret_cast(0x40006400) +#if UAVCAN_STM32_NUM_IFACES > 1 + , + reinterpret_cast(0x40006800) +#endif +}; + +/* CAN master control register */ + +constexpr unsigned long MCR_INRQ = (1U << 0); /* Bit 0: Initialization Request */ +constexpr unsigned long MCR_SLEEP = (1U << 1); /* Bit 1: Sleep Mode Request */ +constexpr unsigned long MCR_TXFP = (1U << 2); /* Bit 2: Transmit FIFO Priority */ +constexpr unsigned long MCR_RFLM = (1U << 3); /* Bit 3: Receive FIFO Locked Mode */ +constexpr unsigned long MCR_NART = (1U << 4); /* Bit 4: No Automatic Retransmission */ +constexpr unsigned long MCR_AWUM = (1U << 5); /* Bit 5: Automatic Wakeup Mode */ +constexpr unsigned long MCR_ABOM = (1U << 6); /* Bit 6: Automatic Bus-Off Management */ +constexpr unsigned long MCR_TTCM = (1U << 7); /* Bit 7: Time Triggered Communication Mode Enable */ +constexpr unsigned long MCR_RESET = (1U << 15);/* Bit 15: bxCAN software master reset */ +constexpr unsigned long MCR_DBF = (1U << 16);/* Bit 16: Debug freeze */ + +/* CAN master status register */ + +constexpr unsigned long MSR_INAK = (1U << 0); /* Bit 0: Initialization Acknowledge */ +constexpr unsigned long MSR_SLAK = (1U << 1); /* Bit 1: Sleep Acknowledge */ +constexpr unsigned long MSR_ERRI = (1U << 2); /* Bit 2: Error Interrupt */ +constexpr unsigned long MSR_WKUI = (1U << 3); /* Bit 3: Wakeup Interrupt */ +constexpr unsigned long MSR_SLAKI = (1U << 4); /* Bit 4: Sleep acknowledge interrupt */ +constexpr unsigned long MSR_TXM = (1U << 8); /* Bit 8: Transmit Mode */ +constexpr unsigned long MSR_RXM = (1U << 9); /* Bit 9: Receive Mode */ +constexpr unsigned long MSR_SAMP = (1U << 10);/* Bit 10: Last Sample Point */ +constexpr unsigned long MSR_RX = (1U << 11);/* Bit 11: CAN Rx Signal */ + +/* CAN transmit status register */ + +constexpr unsigned long TSR_RQCP0 = (1U << 0); /* Bit 0: Request Completed Mailbox 0 */ +constexpr unsigned long TSR_TXOK0 = (1U << 1); /* Bit 1 : Transmission OK of Mailbox 0 */ +constexpr unsigned long TSR_ALST0 = (1U << 2); /* Bit 2 : Arbitration Lost for Mailbox 0 */ +constexpr unsigned long TSR_TERR0 = (1U << 3); /* Bit 3 : Transmission Error of Mailbox 0 */ +constexpr unsigned long TSR_ABRQ0 = (1U << 7); /* Bit 7 : Abort Request for Mailbox 0 */ +constexpr unsigned long TSR_RQCP1 = (1U << 8); /* Bit 8 : Request Completed Mailbox 1 */ +constexpr unsigned long TSR_TXOK1 = (1U << 9); /* Bit 9 : Transmission OK of Mailbox 1 */ +constexpr unsigned long TSR_ALST1 = (1U << 10);/* Bit 10 : Arbitration Lost for Mailbox 1 */ +constexpr unsigned long TSR_TERR1 = (1U << 11);/* Bit 11 : Transmission Error of Mailbox 1 */ +constexpr unsigned long TSR_ABRQ1 = (1U << 15);/* Bit 15 : Abort Request for Mailbox 1 */ +constexpr unsigned long TSR_RQCP2 = (1U << 16);/* Bit 16 : Request Completed Mailbox 2 */ +constexpr unsigned long TSR_TXOK2 = (1U << 17);/* Bit 17 : Transmission OK of Mailbox 2 */ +constexpr unsigned long TSR_ALST2 = (1U << 18);/* Bit 18: Arbitration Lost for Mailbox 2 */ +constexpr unsigned long TSR_TERR2 = (1U << 19);/* Bit 19: Transmission Error of Mailbox 2 */ +constexpr unsigned long TSR_ABRQ2 = (1U << 23);/* Bit 23: Abort Request for Mailbox 2 */ +constexpr unsigned long TSR_CODE_SHIFT = (24U); /* Bits 25-24: Mailbox Code */ +constexpr unsigned long TSR_CODE_MASK = (3U << TSR_CODE_SHIFT); +constexpr unsigned long TSR_TME0 = (1U << 26);/* Bit 26: Transmit Mailbox 0 Empty */ +constexpr unsigned long TSR_TME1 = (1U << 27);/* Bit 27: Transmit Mailbox 1 Empty */ +constexpr unsigned long TSR_TME2 = (1U << 28);/* Bit 28: Transmit Mailbox 2 Empty */ +constexpr unsigned long TSR_LOW0 = (1U << 29);/* Bit 29: Lowest Priority Flag for Mailbox 0 */ +constexpr unsigned long TSR_LOW1 = (1U << 30);/* Bit 30: Lowest Priority Flag for Mailbox 1 */ +constexpr unsigned long TSR_LOW2 = (1U << 31);/* Bit 31: Lowest Priority Flag for Mailbox 2 */ + +/* CAN receive FIFO 0/1 registers */ + +constexpr unsigned long RFR_FMP_SHIFT = (0U); /* Bits 1-0: FIFO Message Pending */ +constexpr unsigned long RFR_FMP_MASK = (3U << RFR_FMP_SHIFT); +constexpr unsigned long RFR_FULL = (1U << 3); /* Bit 3: FIFO 0 Full */ +constexpr unsigned long RFR_FOVR = (1U << 4); /* Bit 4: FIFO 0 Overrun */ +constexpr unsigned long RFR_RFOM = (1U << 5); /* Bit 5: Release FIFO 0 Output Mailbox */ + +/* CAN interrupt enable register */ + +constexpr unsigned long IER_TMEIE = (1U << 0); /* Bit 0: Transmit Mailbox Empty Interrupt Enable */ +constexpr unsigned long IER_FMPIE0 = (1U << 1); /* Bit 1: FIFO Message Pending Interrupt Enable */ +constexpr unsigned long IER_FFIE0 = (1U << 2); /* Bit 2: FIFO Full Interrupt Enable */ +constexpr unsigned long IER_FOVIE0 = (1U << 3); /* Bit 3: FIFO Overrun Interrupt Enable */ +constexpr unsigned long IER_FMPIE1 = (1U << 4); /* Bit 4: FIFO Message Pending Interrupt Enable */ +constexpr unsigned long IER_FFIE1 = (1U << 5); /* Bit 5: FIFO Full Interrupt Enable */ +constexpr unsigned long IER_FOVIE1 = (1U << 6); /* Bit 6: FIFO Overrun Interrupt Enable */ +constexpr unsigned long IER_EWGIE = (1U << 8); /* Bit 8: Error Warning Interrupt Enable */ +constexpr unsigned long IER_EPVIE = (1U << 9); /* Bit 9: Error Passive Interrupt Enable */ +constexpr unsigned long IER_BOFIE = (1U << 10);/* Bit 10: Bus-Off Interrupt Enable */ +constexpr unsigned long IER_LECIE = (1U << 11);/* Bit 11: Last Error Code Interrupt Enable */ +constexpr unsigned long IER_ERRIE = (1U << 15);/* Bit 15: Error Interrupt Enable */ +constexpr unsigned long IER_WKUIE = (1U << 16);/* Bit 16: Wakeup Interrupt Enable */ +constexpr unsigned long IER_SLKIE = (1U << 17);/* Bit 17: Sleep Interrupt Enable */ + +/* CAN error status register */ + +constexpr unsigned long ESR_EWGF = (1U << 0); /* Bit 0: Error Warning Flag */ +constexpr unsigned long ESR_EPVF = (1U << 1); /* Bit 1: Error Passive Flag */ +constexpr unsigned long ESR_BOFF = (1U << 2); /* Bit 2: Bus-Off Flag */ +constexpr unsigned long ESR_LEC_SHIFT = (4U); /* Bits 6-4: Last Error Code */ +constexpr unsigned long ESR_LEC_MASK = (7U << ESR_LEC_SHIFT); +constexpr unsigned long ESR_NOERROR = (0U << ESR_LEC_SHIFT);/* 000: No Error */ +constexpr unsigned long ESR_STUFFERROR = (1U << ESR_LEC_SHIFT);/* 001: Stuff Error */ +constexpr unsigned long ESR_FORMERROR = (2U << ESR_LEC_SHIFT);/* 010: Form Error */ +constexpr unsigned long ESR_ACKERROR = (3U << ESR_LEC_SHIFT);/* 011: Acknowledgment Error */ +constexpr unsigned long ESR_BRECERROR = (4U << ESR_LEC_SHIFT);/* 100: Bit recessive Error */ +constexpr unsigned long ESR_BDOMERROR = (5U << ESR_LEC_SHIFT);/* 101: Bit dominant Error */ +constexpr unsigned long ESR_CRCERRPR = (6U << ESR_LEC_SHIFT);/* 110: CRC Error */ +constexpr unsigned long ESR_SWERROR = (7U << ESR_LEC_SHIFT);/* 111: Set by software */ +constexpr unsigned long ESR_TEC_SHIFT = (16U); /* Bits 23-16: LS byte of the 9-bit Transmit Error Counter */ +constexpr unsigned long ESR_TEC_MASK = (0xFFU << ESR_TEC_SHIFT); +constexpr unsigned long ESR_REC_SHIFT = (24U); /* Bits 31-24: Receive Error Counter */ +constexpr unsigned long ESR_REC_MASK = (0xFFU << ESR_REC_SHIFT); + +/* CAN bit timing register */ + +constexpr unsigned long BTR_BRP_SHIFT = (0U); /* Bits 9-0: Baud Rate Prescaler */ +constexpr unsigned long BTR_BRP_MASK = (0x03FFU << BTR_BRP_SHIFT); +constexpr unsigned long BTR_TS1_SHIFT = (16U); /* Bits 19-16: Time Segment 1 */ +constexpr unsigned long BTR_TS1_MASK = (0x0FU << BTR_TS1_SHIFT); +constexpr unsigned long BTR_TS2_SHIFT = (20U); /* Bits 22-20: Time Segment 2 */ +constexpr unsigned long BTR_TS2_MASK = (7U << BTR_TS2_SHIFT); +constexpr unsigned long BTR_SJW_SHIFT = (24U); /* Bits 25-24: Resynchronization Jump Width */ +constexpr unsigned long BTR_SJW_MASK = (3U << BTR_SJW_SHIFT); +constexpr unsigned long BTR_LBKM = (1U << 30);/* Bit 30: Loop Back Mode (Debug);*/ +constexpr unsigned long BTR_SILM = (1U << 31);/* Bit 31: Silent Mode (Debug);*/ + +constexpr unsigned long BTR_BRP_MAX = (1024U); /* Maximum BTR value (without decrement);*/ +constexpr unsigned long BTR_TSEG1_MAX = (16U); /* Maximum TSEG1 value (without decrement);*/ +constexpr unsigned long BTR_TSEG2_MAX = (8U); /* Maximum TSEG2 value (without decrement);*/ + +/* TX mailbox identifier register */ + +constexpr unsigned long TIR_TXRQ = (1U << 0); /* Bit 0: Transmit Mailbox Request */ +constexpr unsigned long TIR_RTR = (1U << 1); /* Bit 1: Remote Transmission Request */ +constexpr unsigned long TIR_IDE = (1U << 2); /* Bit 2: Identifier Extension */ +constexpr unsigned long TIR_EXID_SHIFT = (3U); /* Bit 3-31: Extended Identifier */ +constexpr unsigned long TIR_EXID_MASK = (0x1FFFFFFFU << TIR_EXID_SHIFT); +constexpr unsigned long TIR_STID_SHIFT = (21U); /* Bits 21-31: Standard Identifier */ +constexpr unsigned long TIR_STID_MASK = (0x07FFU << TIR_STID_SHIFT); + +/* Mailbox data length control and time stamp register */ + +constexpr unsigned long TDTR_DLC_SHIFT = (0U); /* Bits 3:0: Data Length Code */ +constexpr unsigned long TDTR_DLC_MASK = (0x0FU << TDTR_DLC_SHIFT); +constexpr unsigned long TDTR_TGT = (1U << 8); /* Bit 8: Transmit Global Time */ +constexpr unsigned long TDTR_TIME_SHIFT = (16U); /* Bits 31:16: Message Time Stamp */ +constexpr unsigned long TDTR_TIME_MASK = (0xFFFFU << TDTR_TIME_SHIFT); + +/* Mailbox data low register */ + +constexpr unsigned long TDLR_DATA0_SHIFT = (0U); /* Bits 7-0: Data Byte 0 */ +constexpr unsigned long TDLR_DATA0_MASK = (0xFFU << TDLR_DATA0_SHIFT); +constexpr unsigned long TDLR_DATA1_SHIFT = (8U); /* Bits 15-8: Data Byte 1 */ +constexpr unsigned long TDLR_DATA1_MASK = (0xFFU << TDLR_DATA1_SHIFT); +constexpr unsigned long TDLR_DATA2_SHIFT = (16U); /* Bits 23-16: Data Byte 2 */ +constexpr unsigned long TDLR_DATA2_MASK = (0xFFU << TDLR_DATA2_SHIFT); +constexpr unsigned long TDLR_DATA3_SHIFT = (24U); /* Bits 31-24: Data Byte 3 */ +constexpr unsigned long TDLR_DATA3_MASK = (0xFFU << TDLR_DATA3_SHIFT); + +/* Mailbox data high register */ + +constexpr unsigned long TDHR_DATA4_SHIFT = (0U); /* Bits 7-0: Data Byte 4 */ +constexpr unsigned long TDHR_DATA4_MASK = (0xFFU << TDHR_DATA4_SHIFT); +constexpr unsigned long TDHR_DATA5_SHIFT = (8U); /* Bits 15-8: Data Byte 5 */ +constexpr unsigned long TDHR_DATA5_MASK = (0xFFU << TDHR_DATA5_SHIFT); +constexpr unsigned long TDHR_DATA6_SHIFT = (16U); /* Bits 23-16: Data Byte 6 */ +constexpr unsigned long TDHR_DATA6_MASK = (0xFFU << TDHR_DATA6_SHIFT); +constexpr unsigned long TDHR_DATA7_SHIFT = (24U); /* Bits 31-24: Data Byte 7 */ +constexpr unsigned long TDHR_DATA7_MASK = (0xFFU << TDHR_DATA7_SHIFT); + +/* Rx FIFO mailbox identifier register */ + +constexpr unsigned long RIR_RTR = (1U << 1); /* Bit 1: Remote Transmission Request */ +constexpr unsigned long RIR_IDE = (1U << 2); /* Bit 2: Identifier Extension */ +constexpr unsigned long RIR_EXID_SHIFT = (3U); /* Bit 3-31: Extended Identifier */ +constexpr unsigned long RIR_EXID_MASK = (0x1FFFFFFFU << RIR_EXID_SHIFT); +constexpr unsigned long RIR_STID_SHIFT = (21U); /* Bits 21-31: Standard Identifier */ +constexpr unsigned long RIR_STID_MASK = (0x07FFU << RIR_STID_SHIFT); + +/* Receive FIFO mailbox data length control and time stamp register */ + +constexpr unsigned long RDTR_DLC_SHIFT = (0U); /* Bits 3:0: Data Length Code */ +constexpr unsigned long RDTR_DLC_MASK = (0x0FU << RDTR_DLC_SHIFT); +constexpr unsigned long RDTR_FM_SHIFT = (8U); /* Bits 15-8: Filter Match Index */ +constexpr unsigned long RDTR_FM_MASK = (0xFFU << RDTR_FM_SHIFT); +constexpr unsigned long RDTR_TIME_SHIFT = (16U); /* Bits 31:16: Message Time Stamp */ +constexpr unsigned long RDTR_TIME_MASK = (0xFFFFU << RDTR_TIME_SHIFT); + +/* Receive FIFO mailbox data low register */ + +constexpr unsigned long RDLR_DATA0_SHIFT = (0U); /* Bits 7-0: Data Byte 0 */ +constexpr unsigned long RDLR_DATA0_MASK = (0xFFU << RDLR_DATA0_SHIFT); +constexpr unsigned long RDLR_DATA1_SHIFT = (8U); /* Bits 15-8: Data Byte 1 */ +constexpr unsigned long RDLR_DATA1_MASK = (0xFFU << RDLR_DATA1_SHIFT); +constexpr unsigned long RDLR_DATA2_SHIFT = (16U); /* Bits 23-16: Data Byte 2 */ +constexpr unsigned long RDLR_DATA2_MASK = (0xFFU << RDLR_DATA2_SHIFT); +constexpr unsigned long RDLR_DATA3_SHIFT = (24U); /* Bits 31-24: Data Byte 3 */ +constexpr unsigned long RDLR_DATA3_MASK = (0xFFU << RDLR_DATA3_SHIFT); + +/* Receive FIFO mailbox data high register */ + +constexpr unsigned long RDHR_DATA4_SHIFT = (0U); /* Bits 7-0: Data Byte 4 */ +constexpr unsigned long RDHR_DATA4_MASK = (0xFFU << RDHR_DATA4_SHIFT); +constexpr unsigned long RDHR_DATA5_SHIFT = (8U); /* Bits 15-8: Data Byte 5 */ +constexpr unsigned long RDHR_DATA5_MASK = (0xFFU << RDHR_DATA5_SHIFT); +constexpr unsigned long RDHR_DATA6_SHIFT = (16U); /* Bits 23-16: Data Byte 6 */ +constexpr unsigned long RDHR_DATA6_MASK = (0xFFU << RDHR_DATA6_SHIFT); +constexpr unsigned long RDHR_DATA7_SHIFT = (24U); /* Bits 31-24: Data Byte 7 */ +constexpr unsigned long RDHR_DATA7_MASK = (0xFFU << RDHR_DATA7_SHIFT); + +/* CAN filter master register */ + +constexpr unsigned long FMR_FINIT = (1U << 0); /* Bit 0: Filter Init Mode */ + +} +} + +#if UAVCAN_CPP_VERSION < UAVCAN_CPP11 +# undef constexpr +#endif