Siddharth Purohit
6 years ago
committed by
Andrew Tridgell
8 changed files with 1649 additions and 8 deletions
File diff suppressed because it is too large
Load Diff
@ -0,0 +1,448 @@
@@ -0,0 +1,448 @@
|
||||
/*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
* |
||||
* Code by Siddharth Bharat Purohit |
||||
*/ |
||||
|
||||
#pragma once |
||||
|
||||
#include "AP_HAL_ChibiOS.h" |
||||
|
||||
#if HAL_WITH_UAVCAN |
||||
|
||||
#include "CANThread.h" |
||||
#include "fdcan.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; |
||||
static SLCANRouter *_slcan_router; |
||||
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 MessageRAM { |
||||
uavcan::uint32_t StandardFilterSA; |
||||
uavcan::uint32_t ExtendedFilterSA; |
||||
uavcan::uint32_t RxFIFO0SA; |
||||
uavcan::uint32_t RxFIFO1SA; |
||||
uavcan::uint32_t TxFIFOQSA; |
||||
uavcan::uint32_t EndAddress; |
||||
} MessageRam_; |
||||
|
||||
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 loopback; |
||||
bool abort_on_error; |
||||
uint8_t index; |
||||
TxItem() : |
||||
loopback(false), |
||||
abort_on_error(false) |
||||
{ } |
||||
}; |
||||
|
||||
enum { NumTxMailboxes = 32 }; |
||||
|
||||
static const uavcan::uint32_t TSR_ABRQx[NumTxMailboxes]; |
||||
static uint32_t FDCANMessageRAMOffset_; |
||||
RxQueue rx_queue_; |
||||
fdcan::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; |
||||
|
||||
void setupMessageRam(void); |
||||
|
||||
static uint32_t FDCAN2MessageRAMOffset_; |
||||
public: |
||||
enum { MaxRxQueueCapacity = 254 }; |
||||
|
||||
enum OperatingMode { |
||||
NormalMode, |
||||
SilentMode |
||||
}; |
||||
|
||||
CanIface(fdcan::CanType* can, BusEvent& update_event, uavcan::uint8_t self_index, |
||||
CanRxItem* rx_queue_buffer, uavcan::uint8_t rx_queue_capacity); |
||||
|
||||
/**
|
||||
* 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 handleTxCompleteInterrupt(uavcan::uint64_t utc_usec); |
||||
void handleRxInterrupt(uavcan::uint8_t fifo_index); |
||||
|
||||
bool readRxFIFO(uavcan::uint8_t fifo_index); |
||||
/**
|
||||
* 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); |
||||
} |
||||
|
||||
fdcan::CanType* can_reg(void) |
||||
{ |
||||
return can_; |
||||
} |
||||
}; |
||||
|
||||
/**
|
||||
* 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_; |
||||
static bool clock_init_; |
||||
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 <unsigned RxQueueCapacity> |
||||
CanDriver(CanRxItem(&rx_queue_storage)[UAVCAN_STM32_NUM_IFACES][RxQueueCapacity]) |
||||
: update_event_(*this) |
||||
, if0_(fdcan::Can[0], update_event_, 0, rx_queue_storage[0], RxQueueCapacity) |
||||
#if UAVCAN_STM32_NUM_IFACES > 1 |
||||
, if1_(fdcan::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 <unsigned RxQueueCapacity = 128> |
||||
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 <typename DelayCallable> |
||||
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" |
||||
|
||||
#endif //HAL_WITH_UAVCAN
|
||||
@ -0,0 +1,90 @@
@@ -0,0 +1,90 @@
|
||||
/*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
* |
||||
* Code by Siddharth Bharat Purohit |
||||
*/ |
||||
|
||||
#pragma once |
||||
|
||||
#include "AP_HAL_ChibiOS.h" |
||||
|
||||
#if HAL_WITH_UAVCAN |
||||
|
||||
#if defined(STM32H7XX) |
||||
|
||||
#include <uavcan/uavcan.hpp> |
||||
#include <stdint.h> |
||||
|
||||
#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 fdcan |
||||
{ |
||||
typedef FDCAN_GlobalTypeDef CanType; |
||||
|
||||
constexpr unsigned long IDE = (0x40000000U); // Identifier Extension
|
||||
constexpr unsigned long STID_MASK = (0x1FFC0000U); // Standard Identifier Mask
|
||||
constexpr unsigned long EXID_MASK = (0x1FFFFFFFU); // Extended Identifier Mask
|
||||
constexpr unsigned long RTR = (0x20000000U); // Remote Transmission Request
|
||||
constexpr unsigned long DLC_MASK = (0x000F0000U); // Data Length Code
|
||||
|
||||
/**
|
||||
* CANx register sets |
||||
*/ |
||||
CanType* const Can[UAVCAN_STM32_NUM_IFACES] = { |
||||
reinterpret_cast<CanType*>(FDCAN1_BASE) |
||||
#if UAVCAN_STM32_NUM_IFACES > 1 |
||||
, |
||||
reinterpret_cast<CanType*>(FDCAN2_BASE) |
||||
#endif |
||||
}; |
||||
} |
||||
} |
||||
|
||||
#if UAVCAN_CPP_VERSION < UAVCAN_CPP11 |
||||
# undef constexpr |
||||
#endif |
||||
#endif //defined(STM32H7XX)
|
||||
#endif //HAL_WITH_UAVCAN
|
||||
Loading…
Reference in new issue