Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-30 12:30:27 +08:00
Code Issues Actions 9 Packages Projects Releases Wiki Activity

uavcan drivers: update code style

Browse Source 
Pure refactoring, just running 'make format'.
This commit is contained in:
Beat Küng
2019-10-31 11:11:05 +01:00
parent 6049a95073
commit 6622f04feb
17 changed files with 3307 additions and 3429 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Tools/astyle/files_to_check_code_style.sh
-1
View File
@@ -11,7 +11,6 @@ exec find boards msg src platforms \
-path msg/templates/urtps -prune -o \ -path msg/templates/urtps -prune -o \
-path platforms/nuttx/NuttX -prune -o \ -path platforms/nuttx/NuttX -prune -o \
-path src/drivers/uavcan/libuavcan -prune -o \ -path src/drivers/uavcan/libuavcan -prune -o \
-path src/drivers/uavcan/uavcan_drivers -prune -o \
-path src/lib/DriverFramework -prune -o \ -path src/lib/DriverFramework -prune -o \
-path src/lib/ecl -prune -o \ -path src/lib/ecl -prune -o \
-path src/lib/matrix -prune -o \ -path src/lib/matrix -prune -o \
src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/can.hpp
+63 -67
View File
@@ -32,8 +32,7 @@ static const uavcan::int16_t ErrMcrSOFTRSTNotCleared = 1011; ///< MCR_SOFTRST
* RX queue item. * RX queue item.
* The application shall not use this directly. * The application shall not use this directly.
*/ */
struct CanRxItem struct CanRxItem {
{
uavcan::uint64_t utc_usec; uavcan::uint64_t utc_usec;
uavcan::CanFrame frame; uavcan::CanFrame frame;
uavcan::CanIOFlags flags; uavcan::CanIOFlags flags;
@@ -54,7 +53,7 @@ class CanIface : public uavcan::ICanIface
const uavcan::uint32_t FIFO_IFLAG1 = flexcan::CAN_FIFO_NE | flexcan::CAN_FIFO_WARN | flexcan::CAN_FIFO_OV; const uavcan::uint32_t FIFO_IFLAG1 = flexcan::CAN_FIFO_NE | flexcan::CAN_FIFO_WARN | flexcan::CAN_FIFO_OV;
class RxQueue class RxQueue
{ {
CanRxItem* const buf_; CanRxItem *const buf_;
const uavcan::uint8_t capacity_; const uavcan::uint8_t capacity_;
uavcan::uint8_t in_; uavcan::uint8_t in_;
uavcan::uint8_t out_; uavcan::uint8_t out_;
@@ -64,7 +63,7 @@ class CanIface : public uavcan::ICanIface
void registerOverflow(); void registerOverflow();
public: public:
RxQueue(CanRxItem* buf, uavcan::uint8_t capacity) RxQueue(CanRxItem *buf, uavcan::uint8_t capacity)
: buf_(buf), : buf_(buf),
capacity_(capacity), capacity_(capacity),
in_(0), in_(0),
@@ -74,21 +73,22 @@ class CanIface : public uavcan::ICanIface
{ {
} }
void push(const uavcan::CanFrame& frame, const uint64_t& utc_usec, uavcan::CanIOFlags flags); 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 pop(uavcan::CanFrame &out_frame, uavcan::uint64_t &out_utc_usec, uavcan::CanIOFlags &out_flags);
void reset(); void reset();
unsigned getLength() const { unsigned getLength() const
{
return len_; return len_;
} }
uavcan::uint32_t getOverflowCount() const { uavcan::uint32_t getOverflowCount() const
{
return overflow_cnt_; return overflow_cnt_;
} }
}; };
struct Timings struct Timings {
{
uavcan::uint8_t prescaler; uavcan::uint8_t prescaler;
uavcan::uint8_t rjw; uavcan::uint8_t rjw;
uavcan::uint8_t pseg1; uavcan::uint8_t pseg1;
@@ -105,8 +105,7 @@ class CanIface : public uavcan::ICanIface
} }
}; };
struct TxItem struct TxItem {
{
uavcan::MonotonicTime deadline; uavcan::MonotonicTime deadline;
uavcan::CanFrame frame; uavcan::CanFrame frame;
enum {free = 0, busy, abort } pending; enum {free = 0, busy, abort } pending;
@@ -127,29 +126,30 @@ class CanIface : public uavcan::ICanIface
uavcan::uint8_t MaxMB; uavcan::uint8_t MaxMB;
RxQueue rx_queue_; RxQueue rx_queue_;
flexcan::CanType* const can_; flexcan::CanType *const can_;
uavcan::uint64_t error_cnt_; uavcan::uint64_t error_cnt_;
uavcan::uint64_t fifo_warn_cnt_; uavcan::uint64_t fifo_warn_cnt_;
uavcan::uint32_t pending_aborts_; uavcan::uint32_t pending_aborts_;
uavcan::uint32_t served_aborts_cnt_; uavcan::uint32_t served_aborts_cnt_;
BusEvent& update_event_; BusEvent &update_event_;
TxItem pending_tx_[NumTxMesgBuffers]; TxItem pending_tx_[NumTxMesgBuffers];
uavcan::uint8_t peak_tx_mailbox_index_; uavcan::uint8_t peak_tx_mailbox_index_;
const uavcan::uint8_t self_index_; const uavcan::uint8_t self_index_;
bool had_activity_; bool had_activity_;
int computeTimings(uavcan::uint32_t target_bitrate, Timings& out_timings); int computeTimings(uavcan::uint32_t target_bitrate, Timings &out_timings);
virtual uavcan::int16_t send(const uavcan::CanFrame& frame, uavcan::MonotonicTime tx_deadline, virtual uavcan::int16_t send(const uavcan::CanFrame &frame, uavcan::MonotonicTime tx_deadline,
uavcan::CanIOFlags flags); uavcan::CanIOFlags flags);
virtual uavcan::int16_t receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic, virtual uavcan::int16_t receive(uavcan::CanFrame &out_frame, uavcan::MonotonicTime &out_ts_monotonic,
uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags); uavcan::UtcTime &out_ts_utc, uavcan::CanIOFlags &out_flags);
virtual uavcan::int16_t configureFilters(const uavcan::CanFilterConfig* filter_configs, virtual uavcan::int16_t configureFilters(const uavcan::CanFilterConfig *filter_configs,
uavcan::uint16_t num_configs); uavcan::uint16_t num_configs);
virtual uavcan::uint16_t getNumFilters() const { virtual uavcan::uint16_t getNumFilters() const
{
return NumFilters; return NumFilters;
} }
@@ -166,14 +166,13 @@ class CanIface : public uavcan::ICanIface
public: public:
enum { MaxRxQueueCapacity = 254 }; enum { MaxRxQueueCapacity = 254 };
enum OperatingMode enum OperatingMode {
{
NormalMode, NormalMode,
SilentMode SilentMode
}; };
CanIface(flexcan::CanType* can, BusEvent& update_event, uavcan::uint8_t self_index, CanIface(flexcan::CanType *can, BusEvent &update_event, uavcan::uint8_t self_index,
CanRxItem* rx_queue_buffer, uavcan::uint8_t rx_queue_capacity) CanRxItem *rx_queue_buffer, uavcan::uint8_t rx_queue_capacity)
: MaxMB(flexcan::HWMaxMB), : MaxMB(flexcan::HWMaxMB),
rx_queue_(rx_queue_buffer, rx_queue_capacity), rx_queue_(rx_queue_buffer, rx_queue_capacity),
can_(can), can_(can),
@@ -189,7 +188,7 @@ public:
UAVCAN_ASSERT(self_index_ < UAVCAN_KINETIS_NUM_IFACES); UAVCAN_ASSERT(self_index_ < UAVCAN_KINETIS_NUM_IFACES);
} }
/** /**
* Initializes the hardware CAN controller. * Initializes the hardware CAN controller.
* Assumes: * Assumes:
* - Iface clock is enabled * - Iface clock is enabled
@@ -201,7 +200,7 @@ public:
void handleTxInterrupt(uavcan::uint32_t tx_iflags, uavcan::uint64_t utc_usec); void handleTxInterrupt(uavcan::uint32_t tx_iflags, uavcan::uint64_t utc_usec);
void handleRxInterrupt(uavcan::uint32_t rx_iflags, uavcan::uint64_t utc_usec); void handleRxInterrupt(uavcan::uint32_t rx_iflags, uavcan::uint64_t utc_usec);
/** /**
* This method is used to count errors and abort transmission on error if necessary. * 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 * This functionality used to be implemented in the SCE interrupt handler, but that approach was
* generating too much processing overhead, especially on disconnected interfaces. * generating too much processing overhead, especially on disconnected interfaces.
@@ -212,50 +211,53 @@ public:
void discardTimedOutTxMailboxes(uavcan::MonotonicTime current_time); void discardTimedOutTxMailboxes(uavcan::MonotonicTime current_time);
bool canAcceptNewTxFrame(const uavcan::CanFrame& frame) const; bool canAcceptNewTxFrame(const uavcan::CanFrame &frame) const;
bool isRxBufferEmpty() const; bool isRxBufferEmpty() const;
/** /**
* Number of RX frames lost due to queue overflow. * Number of RX frames lost due to queue overflow.
* This is an atomic read, it doesn't require a critical section. * This is an atomic read, it doesn't require a critical section.
*/ */
uavcan::uint32_t getRxQueueOverflowCount() const { uavcan::uint32_t getRxQueueOverflowCount() const
{
return rx_queue_.getOverflowCount(); return rx_queue_.getOverflowCount();
} }
/** /**
* Total number of hardware failures and other kinds of errors (e.g. queue overruns). * 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. * May increase continuously if the interface is not connected to the bus.
*/ */
virtual uavcan::uint64_t getErrorCount() const; virtual uavcan::uint64_t getErrorCount() const;
/** /**
* Number of times the driver exercised library's requirement to abort transmission on first error. * 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. * This is an atomic read, it doesn't require a critical section.
* See @ref uavcan::CanIOFlagAbortOnError. * See @ref uavcan::CanIOFlagAbortOnError.
*/ */
uavcan::uint32_t getVoluntaryTxAbortCount() const { uavcan::uint32_t getVoluntaryTxAbortCount() const
{
return served_aborts_cnt_; return served_aborts_cnt_;
} }
/** /**
* Returns the number of frames pending in the RX queue. * Returns the number of frames pending in the RX queue.
* This is intended for debug use only. * This is intended for debug use only.
*/ */
unsigned getRxQueueLength() const; unsigned getRxQueueLength() const;
/** /**
* Whether this iface had at least one successful IO since the previous call of this method. * 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. * This is designed for use with iface activity LEDs.
*/ */
bool hadActivity(); bool hadActivity();
/** /**
* Peak number of TX mailboxes used concurrently since initialization. * Peak number of TX mailboxes used concurrently since initialization.
* Range is [1, 3]. * Range is [1, 3].
* Value of 3 suggests that priority inversion could be taking place. * Value of 3 suggests that priority inversion could be taking place.
*/ */
uavcan::uint8_t getPeakNumTxMailboxesUsed() const { uavcan::uint8_t getPeakNumTxMailboxesUsed() const
{
return uavcan::uint8_t(peak_tx_mailbox_index_ + 1); return uavcan::uint8_t(peak_tx_mailbox_index_ + 1);
} }
}; };
@@ -273,53 +275,54 @@ class CanDriver : public uavcan::ICanDriver
CanIface if1_; CanIface if1_;
#endif #endif
virtual uavcan::int16_t select(uavcan::CanSelectMasks& inout_masks, virtual uavcan::int16_t select(uavcan::CanSelectMasks &inout_masks,
const uavcan::CanFrame* (&pending_tx)[uavcan::MaxCanIfaces], const uavcan::CanFrame * (&pending_tx)[uavcan::MaxCanIfaces],
uavcan::MonotonicTime blocking_deadline); uavcan::MonotonicTime blocking_deadline);
static void initOnce(); static void initOnce();
public: public:
template <unsigned RxQueueCapacity> template <unsigned RxQueueCapacity>
CanDriver(CanRxItem (&rx_queue_storage)[UAVCAN_KINETIS_NUM_IFACES][RxQueueCapacity]) CanDriver(CanRxItem(&rx_queue_storage)[UAVCAN_KINETIS_NUM_IFACES][RxQueueCapacity])
: update_event_(*this), : update_event_(*this),
if0_(flexcan::Can[0], update_event_, 0, rx_queue_storage[0], RxQueueCapacity) if0_(flexcan::Can[0], update_event_, 0, rx_queue_storage[0], RxQueueCapacity)
#if UAVCAN_KINETIS_NUM_IFACES > 1 #if UAVCAN_KINETIS_NUM_IFACES > 1
, if1_(flexcan::Can[1], update_event_, 1, rx_queue_storage[1], RxQueueCapacity) , if1_(flexcan::Can[1], update_event_, 1, rx_queue_storage[1], RxQueueCapacity)
#endif #endif
{ {
uavcan::StaticAssert<(RxQueueCapacity <= CanIface::MaxRxQueueCapacity)>::check(); uavcan::StaticAssert < (RxQueueCapacity <= CanIface::MaxRxQueueCapacity) >::check();
} }
/** /**
* This function returns select masks indicating which interfaces are available for read/write. * This function returns select masks indicating which interfaces are available for read/write.
*/ */
uavcan::CanSelectMasks makeSelectMasks(const uavcan::CanFrame* (&pending_tx)[uavcan::MaxCanIfaces]) const; uavcan::CanSelectMasks makeSelectMasks(const uavcan::CanFrame * (&pending_tx)[uavcan::MaxCanIfaces]) const;
/** /**
* Whether there's at least one interface where receive() would return a frame. * Whether there's at least one interface where receive() would return a frame.
*/ */
bool hasReadableInterfaces() const; bool hasReadableInterfaces() const;
/** /**
* Returns zero if OK. * Returns zero if OK.
* Returns negative value if failed (e.g. invalid bitrate). * 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);
virtual CanIface* getIface(uavcan::uint8_t iface_index); virtual CanIface *getIface(uavcan::uint8_t iface_index);
virtual uavcan::uint8_t getNumIfaces() const { virtual uavcan::uint8_t getNumIfaces() const
{
return UAVCAN_KINETIS_NUM_IFACES; return UAVCAN_KINETIS_NUM_IFACES;
} }
/** /**
* Whether at least one iface had at least one successful IO since previous call of this method. * 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. * This is designed for use with iface activity LEDs.
*/ */
bool hadActivity(); bool hadActivity();
BusEvent& updateEvent() { return update_event_; } BusEvent &updateEvent() { return update_event_; }
}; };
/** /**
@@ -342,7 +345,7 @@ public:
{ {
} }
/** /**
* This overload simply configures the provided bitrate. * This overload simply configures the provided bitrate.
* Auto bit rate detection will not be performed. * Auto bit rate detection will not be performed.
* Bitrate value must be positive. * Bitrate value must be positive.
@@ -353,7 +356,7 @@ public:
return driver.init(bitrate, CanIface::NormalMode); return driver.init(bitrate, CanIface::NormalMode);
} }
/** /**
* This function can either initialize the driver at a fixed bit rate, or it can perform * 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. * automatic bit rate detection. For theory please refer to the CiA application note #801.
* *
@@ -368,36 +371,29 @@ public:
* @return Negative value on error; non-negative on success. Refer to constants Err*. * @return Negative value on error; non-negative on success. Refer to constants Err*.
*/ */
template <typename DelayCallable> template <typename DelayCallable>
int init(DelayCallable delay_callable, uavcan::uint32_t& inout_bitrate = BitRateAutoDetect) int init(DelayCallable delay_callable, uavcan::uint32_t &inout_bitrate = BitRateAutoDetect)
{
if (inout_bitrate > 0)
{ {
if (inout_bitrate > 0) {
return driver.init(inout_bitrate, CanIface::NormalMode); return driver.init(inout_bitrate, CanIface::NormalMode);
}
else } else {
{ static const uavcan::uint32_t StandardBitRates[] = {
static const uavcan::uint32_t StandardBitRates[] =
{
1000000, 1000000,
500000, 500000,
250000, 250000,
125000 125000
}; };
for (uavcan::uint8_t br = 0; br < sizeof(StandardBitRates) / sizeof(StandardBitRates[0]); br++) for (uavcan::uint8_t br = 0; br < sizeof(StandardBitRates) / sizeof(StandardBitRates[0]); br++) {
{
inout_bitrate = StandardBitRates[br]; inout_bitrate = StandardBitRates[br];
const int res = driver.init(inout_bitrate, CanIface::SilentMode); const int res = driver.init(inout_bitrate, CanIface::SilentMode);
delay_callable(); delay_callable();
if (res >= 0) if (res >= 0) {
{ for (uavcan::uint8_t iface = 0; iface < driver.getNumIfaces(); iface++) {
for (uavcan::uint8_t iface = 0; iface < driver.getNumIfaces(); iface++) if (!driver.getIface(iface)->isRxBufferEmpty()) {
{
if (!driver.getIface(iface)->isRxBufferEmpty())
{
// Re-initializing in normal mode // Re-initializing in normal mode
return driver.init(inout_bitrate, CanIface::NormalMode); return driver.init(inout_bitrate, CanIface::NormalMode);
} }
@@ -409,7 +405,7 @@ public:
} }
} }
/** /**
* Use this value for listening delay during automatic bit rate detection. * Use this value for listening delay during automatic bit rate detection.
*/ */
static uavcan::MonotonicDuration getRecommendedListeningDelay() static uavcan::MonotonicDuration getRecommendedListeningDelay()
src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/clock.hpp
+12 -9
View File
@@ -49,8 +49,7 @@ void adjustUtc(uavcan::UtcDuration adjustment);
/** /**
* UTC clock synchronization parameters * UTC clock synchronization parameters
*/ */
struct UtcSyncParams struct UtcSyncParams {
{
float offset_p; ///< PPM per one usec error float offset_p; ///< PPM per one usec error
float rate_i; ///< PPM per one PPM error for second float rate_i; ///< PPM per one PPM error for second
float rate_error_corner_freq; float rate_error_corner_freq;
@@ -96,7 +95,7 @@ bool isUtcLocked();
* Both functions are thread safe. * Both functions are thread safe.
*/ */
UtcSyncParams getUtcSyncParams(); UtcSyncParams getUtcSyncParams();
void setUtcSyncParams(const UtcSyncParams& params); void setUtcSyncParams(const UtcSyncParams &params);
} }
@@ -106,26 +105,30 @@ void setUtcSyncParams(const UtcSyncParams& params);
class SystemClock : public uavcan::ISystemClock class SystemClock : public uavcan::ISystemClock
, uavcan::Noncopyable , uavcan::Noncopyable
{ {
SystemClock() { SystemClock()
{
} }
virtual void adjustUtc(uavcan::UtcDuration adjustment) { virtual void adjustUtc(uavcan::UtcDuration adjustment)
{
clock::adjustUtc(adjustment); clock::adjustUtc(adjustment);
} }
public: public:
virtual uavcan::MonotonicTime getMonotonic() const { virtual uavcan::MonotonicTime getMonotonic() const
{
return clock::getMonotonic(); return clock::getMonotonic();
} }
virtual uavcan::UtcTime getUtc() const { virtual uavcan::UtcTime getUtc() const
{
return clock::getUtc(); return clock::getUtc();
} }
/** /**
* Calls clock::init() as needed. * Calls clock::init() as needed.
* This function is thread safe. * This function is thread safe.
*/ */
static SystemClock& instance(); static SystemClock &instance();
}; };
} }
src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/flexcan.hpp
+33 -53
View File
@@ -26,11 +26,9 @@ namespace flexcan
{ {
enum { HWMaxMB = 16 }; enum { HWMaxMB = 16 };
union MBcsType union MBcsType {
{
volatile uint32_t w; volatile uint32_t w;
struct struct {
{
volatile uint32_t time_stamp : 16; volatile uint32_t time_stamp : 16;
volatile uint32_t dlc : 4; volatile uint32_t dlc : 4;
volatile uint32_t rtr : 1; volatile uint32_t rtr : 1;
@@ -40,13 +38,11 @@ union MBcsType
volatile uint32_t code : 4; volatile uint32_t code : 4;
volatile uint32_t res2 : 4; volatile uint32_t res2 : 4;
}; };
}; };
union FIFOcsType union FIFOcsType {
{
volatile uint32_t cs; volatile uint32_t cs;
struct struct {
{
volatile uint32_t time_stamp : 16; volatile uint32_t time_stamp : 16;
volatile uint32_t dlc : 4; volatile uint32_t dlc : 4;
volatile uint32_t rtr : 1; volatile uint32_t rtr : 1;
@@ -54,51 +50,42 @@ union FIFOcsType
volatile uint32_t srr : 1; volatile uint32_t srr : 1;
volatile uint32_t res : 9; volatile uint32_t res : 9;
}; };
}; };
union IDType union IDType {
{
volatile uint32_t w; volatile uint32_t w;
struct struct {
{
volatile uint32_t ext : 29; volatile uint32_t ext : 29;
volatile uint32_t resex : 3; volatile uint32_t resex : 3;
}; };
struct struct {
{
volatile uint32_t res : 18; volatile uint32_t res : 18;
volatile uint32_t std : 11; volatile uint32_t std : 11;
volatile uint32_t resstd : 3; volatile uint32_t resstd : 3;
}; };
}; };
union FilterType union FilterType {
{
volatile uint32_t w; volatile uint32_t w;
struct struct {
{
volatile uint32_t res : 1; // Bit 0 - Reserved volatile uint32_t res : 1; // Bit 0 - Reserved
volatile uint32_t ext : 29; // Bits 1 - 29 EID volatile uint32_t ext : 29; // Bits 1 - 29 EID
}; };
struct struct {
{
volatile uint32_t ress : 19; // Bits 0, 1-18 Reserved volatile uint32_t ress : 19; // Bits 0, 1-18 Reserved
volatile uint32_t std : 11; // StD ID volatile uint32_t std : 11; // StD ID
}; };
struct struct {
{
volatile uint32_t resc : 30; // Bits 0 - 29 Reserved volatile uint32_t resc : 30; // Bits 0 - 29 Reserved
volatile uint32_t ide : 1; // Bit 30 - EID volatile uint32_t ide : 1; // Bit 30 - EID
volatile uint32_t rtr : 1; // Bit 31 Remote volatile uint32_t rtr : 1; // Bit 31 Remote
}; };
}; };
union DataType union DataType {
{
volatile uint32_t l; volatile uint32_t l;
volatile uint32_t h; volatile uint32_t h;
struct struct {
{
volatile uint32_t b3 : 8; volatile uint32_t b3 : 8;
volatile uint32_t b2 : 8; volatile uint32_t b2 : 8;
volatile uint32_t b1 : 8; volatile uint32_t b1 : 8;
@@ -108,18 +95,16 @@ union DataType
volatile uint32_t b5 : 8; volatile uint32_t b5 : 8;
volatile uint32_t b4 : 8; volatile uint32_t b4 : 8;
}; };
}; };
struct MessageBufferType struct MessageBufferType {
{
MBcsType CS; MBcsType CS;
IDType ID; IDType ID;
DataType data; DataType data;
}; };
enum mb_code_tx enum mb_code_tx {
{
TxMbInactive = 0x8, /*!< MB is not active.*/ TxMbInactive = 0x8, /*!< MB is not active.*/
TxMbAbort = 0x9, /*!< MB is aborted.*/ TxMbAbort = 0x9, /*!< MB is aborted.*/
TxMbDataOrRemote = 0xC, /*!< MB is a TX Data Frame(when MB RTR = 0) or */ TxMbDataOrRemote = 0xC, /*!< MB is a TX Data Frame(when MB RTR = 0) or */
@@ -129,15 +114,13 @@ enum mb_code_tx
TxMbNotUsed = 0xF, /*!< Not used.*/ TxMbNotUsed = 0xF, /*!< Not used.*/
}; };
struct RxFiFoType struct RxFiFoType {
{
FIFOcsType CS; FIFOcsType CS;
IDType ID; IDType ID;
DataType data; DataType data;
}; };
enum mb_code_rx enum mb_code_rx {
{
kRxMbInactive = 0x0, /*!< MB is not active.*/ kRxMbInactive = 0x0, /*!< MB is not active.*/
kRxMbFull = 0x2, /*!< MB is full.*/ kRxMbFull = 0x2, /*!< MB is full.*/
kRxMbEmpty = 0x4, /*!< MB is active and empty.*/ kRxMbEmpty = 0x4, /*!< MB is active and empty.*/
@@ -149,8 +132,7 @@ enum mb_code_rx
kRxMbNotUsed = 0xF, /*!< Not used.*/ kRxMbNotUsed = 0xF, /*!< Not used.*/
}; };
struct CanType struct CanType {
{
volatile uint32_t MCR; /*!< Module Configuration Register, Address offset: 0x0000 */ volatile uint32_t MCR; /*!< Module Configuration Register, Address offset: 0x0000 */
volatile uint32_t CTRL1; /*!< Control 1 Register, Address offset: 0x0004 */ volatile uint32_t CTRL1; /*!< Control 1 Register, Address offset: 0x0004 */
volatile uint32_t TIMER; /*!< Free Running Timer, Address offset: 0x0008 */ volatile uint32_t TIMER; /*!< Free Running Timer, Address offset: 0x0008 */
@@ -172,8 +154,7 @@ struct CanType
volatile uint32_t RXFGMASK; /*!< Rx FIFO Global Mask Register, Address offset: 0x0048 */ volatile uint32_t RXFGMASK; /*!< Rx FIFO Global Mask Register, Address offset: 0x0048 */
volatile uint32_t RXFIR; /*!< Rx FIFO Information Register, Address offset: 0x004c */ volatile uint32_t RXFIR; /*!< Rx FIFO Information Register, Address offset: 0x004c */
uint32_t RESERVED5[12]; /*!< Reserved Address offset: 0x0050 */ uint32_t RESERVED5[12]; /*!< Reserved Address offset: 0x0050 */
union union {
{
RxFiFoType fifo; RxFiFoType fifo;
MessageBufferType mb; MessageBufferType mb;
} MB[HWMaxMB]; } MB[HWMaxMB];
@@ -214,12 +195,11 @@ enum { TXMBMask = (0b111111 << NumMBinFiFoAndFilters) };
/* Address of the CAN registers */ /* Address of the CAN registers */
CanType* const Can[UAVCAN_KINETIS_NUM_IFACES] = CanType *const Can[UAVCAN_KINETIS_NUM_IFACES] = {
{ reinterpret_cast<CanType *>(0x40024000) // CAN0
reinterpret_cast<CanType*>(0x40024000) // CAN0
#if UAVCAN_KINETIS_NUM_IFACES > 1 #if UAVCAN_KINETIS_NUM_IFACES > 1
, ,
reinterpret_cast<CanType*>(0x400A4000) // CAN1 reinterpret_cast<CanType *>(0x400A4000) // CAN1
#endif #endif
}; };
@@ -237,10 +217,10 @@ constexpr unsigned long MCR_IDAM_FMTD(3U << MCR_IDAM_SHIFT); /* Format
/* Bits 10-11: Reserved */ /* Bits 10-11: Reserved */
constexpr unsigned long MCR_AEN = (1U << 12); /* Bit 12: Abort Enable */ constexpr unsigned long MCR_AEN = (1U << 12); /* Bit 12: Abort Enable */
constexpr unsigned long MCR_LPRIOEN = (1U << 13); /* Bit 13: Local Priority Enable */ constexpr unsigned long MCR_LPRIOEN = (1U << 13); /* Bit 13: Local Priority Enable */
/* Bits 14-15: Reserved */ /* Bits 14-15: Reserved */
constexpr unsigned long MCR_IRMQ = (1U << 16); /* Bit 16: Individual Rx Masking and Queue Enable */ constexpr unsigned long MCR_IRMQ = (1U << 16); /* Bit 16: Individual Rx Masking and Queue Enable */
constexpr unsigned long MCR_SRXDIS = (1U << 17); /* Bit 17: Self Reception Disable */ constexpr unsigned long MCR_SRXDIS = (1U << 17); /* Bit 17: Self Reception Disable */
/* Bit 18: Reserved */ /* Bit 18: Reserved */
constexpr unsigned long MCR_WAKSRC = (1U << 19); /* Bit 19: Wake up Source */ constexpr unsigned long MCR_WAKSRC = (1U << 19); /* Bit 19: Wake up Source */
constexpr unsigned long MCR_LPMACK = (1U << 20); /* Bit 20: Low Power Mode Acknowledge */ constexpr unsigned long MCR_LPMACK = (1U << 20); /* Bit 20: Low Power Mode Acknowledge */
constexpr unsigned long MCR_WRNEN = (1U << 21); /* Bit 21: Warning Interrupt Enable */ constexpr unsigned long MCR_WRNEN = (1U << 21); /* Bit 21: Warning Interrupt Enable */
@@ -264,7 +244,7 @@ constexpr unsigned long CTRL1_LBUF = (1U << 4); /* Bit 4: Lowest Buff
constexpr unsigned long CTRL1_TSYN = (1U << 5); /* Bit 5: Timer Sync */ constexpr unsigned long CTRL1_TSYN = (1U << 5); /* Bit 5: Timer Sync */
constexpr unsigned long CTRL1_BOFFREC = (1U << 6); /* Bit 6: Bus Off Recovery */ constexpr unsigned long CTRL1_BOFFREC = (1U << 6); /* Bit 6: Bus Off Recovery */
constexpr unsigned long CTRL1_SMP = (1U << 7); /* Bit 7: CAN Bit Sampling */ constexpr unsigned long CTRL1_SMP = (1U << 7); /* Bit 7: CAN Bit Sampling */
/* Bits 8-9: Reserved */ /* Bits 8-9: Reserved */
constexpr unsigned long CTRL1_RWRNMSK = (1U << 10); /* Bit 10: Rx Warning Interrupt Mask */ constexpr unsigned long CTRL1_RWRNMSK = (1U << 10); /* Bit 10: Rx Warning Interrupt Mask */
constexpr unsigned long CTRL1_TWRNMSK = (1U << 11); /* Bit 11: Tx Warning Interrupt Mask */ constexpr unsigned long CTRL1_TWRNMSK = (1U << 11); /* Bit 11: Tx Warning Interrupt Mask */
constexpr unsigned long CTRL1_LPB = (1U << 12); /* Bit 12: Loop Back Mode */ constexpr unsigned long CTRL1_LPB = (1U << 12); /* Bit 12: Loop Back Mode */
@@ -422,7 +402,7 @@ constexpr unsigned long ESR1_BIT1ERR = (1U << 15); /* Bit 15: Bit1 Err
constexpr unsigned long ESR1_RWRNINT = (1U << 16); /* Bit 16: Rx Warning Interrupt Flag */ constexpr unsigned long ESR1_RWRNINT = (1U << 16); /* Bit 16: Rx Warning Interrupt Flag */
constexpr unsigned long ESR1_TWRNINT = (1U << 17); /* Bit 17: Tx Warning Interrupt Flag */ constexpr unsigned long ESR1_TWRNINT = (1U << 17); /* Bit 17: Tx Warning Interrupt Flag */
constexpr unsigned long ESR1_SYNCH = (1U << 18); /* Bit 18: CAN Synchronization Status */ constexpr unsigned long ESR1_SYNCH = (1U << 18); /* Bit 18: CAN Synchronization Status */
/* Bits 19-31: Reserved */ /* Bits 19-31: Reserved */
/* Interrupt Masks 2 Register */ /* Interrupt Masks 2 Register */
constexpr unsigned long CAN_IMASK2_0 = (1U << 0); /* Bit 0: Buffer MB0 Mask */ constexpr unsigned long CAN_IMASK2_0 = (1U << 0); /* Bit 0: Buffer MB0 Mask */
@@ -593,13 +573,13 @@ constexpr unsigned long CTRL2_RFFN_112MB(13U << CTRL2_RFFN_SHIFT);
constexpr unsigned long CTRL2_RFFN_120MB(14U << CTRL2_RFFN_SHIFT); constexpr unsigned long CTRL2_RFFN_120MB(14U << CTRL2_RFFN_SHIFT);
constexpr unsigned long CTRL2_RFFN_128MB(15U << CTRL2_RFFN_SHIFT); constexpr unsigned long CTRL2_RFFN_128MB(15U << CTRL2_RFFN_SHIFT);
constexpr unsigned long CTRL2_WRMFRZ = (1U << 28U); /* Bit 28: Write-Access to Memory in Freeze mode */ constexpr unsigned long CTRL2_WRMFRZ = (1U << 28U); /* Bit 28: Write-Access to Memory in Freeze mode */
/* Bits 29-31: Reserved */ /* Bits 29-31: Reserved */
/* Error and Status 2 Register */ /* Error and Status 2 Register */
/* Bits 0-12: Reserved */ /* Bits 0-12: Reserved */
constexpr unsigned long ESR2_IMB = (1U << 13); /* Bit 13: Inactive Mailbox */ constexpr unsigned long ESR2_IMB = (1U << 13); /* Bit 13: Inactive Mailbox */
constexpr unsigned long ESR2_VPS = (1U << 14); /* Bit 14: Valid Priority Status */ constexpr unsigned long ESR2_VPS = (1U << 14); /* Bit 14: Valid Priority Status */
/* Bit 15: Reserved */ /* Bit 15: Reserved */
constexpr unsigned long ESR2_LPTM_SHIFT = (16); /* Bits 16-22: Lowest Priority Tx Mailbox */ constexpr unsigned long ESR2_LPTM_SHIFT = (16); /* Bits 16-22: Lowest Priority Tx Mailbox */
constexpr unsigned long ESR2_LPTM_MASK = (0x7fU << ESR2_LPTM_SHIFT); constexpr unsigned long ESR2_LPTM_MASK = (0x7fU << ESR2_LPTM_SHIFT);
/* Bits 23-31: Reserved */ /* Bits 23-31: Reserved */
src/drivers/uavcan/uavcan_drivers/kinetis/driver/include/uavcan_kinetis/thread.hpp
+3 -3
View File
@@ -34,7 +34,7 @@ class BusEvent : uavcan::Noncopyable
sem_t sem_; sem_t sem_;
public: public:
BusEvent(CanDriver& can_driver); BusEvent(CanDriver &can_driver);
~BusEvent(); ~BusEvent();
void registerSignalCallback(SignalCallbackHandler handler) { signal_cb_ = handler; } void registerSignalCallback(SignalCallbackHandler handler) { signal_cb_ = handler; }
@@ -78,10 +78,10 @@ public:
class MutexLocker class MutexLocker
{ {
Mutex& mutex_; Mutex &mutex_;
public: public:
MutexLocker(Mutex& mutex) MutexLocker(Mutex &mutex)
: mutex_(mutex) : mutex_(mutex)
{ {
mutex_.lock(); mutex_.lock();
src/drivers/uavcan/uavcan_drivers/kinetis/driver/src/internal.hpp
+1 -2
View File
@@ -48,8 +48,7 @@ namespace uavcan_kinetis
{ {
#if UAVCAN_KINETIS_NUTTX #if UAVCAN_KINETIS_NUTTX
struct CriticalSectionLocker struct CriticalSectionLocker {
{
const irqstate_t flags_; const irqstate_t flags_;
CriticalSectionLocker() CriticalSectionLocker()
src/drivers/uavcan/uavcan_drivers/kinetis/driver/src/uc_kinetis_clock.cpp
+35 -44
View File
@@ -67,10 +67,11 @@ uavcan::uint64_t time_utc = 0;
void init() void init()
{ {
CriticalSectionLocker lock; CriticalSectionLocker lock;
if (initialized)
{ if (initialized) {
return; return;
} }
initialized = true; initialized = true;
// Attach IRQ // Attach IRQ
@@ -125,13 +126,13 @@ static uavcan::uint64_t sampleUtcFromCriticalSection()
volatile uavcan::uint64_t time = time_utc; volatile uavcan::uint64_t time = time_utc;
volatile uavcan::uint32_t cnt = CountsPerPeriod - getreg32(TMR_REG(KINETIS_PIT_CVAL_OFFSET)); volatile uavcan::uint32_t cnt = CountsPerPeriod - getreg32(TMR_REG(KINETIS_PIT_CVAL_OFFSET));
if (getreg32(TMR_REG(KINETIS_PIT_TFLG_OFFSET)) & PIT_TFLG_TIF) if (getreg32(TMR_REG(KINETIS_PIT_TFLG_OFFSET)) & PIT_TFLG_TIF) {
{
cnt = CountsPerPeriod - getreg32(TMR_REG(KINETIS_PIT_CVAL_OFFSET)); cnt = CountsPerPeriod - getreg32(TMR_REG(KINETIS_PIT_CVAL_OFFSET));
const uavcan::int32_t add = uavcan::int32_t(USecPerOverflow) + const uavcan::int32_t add = uavcan::int32_t(USecPerOverflow) +
(utc_accumulated_correction_nsec + utc_correction_nsec_per_overflow) / 1000; (utc_accumulated_correction_nsec + utc_correction_nsec_per_overflow) / 1000;
time = uavcan::uint64_t(uavcan::int64_t(time) + add); time = uavcan::uint64_t(uavcan::int64_t(time) + add);
} }
return time + (cnt / CountsPerUs); return time + (cnt / CountsPerUs);
} }
@@ -150,11 +151,11 @@ uavcan::MonotonicTime getMonotonic()
volatile uavcan::uint64_t time = time_mono; volatile uavcan::uint64_t time = time_mono;
volatile uavcan::uint32_t cnt = CountsPerPeriod - getreg32(TMR_REG(KINETIS_PIT_CVAL_OFFSET)); volatile uavcan::uint32_t cnt = CountsPerPeriod - getreg32(TMR_REG(KINETIS_PIT_CVAL_OFFSET));
if (getreg32(TMR_REG(KINETIS_PIT_TFLG_OFFSET)) & PIT_TFLG_TIF) if (getreg32(TMR_REG(KINETIS_PIT_TFLG_OFFSET)) & PIT_TFLG_TIF) {
{
cnt = CountsPerPeriod - getreg32(TMR_REG(KINETIS_PIT_CVAL_OFFSET)); cnt = CountsPerPeriod - getreg32(TMR_REG(KINETIS_PIT_CVAL_OFFSET));
time += USecPerOverflow; time += USecPerOverflow;
} }
usec = time + (cnt / CountsPerUs); usec = time + (cnt / CountsPerUs);
} // End Scope Critical section } // End Scope Critical section
@@ -165,8 +166,7 @@ uavcan::MonotonicTime getMonotonic()
uavcan::UtcTime getUtc() uavcan::UtcTime getUtc()
{ {
if (utc_set) if (utc_set) {
{
uavcan::uint64_t usec = 0; uavcan::uint64_t usec = 0;
{ {
CriticalSectionLocker locker; CriticalSectionLocker locker;
@@ -174,6 +174,7 @@ uavcan::UtcTime getUtc()
} }
return uavcan::UtcTime::fromUSec(usec); return uavcan::UtcTime::fromUSec(usec);
} }
return uavcan::UtcTime(); return uavcan::UtcTime();
} }
@@ -206,12 +207,10 @@ static void updateRatePID(uavcan::UtcDuration adjustment)
const float rel_rate_error = target_rel_rate_ppm - utc_rel_rate_ppm; const float rel_rate_error = target_rel_rate_ppm - utc_rel_rate_ppm;
if (dt > 10) if (dt > 10) {
{
utc_rel_rate_error_integral = 0; utc_rel_rate_error_integral = 0;
}
else } else {
{
utc_rel_rate_error_integral += rel_rate_error * dt * utc_sync_params.rate_i; utc_rel_rate_error_integral += rel_rate_error * dt * utc_sync_params.rate_i;
utc_rel_rate_error_integral = utc_rel_rate_error_integral =
uavcan::max(utc_rel_rate_error_integral, -utc_sync_params.max_rate_correction_ppm); uavcan::max(utc_rel_rate_error_integral, -utc_sync_params.max_rate_correction_ppm);
@@ -238,18 +237,16 @@ void adjustUtc(uavcan::UtcDuration adjustment)
MutexLocker mlocker(mutex); MutexLocker mlocker(mutex);
UAVCAN_ASSERT(initialized); UAVCAN_ASSERT(initialized);
if (adjustment.getAbs() > utc_sync_params.min_jump || !utc_set) if (adjustment.getAbs() > utc_sync_params.min_jump || !utc_set) {
{
const uavcan::int64_t adj_usec = adjustment.toUSec(); const uavcan::int64_t adj_usec = adjustment.toUSec();
{ {
CriticalSectionLocker locker; CriticalSectionLocker locker;
if ((adj_usec < 0) && uavcan::uint64_t(-adj_usec) > time_utc)
{ if ((adj_usec < 0) && uavcan::uint64_t(-adj_usec) > time_utc) {
time_utc = 1; time_utc = 1;
}
else } else {
{
time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + adj_usec); time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + adj_usec);
} }
} }
@@ -259,13 +256,11 @@ void adjustUtc(uavcan::UtcDuration adjustment)
utc_jump_cnt++; utc_jump_cnt++;
utc_prev_adj = 0; utc_prev_adj = 0;
utc_rel_rate_ppm = 0; utc_rel_rate_ppm = 0;
}
else } else {
{
updateRatePID(adjustment); updateRatePID(adjustment);
if (!utc_locked) if (!utc_locked) {
{
utc_locked = utc_locked =
(std::abs(utc_rel_rate_ppm) < utc_sync_params.lock_thres_rate_ppm) && (std::abs(utc_rel_rate_ppm) < utc_sync_params.lock_thres_rate_ppm) &&
(std::abs(utc_prev_adj) < float(utc_sync_params.lock_thres_offset.toUSec())); (std::abs(utc_prev_adj) < float(utc_sync_params.lock_thres_offset.toUSec()));
@@ -298,7 +293,7 @@ UtcSyncParams getUtcSyncParams()
return utc_sync_params; return utc_sync_params;
} }
void setUtcSyncParams(const UtcSyncParams& params) void setUtcSyncParams(const UtcSyncParams &params)
{ {
MutexLocker mlocker(mutex); MutexLocker mlocker(mutex);
// Add some sanity check // Add some sanity check
@@ -307,23 +302,22 @@ void setUtcSyncParams(const UtcSyncParams& params)
} // namespace clock } // namespace clock
SystemClock& SystemClock::instance() SystemClock &SystemClock::instance()
{ {
static union SystemClockStorage static union SystemClockStorage {
{
uavcan::uint8_t buffer[sizeof(SystemClock)]; uavcan::uint8_t buffer[sizeof(SystemClock)];
long long _aligner_1; long long _aligner_1;
long double _aligner_2; long double _aligner_2;
} storage; } storage;
SystemClock* const ptr = reinterpret_cast<SystemClock*>(storage.buffer); SystemClock *const ptr = reinterpret_cast<SystemClock *>(storage.buffer);
if (!clock::initialized) if (!clock::initialized) {
{
MutexLocker mlocker(clock::mutex); MutexLocker mlocker(clock::mutex);
clock::init(); clock::init();
new (ptr)SystemClock(); new (ptr)SystemClock();
} }
return *ptr; return *ptr;
} }
@@ -344,30 +338,27 @@ UAVCAN_KINETIS_IRQ_HANDLER(TIMX_IRQHandler)
time_mono += USecPerOverflow; time_mono += USecPerOverflow;
if (utc_set) if (utc_set) {
{
time_utc += USecPerOverflow; time_utc += USecPerOverflow;
utc_accumulated_correction_nsec += utc_correction_nsec_per_overflow; utc_accumulated_correction_nsec += utc_correction_nsec_per_overflow;
if (std::abs(utc_accumulated_correction_nsec) >= 1000)
{ if (std::abs(utc_accumulated_correction_nsec) >= 1000) {
time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + 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; utc_accumulated_correction_nsec %= 1000;
} }
// Correction decay - 1 nsec per 65536 usec // Correction decay - 1 nsec per 65536 usec
if (utc_correction_nsec_per_overflow > 0) if (utc_correction_nsec_per_overflow > 0) {
{
utc_correction_nsec_per_overflow--; utc_correction_nsec_per_overflow--;
}
else if (utc_correction_nsec_per_overflow < 0) } else if (utc_correction_nsec_per_overflow < 0) {
{
utc_correction_nsec_per_overflow++; utc_correction_nsec_per_overflow++;
}
else } else {
{
; // Zero ; // Zero
} }
} }
return 0; return 0;
} }
src/drivers/uavcan/uavcan_drivers/kinetis/driver/src/uc_kinetis_flexcan.cpp
+192 -218
View File
File diff suppressed because it is too large Load Diff
src/drivers/uavcan/uavcan_drivers/kinetis/driver/src/uc_kinetis_thread.cpp
+8 -5
View File
@@ -11,7 +11,7 @@
namespace uavcan_kinetis namespace uavcan_kinetis
{ {
BusEvent::BusEvent(CanDriver& can_driver) BusEvent::BusEvent(CanDriver &can_driver)
{ {
sem_init(&sem_, 0, 0); sem_init(&sem_, 0, 0);
sem_setprotocol(&sem_, SEM_PRIO_NONE); sem_setprotocol(&sem_, SEM_PRIO_NONE);
@@ -35,24 +35,27 @@ bool BusEvent::wait(uavcan::MonotonicDuration duration)
abstime.tv_nsec = nsecs; abstime.tv_nsec = nsecs;
int ret; int ret;
while ((ret = sem_timedwait(&sem_, &abstime)) == -1 && errno == EINTR); while ((ret = sem_timedwait(&sem_, &abstime)) == -1 && errno == EINTR);
if (ret == -1) { // timed out or error if (ret == -1) { // timed out or error
return false; return false;
} }
return true; return true;
} }
} }
return false; return false;
} }
void BusEvent::signalFromInterrupt() void BusEvent::signalFromInterrupt()
{ {
if (sem_.semcount <= 0) if (sem_.semcount <= 0) {
{
(void)sem_post(&sem_); (void)sem_post(&sem_);
} }
if (signal_cb_)
{ if (signal_cb_) {
signal_cb_(); signal_cb_();
} }
} }
src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/bxcan.hpp
+40 -45
View File
@@ -24,30 +24,26 @@ namespace uavcan_stm32
namespace bxcan namespace bxcan
{ {
struct TxMailboxType struct TxMailboxType {
{
volatile uint32_t TIR; volatile uint32_t TIR;
volatile uint32_t TDTR; volatile uint32_t TDTR;
volatile uint32_t TDLR; volatile uint32_t TDLR;
volatile uint32_t TDHR; volatile uint32_t TDHR;
}; };
struct RxMailboxType struct RxMailboxType {
{
volatile uint32_t RIR; volatile uint32_t RIR;
volatile uint32_t RDTR; volatile uint32_t RDTR;
volatile uint32_t RDLR; volatile uint32_t RDLR;
volatile uint32_t RDHR; volatile uint32_t RDHR;
}; };
struct FilterRegisterType struct FilterRegisterType {
{
volatile uint32_t FR1; volatile uint32_t FR1;
volatile uint32_t FR2; volatile uint32_t FR2;
}; };
struct CanType struct CanType {
{
volatile uint32_t MCR; /*!< CAN master control register, Address offset: 0x00 */ 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 MSR; /*!< CAN master status register, Address offset: 0x04 */
volatile uint32_t TSR; /*!< CAN transmit status register, Address offset: 0x08 */ volatile uint32_t TSR; /*!< CAN transmit status register, Address offset: 0x08 */
@@ -75,12 +71,11 @@ struct CanType
/** /**
* CANx register sets * CANx register sets
*/ */
CanType* const Can[UAVCAN_STM32_NUM_IFACES] = CanType *const Can[UAVCAN_STM32_NUM_IFACES] = {
{ reinterpret_cast<CanType *>(0x40006400)
reinterpret_cast<CanType*>(0x40006400)
#if UAVCAN_STM32_NUM_IFACES > 1 #if UAVCAN_STM32_NUM_IFACES > 1
, ,
reinterpret_cast<CanType*>(0x40006800) reinterpret_cast<CanType *>(0x40006800)
#endif #endif
}; };
@@ -94,8 +89,8 @@ constexpr unsigned long MCR_NART = (1U << 4); /* Bit 4: No Automatic
constexpr unsigned long MCR_AWUM = (1U << 5); /* Bit 5: Automatic Wakeup Mode */ 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_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_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_RESET = (1U << 15); /* Bit 15: bxCAN software master reset */
constexpr unsigned long MCR_DBF = (1U << 16);/* Bit 16: Debug freeze */ constexpr unsigned long MCR_DBF = (1U << 16); /* Bit 16: Debug freeze */
/* CAN master status register */ /* CAN master status register */
@@ -106,8 +101,8 @@ constexpr unsigned long MSR_WKUI = (1U << 3); /* Bit 3: Wakeup Interr
constexpr unsigned long MSR_SLAKI = (1U << 4); /* Bit 4: Sleep acknowledge 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_TXM = (1U << 8); /* Bit 8: Transmit Mode */
constexpr unsigned long MSR_RXM = (1U << 9); /* Bit 9: Receive 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_SAMP = (1U << 10); /* Bit 10: Last Sample Point */
constexpr unsigned long MSR_RX = (1U << 11);/* Bit 11: CAN Rx Signal */ constexpr unsigned long MSR_RX = (1U << 11); /* Bit 11: CAN Rx Signal */
/* CAN transmit status register */ /* CAN transmit status register */
@@ -118,22 +113,22 @@ constexpr unsigned long TSR_TERR0 = (1U << 3); /* Bit 3 : Transmission
constexpr unsigned long TSR_ABRQ0 = (1U << 7); /* Bit 7 : Abort Request for 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_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_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_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_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_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_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_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_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_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_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_SHIFT = (24U); /* Bits 25-24: Mailbox Code */
constexpr unsigned long TSR_CODE_MASK = (3U << TSR_CODE_SHIFT); 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_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_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_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_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_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 */ constexpr unsigned long TSR_LOW2 = (1U << 31); /* Bit 31: Lowest Priority Flag for Mailbox 2 */
/* CAN receive FIFO 0/1 registers */ /* CAN receive FIFO 0/1 registers */
@@ -154,11 +149,11 @@ constexpr unsigned long IER_FFIE1 = (1U << 5); /* Bit 5: FIFO Full Int
constexpr unsigned long IER_FOVIE1 = (1U << 6); /* Bit 6: FIFO Overrun 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_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_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_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_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_ERRIE = (1U << 15); /* Bit 15: Error Interrupt Enable */
constexpr unsigned long IER_WKUIE = (1U << 16);/* Bit 16: Wakeup 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 */ constexpr unsigned long IER_SLKIE = (1U << 17); /* Bit 17: Sleep Interrupt Enable */
/* CAN error status register */ /* CAN error status register */
@@ -167,14 +162,14 @@ constexpr unsigned long ESR_EPVF = (1U << 1); /* Bit 1: Error Passive
constexpr unsigned long ESR_BOFF = (1U << 2); /* Bit 2: Bus-Off 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_SHIFT = (4U); /* Bits 6-4: Last Error Code */
constexpr unsigned long ESR_LEC_MASK = (7U << ESR_LEC_SHIFT); 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_NOERROR = (0U << ESR_LEC_SHIFT); /* 000: No Error */
constexpr unsigned long ESR_STUFFERROR = (1U << ESR_LEC_SHIFT);/* 001: Stuff 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_FORMERROR = (2U << ESR_LEC_SHIFT); /* 010: Form Error */
constexpr unsigned long ESR_ACKERROR = (3U << ESR_LEC_SHIFT);/* 011: Acknowledgment 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_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_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_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_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_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_TEC_MASK = (0xFFU << ESR_TEC_SHIFT);
constexpr unsigned long ESR_REC_SHIFT = (24U); /* Bits 31-24: Receive Error Counter */ constexpr unsigned long ESR_REC_SHIFT = (24U); /* Bits 31-24: Receive Error Counter */
@@ -190,8 +185,8 @@ constexpr unsigned long BTR_TS2_SHIFT = (20U); /* Bits 22-20: Time Seg
constexpr unsigned long BTR_TS2_MASK = (7U << BTR_TS2_SHIFT); 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_SHIFT = (24U); /* Bits 25-24: Resynchronization Jump Width */
constexpr unsigned long BTR_SJW_MASK = (3U << BTR_SJW_SHIFT); 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_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_SILM = (1U << 31); /* Bit 31: Silent Mode (Debug);*/
constexpr unsigned long BTR_BRP_MAX = (1024U); /* Maximum BTR value (without decrement);*/ 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_TSEG1_MAX = (16U); /* Maximum TSEG1 value (without decrement);*/
src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp
+34 -45
View File
@@ -29,8 +29,7 @@ static const uavcan::int16_t ErrFilterNumConfigs = 1008; ///< Number of f
* RX queue item. * RX queue item.
* The application shall not use this directly. * The application shall not use this directly.
*/ */
struct CanRxItem struct CanRxItem {
{
uavcan::uint64_t utc_usec; uavcan::uint64_t utc_usec;
uavcan::CanFrame frame; uavcan::CanFrame frame;
uavcan::CanIOFlags flags; uavcan::CanIOFlags flags;
@@ -48,7 +47,7 @@ class CanIface : public uavcan::ICanIface, uavcan::Noncopyable
{ {
class RxQueue class RxQueue
{ {
CanRxItem* const buf_; CanRxItem *const buf_;
const uavcan::uint8_t capacity_; const uavcan::uint8_t capacity_;
uavcan::uint8_t in_; uavcan::uint8_t in_;
uavcan::uint8_t out_; uavcan::uint8_t out_;
@@ -58,7 +57,7 @@ class CanIface : public uavcan::ICanIface, uavcan::Noncopyable
void registerOverflow(); void registerOverflow();
public: public:
RxQueue(CanRxItem* buf, uavcan::uint8_t capacity) RxQueue(CanRxItem *buf, uavcan::uint8_t capacity)
: buf_(buf) : buf_(buf)
, capacity_(capacity) , capacity_(capacity)
, in_(0) , in_(0)
@@ -67,8 +66,8 @@ class CanIface : public uavcan::ICanIface, uavcan::Noncopyable
, overflow_cnt_(0) , overflow_cnt_(0)
{ } { }
void push(const uavcan::CanFrame& frame, const uint64_t& utc_usec, uavcan::CanIOFlags flags); 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 pop(uavcan::CanFrame &out_frame, uavcan::uint64_t &out_utc_usec, uavcan::CanIOFlags &out_flags);
void reset(); void reset();
@@ -77,8 +76,7 @@ class CanIface : public uavcan::ICanIface, uavcan::Noncopyable
uavcan::uint32_t getOverflowCount() const { return overflow_cnt_; } uavcan::uint32_t getOverflowCount() const { return overflow_cnt_; }
}; };
struct Timings struct Timings {
{
uavcan::uint16_t prescaler; uavcan::uint16_t prescaler;
uavcan::uint8_t sjw; uavcan::uint8_t sjw;
uavcan::uint8_t bs1; uavcan::uint8_t bs1;
@@ -92,8 +90,7 @@ class CanIface : public uavcan::ICanIface, uavcan::Noncopyable
{ } { }
}; };
struct TxItem struct TxItem {
{
uavcan::MonotonicTime deadline; uavcan::MonotonicTime deadline;
uavcan::CanFrame frame; uavcan::CanFrame frame;
bool pending; bool pending;
@@ -113,24 +110,24 @@ class CanIface : public uavcan::ICanIface, uavcan::Noncopyable
static const uavcan::uint32_t TSR_ABRQx[NumTxMailboxes]; static const uavcan::uint32_t TSR_ABRQx[NumTxMailboxes];
RxQueue rx_queue_; RxQueue rx_queue_;
bxcan::CanType* const can_; bxcan::CanType *const can_;
uavcan::uint64_t error_cnt_; uavcan::uint64_t error_cnt_;
uavcan::uint32_t served_aborts_cnt_; uavcan::uint32_t served_aborts_cnt_;
BusEvent& update_event_; BusEvent &update_event_;
TxItem pending_tx_[NumTxMailboxes]; TxItem pending_tx_[NumTxMailboxes];
uavcan::uint8_t peak_tx_mailbox_index_; uavcan::uint8_t peak_tx_mailbox_index_;
const uavcan::uint8_t self_index_; const uavcan::uint8_t self_index_;
bool had_activity_; bool had_activity_;
int computeTimings(uavcan::uint32_t target_bitrate, Timings& out_timings); int computeTimings(uavcan::uint32_t target_bitrate, Timings &out_timings);
virtual uavcan::int16_t send(const uavcan::CanFrame& frame, uavcan::MonotonicTime tx_deadline, virtual uavcan::int16_t send(const uavcan::CanFrame &frame, uavcan::MonotonicTime tx_deadline,
uavcan::CanIOFlags flags); uavcan::CanIOFlags flags);
virtual uavcan::int16_t receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic, virtual uavcan::int16_t receive(uavcan::CanFrame &out_frame, uavcan::MonotonicTime &out_ts_monotonic,
uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags); uavcan::UtcTime &out_ts_utc, uavcan::CanIOFlags &out_flags);
virtual uavcan::int16_t configureFilters(const uavcan::CanFilterConfig* filter_configs, virtual uavcan::int16_t configureFilters(const uavcan::CanFilterConfig *filter_configs,
uavcan::uint16_t num_configs); uavcan::uint16_t num_configs);
virtual uavcan::uint16_t getNumFilters() const { return NumFilters; } virtual uavcan::uint16_t getNumFilters() const { return NumFilters; }
@@ -142,14 +139,13 @@ class CanIface : public uavcan::ICanIface, uavcan::Noncopyable
public: public:
enum { MaxRxQueueCapacity = 254 }; enum { MaxRxQueueCapacity = 254 };
enum OperatingMode enum OperatingMode {
{
NormalMode, NormalMode,
SilentMode SilentMode
}; };
CanIface(bxcan::CanType* can, BusEvent& update_event, uavcan::uint8_t self_index, CanIface(bxcan::CanType *can, BusEvent &update_event, uavcan::uint8_t self_index,
CanRxItem* rx_queue_buffer, uavcan::uint8_t rx_queue_capacity) CanRxItem *rx_queue_buffer, uavcan::uint8_t rx_queue_capacity)
: rx_queue_(rx_queue_buffer, rx_queue_capacity) : rx_queue_(rx_queue_buffer, rx_queue_capacity)
, can_(can) , can_(can)
, error_cnt_(0) , error_cnt_(0)
@@ -185,7 +181,7 @@ public:
void discardTimedOutTxMailboxes(uavcan::MonotonicTime current_time); void discardTimedOutTxMailboxes(uavcan::MonotonicTime current_time);
bool canAcceptNewTxFrame(const uavcan::CanFrame& frame) const; bool canAcceptNewTxFrame(const uavcan::CanFrame &frame) const;
bool isRxBufferEmpty() const; bool isRxBufferEmpty() const;
/** /**
@@ -239,28 +235,28 @@ class CanDriver : public uavcan::ICanDriver, uavcan::Noncopyable
CanIface if1_; CanIface if1_;
#endif #endif
virtual uavcan::int16_t select(uavcan::CanSelectMasks& inout_masks, virtual uavcan::int16_t select(uavcan::CanSelectMasks &inout_masks,
const uavcan::CanFrame* (& pending_tx)[uavcan::MaxCanIfaces], const uavcan::CanFrame * (& pending_tx)[uavcan::MaxCanIfaces],
uavcan::MonotonicTime blocking_deadline); uavcan::MonotonicTime blocking_deadline);
static void initOnce(); static void initOnce();
public: public:
template <unsigned RxQueueCapacity> template <unsigned RxQueueCapacity>
CanDriver(CanRxItem (&rx_queue_storage)[UAVCAN_STM32_NUM_IFACES][RxQueueCapacity]) CanDriver(CanRxItem(&rx_queue_storage)[UAVCAN_STM32_NUM_IFACES][RxQueueCapacity])
: update_event_(*this) : update_event_(*this)
, if0_(bxcan::Can[0], update_event_, 0, rx_queue_storage[0], RxQueueCapacity) , if0_(bxcan::Can[0], update_event_, 0, rx_queue_storage[0], RxQueueCapacity)
#if UAVCAN_STM32_NUM_IFACES > 1 #if UAVCAN_STM32_NUM_IFACES > 1
, if1_(bxcan::Can[1], update_event_, 1, rx_queue_storage[1], RxQueueCapacity) , if1_(bxcan::Can[1], update_event_, 1, rx_queue_storage[1], RxQueueCapacity)
#endif #endif
{ {
uavcan::StaticAssert<(RxQueueCapacity <= CanIface::MaxRxQueueCapacity)>::check(); uavcan::StaticAssert < (RxQueueCapacity <= CanIface::MaxRxQueueCapacity) >::check();
} }
/** /**
* This function returns select masks indicating which interfaces are available for read/write. * This function returns select masks indicating which interfaces are available for read/write.
*/ */
uavcan::CanSelectMasks makeSelectMasks(const uavcan::CanFrame* (& pending_tx)[uavcan::MaxCanIfaces]) const; uavcan::CanSelectMasks makeSelectMasks(const uavcan::CanFrame * (& pending_tx)[uavcan::MaxCanIfaces]) const;
/** /**
* Whether there's at least one interface where receive() would return a frame. * Whether there's at least one interface where receive() would return a frame.
@@ -273,7 +269,7 @@ public:
*/ */
int init(const uavcan::uint32_t bitrate, const CanIface::OperatingMode mode); int init(const uavcan::uint32_t bitrate, const CanIface::OperatingMode mode);
virtual CanIface* getIface(uavcan::uint8_t iface_index); virtual CanIface *getIface(uavcan::uint8_t iface_index);
virtual uavcan::uint8_t getNumIfaces() const { return UAVCAN_STM32_NUM_IFACES; } virtual uavcan::uint8_t getNumIfaces() const { return UAVCAN_STM32_NUM_IFACES; }
@@ -283,7 +279,7 @@ public:
*/ */
bool hadActivity(); bool hadActivity();
BusEvent& updateEvent() { return update_event_; } BusEvent &updateEvent() { return update_event_; }
}; };
/** /**
@@ -331,36 +327,29 @@ public:
* @return Negative value on error; non-negative on success. Refer to constants Err*. * @return Negative value on error; non-negative on success. Refer to constants Err*.
*/ */
template <typename DelayCallable> template <typename DelayCallable>
int init(DelayCallable delay_callable, uavcan::uint32_t& inout_bitrate = BitRateAutoDetect) int init(DelayCallable delay_callable, uavcan::uint32_t &inout_bitrate = BitRateAutoDetect)
{
if (inout_bitrate > 0)
{ {
if (inout_bitrate > 0) {
return driver.init(inout_bitrate, CanIface::NormalMode); return driver.init(inout_bitrate, CanIface::NormalMode);
}
else } else {
{ static const uavcan::uint32_t StandardBitRates[] = {
static const uavcan::uint32_t StandardBitRates[] =
{
1000000, 1000000,
500000, 500000,
250000, 250000,
125000 125000
}; };
for (uavcan::uint8_t br = 0; br < sizeof(StandardBitRates) / sizeof(StandardBitRates[0]); br++) for (uavcan::uint8_t br = 0; br < sizeof(StandardBitRates) / sizeof(StandardBitRates[0]); br++) {
{
inout_bitrate = StandardBitRates[br]; inout_bitrate = StandardBitRates[br];
const int res = driver.init(inout_bitrate, CanIface::SilentMode); const int res = driver.init(inout_bitrate, CanIface::SilentMode);
delay_callable(); delay_callable();
if (res >= 0) if (res >= 0) {
{ for (uavcan::uint8_t iface = 0; iface < driver.getNumIfaces(); iface++) {
for (uavcan::uint8_t iface = 0; iface < driver.getNumIfaces(); iface++) if (!driver.getIface(iface)->isRxBufferEmpty()) {
{
if (!driver.getIface(iface)->isRxBufferEmpty())
{
// Re-initializing in normal mode // Re-initializing in normal mode
return driver.init(inout_bitrate, CanIface::NormalMode); return driver.init(inout_bitrate, CanIface::NormalMode);
} }
src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/clock.hpp
+3 -4
View File
@@ -48,8 +48,7 @@ void adjustUtc(uavcan::UtcDuration adjustment);
/** /**
* UTC clock synchronization parameters * UTC clock synchronization parameters
*/ */
struct UtcSyncParams struct UtcSyncParams {
{
float offset_p; ///< PPM per one usec error float offset_p; ///< PPM per one usec error
float rate_i; ///< PPM per one PPM error for second float rate_i; ///< PPM per one PPM error for second
float rate_error_corner_freq; float rate_error_corner_freq;
@@ -94,7 +93,7 @@ bool isUtcLocked();
* Both functions are thread safe. * Both functions are thread safe.
*/ */
UtcSyncParams getUtcSyncParams(); UtcSyncParams getUtcSyncParams();
void setUtcSyncParams(const UtcSyncParams& params); void setUtcSyncParams(const UtcSyncParams &params);
} }
@@ -115,7 +114,7 @@ public:
* Calls clock::init() as needed. * Calls clock::init() as needed.
* This function is thread safe. * This function is thread safe.
*/ */
static SystemClock& instance(); static SystemClock &instance();
}; };
} }
src/drivers/uavcan/uavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp
+3 -3
View File
@@ -38,7 +38,7 @@ class BusEvent : uavcan::Noncopyable
sem_t sem_; sem_t sem_;
public: public:
BusEvent(CanDriver& can_driver); BusEvent(CanDriver &can_driver);
~BusEvent(); ~BusEvent();
void registerSignalCallback(SignalCallbackHandler handler) { signal_cb_ = handler; } void registerSignalCallback(SignalCallbackHandler handler) { signal_cb_ = handler; }
@@ -83,10 +83,10 @@ public:
class MutexLocker class MutexLocker
{ {
Mutex& mutex_; Mutex &mutex_;
public: public:
MutexLocker(Mutex& mutex) MutexLocker(Mutex &mutex)
: mutex_(mutex) : mutex_(mutex)
{ {
mutex_.lock(); mutex_.lock();
src/drivers/uavcan/uavcan_drivers/stm32/driver/src/internal.hpp
+1 -2
View File
@@ -50,8 +50,7 @@ namespace uavcan_stm32
{ {
#if UAVCAN_STM32_NUTTX #if UAVCAN_STM32_NUTTX
struct CriticalSectionLocker struct CriticalSectionLocker {
{
const irqstate_t flags_; const irqstate_t flags_;
CriticalSectionLocker() CriticalSectionLocker()
src/drivers/uavcan/uavcan_drivers/stm32/driver/src/uc_stm32_can.cpp
+190 -232
View File
File diff suppressed because it is too large Load Diff
src/drivers/uavcan/uavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp
+35 -45
View File
@@ -83,10 +83,11 @@ uavcan::uint64_t time_utc = 0;
void init() void init()
{ {
CriticalSectionLocker lock; CriticalSectionLocker lock;
if (initialized)
{ if (initialized) {
return; return;
} }
initialized = true; initialized = true;
@@ -103,7 +104,7 @@ void init()
// Start the timer // Start the timer
putreg32(0xFFFF, TMR_REG(STM32_BTIM_ARR_OFFSET)); putreg32(0xFFFF, TMR_REG(STM32_BTIM_ARR_OFFSET));
putreg16(((TIMX_INPUT_CLOCK / 1000000)-1), TMR_REG(STM32_BTIM_PSC_OFFSET)); putreg16(((TIMX_INPUT_CLOCK / 1000000) - 1), TMR_REG(STM32_BTIM_PSC_OFFSET));
putreg16(BTIM_CR1_URS, TMR_REG(STM32_BTIM_CR1_OFFSET)); putreg16(BTIM_CR1_URS, TMR_REG(STM32_BTIM_CR1_OFFSET));
putreg16(0, TMR_REG(STM32_BTIM_SR_OFFSET)); putreg16(0, TMR_REG(STM32_BTIM_SR_OFFSET));
putreg16(BTIM_EGR_UG, TMR_REG(STM32_BTIM_EGR_OFFSET)); // Reload immediately putreg16(BTIM_EGR_UG, TMR_REG(STM32_BTIM_EGR_OFFSET)); // Reload immediately
@@ -146,13 +147,13 @@ static uavcan::uint64_t sampleUtcFromCriticalSection()
volatile uavcan::uint64_t time = time_utc; volatile uavcan::uint64_t time = time_utc;
volatile uavcan::uint32_t cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET)); volatile uavcan::uint32_t cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET));
if (getreg16(TMR_REG(STM32_BTIM_SR_OFFSET)) & BTIM_SR_UIF) if (getreg16(TMR_REG(STM32_BTIM_SR_OFFSET)) & BTIM_SR_UIF) {
{
cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET)); cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET));
const uavcan::int32_t add = uavcan::int32_t(USecPerOverflow) + const uavcan::int32_t add = uavcan::int32_t(USecPerOverflow) +
(utc_accumulated_correction_nsec + utc_correction_nsec_per_overflow) / 1000; (utc_accumulated_correction_nsec + utc_correction_nsec_per_overflow) / 1000;
time = uavcan::uint64_t(uavcan::int64_t(time) + add); time = uavcan::uint64_t(uavcan::int64_t(time) + add);
} }
return time + cnt; return time + cnt;
# endif # endif
} }
@@ -175,12 +176,12 @@ uavcan::MonotonicTime getMonotonic()
volatile uavcan::uint32_t cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET)); volatile uavcan::uint32_t cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET));
if (getreg16(TMR_REG(STM32_BTIM_SR_OFFSET)) & BTIM_SR_UIF) if (getreg16(TMR_REG(STM32_BTIM_SR_OFFSET)) & BTIM_SR_UIF) {
{
cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET)); cnt = getreg16(TMR_REG(STM32_BTIM_CNT_OFFSET));
# endif # endif
time += USecPerOverflow; time += USecPerOverflow;
} }
usec = time + cnt; usec = time + cnt;
# ifndef NDEBUG # ifndef NDEBUG
@@ -196,8 +197,7 @@ uavcan::MonotonicTime getMonotonic()
uavcan::UtcTime getUtc() uavcan::UtcTime getUtc()
{ {
if (utc_set) if (utc_set) {
{
uavcan::uint64_t usec = 0; uavcan::uint64_t usec = 0;
{ {
CriticalSectionLocker locker; CriticalSectionLocker locker;
@@ -205,6 +205,7 @@ uavcan::UtcTime getUtc()
} }
return uavcan::UtcTime::fromUSec(usec); return uavcan::UtcTime::fromUSec(usec);
} }
return uavcan::UtcTime(); return uavcan::UtcTime();
} }
@@ -237,12 +238,10 @@ static void updateRatePID(uavcan::UtcDuration adjustment)
const float rel_rate_error = target_rel_rate_ppm - utc_rel_rate_ppm; const float rel_rate_error = target_rel_rate_ppm - utc_rel_rate_ppm;
if (dt > 10) if (dt > 10) {
{
utc_rel_rate_error_integral = 0; utc_rel_rate_error_integral = 0;
}
else } else {
{
utc_rel_rate_error_integral += rel_rate_error * dt * utc_sync_params.rate_i; utc_rel_rate_error_integral += rel_rate_error * dt * utc_sync_params.rate_i;
utc_rel_rate_error_integral = utc_rel_rate_error_integral =
uavcan::max(utc_rel_rate_error_integral, -utc_sync_params.max_rate_correction_ppm); uavcan::max(utc_rel_rate_error_integral, -utc_sync_params.max_rate_correction_ppm);
@@ -269,18 +268,16 @@ void adjustUtc(uavcan::UtcDuration adjustment)
MutexLocker mlocker(mutex); MutexLocker mlocker(mutex);
UAVCAN_ASSERT(initialized); UAVCAN_ASSERT(initialized);
if (adjustment.getAbs() > utc_sync_params.min_jump || !utc_set) if (adjustment.getAbs() > utc_sync_params.min_jump || !utc_set) {
{
const uavcan::int64_t adj_usec = adjustment.toUSec(); const uavcan::int64_t adj_usec = adjustment.toUSec();
{ {
CriticalSectionLocker locker; CriticalSectionLocker locker;
if ((adj_usec < 0) && uavcan::uint64_t(-adj_usec) > time_utc)
{ if ((adj_usec < 0) && uavcan::uint64_t(-adj_usec) > time_utc) {
time_utc = 1; time_utc = 1;
}
else } else {
{
time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + adj_usec); time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + adj_usec);
} }
} }
@@ -290,13 +287,11 @@ void adjustUtc(uavcan::UtcDuration adjustment)
utc_jump_cnt++; utc_jump_cnt++;
utc_prev_adj = 0; utc_prev_adj = 0;
utc_rel_rate_ppm = 0; utc_rel_rate_ppm = 0;
}
else } else {
{
updateRatePID(adjustment); updateRatePID(adjustment);
if (!utc_locked) if (!utc_locked) {
{
utc_locked = utc_locked =
(std::abs(utc_rel_rate_ppm) < utc_sync_params.lock_thres_rate_ppm) && (std::abs(utc_rel_rate_ppm) < utc_sync_params.lock_thres_rate_ppm) &&
(std::abs(utc_prev_adj) < float(utc_sync_params.lock_thres_offset.toUSec())); (std::abs(utc_prev_adj) < float(utc_sync_params.lock_thres_offset.toUSec()));
@@ -329,7 +324,7 @@ UtcSyncParams getUtcSyncParams()
return utc_sync_params; return utc_sync_params;
} }
void setUtcSyncParams(const UtcSyncParams& params) void setUtcSyncParams(const UtcSyncParams &params)
{ {
MutexLocker mlocker(mutex); MutexLocker mlocker(mutex);
// Add some sanity check // Add some sanity check
@@ -338,23 +333,22 @@ void setUtcSyncParams(const UtcSyncParams& params)
} // namespace clock } // namespace clock
SystemClock& SystemClock::instance() SystemClock &SystemClock::instance()
{ {
static union SystemClockStorage static union SystemClockStorage {
{
uavcan::uint8_t buffer[sizeof(SystemClock)]; uavcan::uint8_t buffer[sizeof(SystemClock)];
long long _aligner_1; long long _aligner_1;
long double _aligner_2; long double _aligner_2;
} storage; } storage;
SystemClock* const ptr = reinterpret_cast<SystemClock*>(storage.buffer); SystemClock *const ptr = reinterpret_cast<SystemClock *>(storage.buffer);
if (!clock::initialized) if (!clock::initialized) {
{
MutexLocker mlocker(clock::mutex); MutexLocker mlocker(clock::mutex);
clock::init(); clock::init();
new (ptr)SystemClock(); new (ptr)SystemClock();
} }
return *ptr; return *ptr;
} }
@@ -379,27 +373,23 @@ UAVCAN_STM32_IRQ_HANDLER(TIMX_IRQHandler)
time_mono += USecPerOverflow; time_mono += USecPerOverflow;
if (utc_set) if (utc_set) {
{
time_utc += USecPerOverflow; time_utc += USecPerOverflow;
utc_accumulated_correction_nsec += utc_correction_nsec_per_overflow; utc_accumulated_correction_nsec += utc_correction_nsec_per_overflow;
if (std::abs(utc_accumulated_correction_nsec) >= 1000)
{ if (std::abs(utc_accumulated_correction_nsec) >= 1000) {
time_utc = uavcan::uint64_t(uavcan::int64_t(time_utc) + 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; utc_accumulated_correction_nsec %= 1000;
} }
// Correction decay - 1 nsec per 65536 usec // Correction decay - 1 nsec per 65536 usec
if (utc_correction_nsec_per_overflow > 0) if (utc_correction_nsec_per_overflow > 0) {
{
utc_correction_nsec_per_overflow--; utc_correction_nsec_per_overflow--;
}
else if (utc_correction_nsec_per_overflow < 0) } else if (utc_correction_nsec_per_overflow < 0) {
{
utc_correction_nsec_per_overflow++; utc_correction_nsec_per_overflow++;
}
else } else {
{
; // Zero ; // Zero
} }
} }
src/drivers/uavcan/uavcan_drivers/stm32/driver/src/uc_stm32_thread.cpp
+8 -5
View File
@@ -13,7 +13,7 @@ namespace uavcan_stm32
#if UAVCAN_STM32_NUTTX #if UAVCAN_STM32_NUTTX
BusEvent::BusEvent(CanDriver& can_driver) BusEvent::BusEvent(CanDriver &can_driver)
{ {
sem_init(&sem_, 0, 0); sem_init(&sem_, 0, 0);
sem_setprotocol(&sem_, SEM_PRIO_NONE); sem_setprotocol(&sem_, SEM_PRIO_NONE);
@@ -37,24 +37,27 @@ bool BusEvent::wait(uavcan::MonotonicDuration duration)
abstime.tv_nsec = nsecs; abstime.tv_nsec = nsecs;
int ret; int ret;
while ((ret = sem_timedwait(&sem_, &abstime)) == -1 && errno == EINTR); while ((ret = sem_timedwait(&sem_, &abstime)) == -1 && errno == EINTR);
if (ret == -1) { // timed out or error if (ret == -1) { // timed out or error
return false; return false;
} }
return true; return true;
} }
} }
return false; return false;
} }
void BusEvent::signalFromInterrupt() void BusEvent::signalFromInterrupt()
{ {
if (sem_.semcount <= 0) if (sem_.semcount <= 0) {
{
(void)sem_post(&sem_); (void)sem_post(&sem_);
} }
if (signal_cb_)
{ if (signal_cb_) {
signal_cb_(); signal_cb_();
} }
} }