fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-14 03:23:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

STM32: Partially implemented CAN driver (transmission only); added debug tracing macro and some stuff into internal.hpp

Browse Source
This commit is contained in:
Pavel Kirienko
2014-04-04 23:28:34 +04:00
parent daa7b9ec19
commit f66338d329
6 changed files with 851 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files
.gitignore
+1
View File
@@ -13,6 +13,7 @@ __pycache__
.project
.cproject
.pydevproject
.gdbinit
# libuavcan DSDL compiler default output directory
dsdlc_generated
libuavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp
+185 -10
View File
@@ -4,27 +4,116 @@
#pragma once
#include <uavcan_stm32/thread.hpp>
#include <uavcan/driver/can.hpp>
#if UAVCAN_STM32_CHIBIOS
# include <hal.h>
#else
# error "Unknown OS"
#endif
#include <uavcan/driver/can.hpp>
#ifndef UAVCAN_STM32_NUM_IFACES
# if defined(STM32F10X_CL) || defined(STM32F2XX) || defined(STM32F4XX)
# define UAVCAN_STM32_NUM_IFACES 2
# else
# define UAVCAN_STM32_NUM_IFACES 1
# endif
#endif
#if UAVCAN_STM32_NUM_IFACES != 1 && UAVCAN_STM32_NUM_IFACES != 2
# error UAVCAN_STM32_NUM_IFACES
#endif
namespace uavcan_stm32
{
enum { CanFiltersPerIface = 14 };
class CanIface : public uavcan::ICanIface
/**
* RX queue item
*/
struct CanRxItem
{
CAN_TypeDef* can_;
uavcan::uint64_t utc_usec;
uavcan::CanFrame frame;
uavcan::CanIOFlags flags;
CanRxItem()
: utc_usec(0)
, flags(0)
{ }
};
/**
* Single CAN iface.
*/
class CanIface : public uavcan::ICanIface, uavcan::Noncopyable
{
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);
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::CanFrame frame;
uavcan::MonotonicTime deadline;
bool pending;
bool loopback;
TxItem()
: pending(false)
, loopback(false)
{ }
};
enum { NumTxMailboxes = 3 };
enum { NumFilters = 14 };
RxQueue rx_queue_;
CAN_TypeDef* const can_;
uavcan::uint64_t error_cnt_;
Event& update_event_;
TxItem pending_tx_[NumTxMailboxes];
uavcan::uint8_t last_hw_error_code_;
int computeTimings(uavcan::uint32_t target_bitrate, Timings& out_timings);
public:
virtual uavcan::int16_t send(const uavcan::CanFrame& frame, uavcan::MonotonicTime tx_deadline,
uavcan::CanIOFlags flags);
@@ -34,9 +123,95 @@ public:
virtual uavcan::int16_t configureFilters(const uavcan::CanFilterConfig* filter_configs,
uavcan::uint16_t num_configs);
virtual uavcan::uint16_t getNumFilters() const { return CanFiltersPerIface; }
virtual uavcan::uint16_t getNumFilters() const { return NumFilters; }
virtual uavcan::uint64_t getErrorCount() const { return error_cnt_; }
void handleTxMailboxInterrupt(uavcan::uint8_t mailbox_index, bool txok, uavcan::uint64_t utc_usec);
public:
enum { MaxRxQueueCapacity = 254 };
CanIface(CAN_TypeDef* can, Event& update_event, CanRxItem* rx_queue_buffer, uavcan::uint8_t rx_queue_capacity)
: rx_queue_(rx_queue_buffer, rx_queue_capacity)
, can_(can)
, error_cnt_(0)
, update_event_(update_event)
, last_hw_error_code_(0)
{ }
/**
* Assumes:
* - Iface clock is enabled
* - Iface has been resetted via RCC
* - Interrupts are disabled
* - Caller will configure NVIC by itself
*/
int init(uavcan::uint32_t bitrate);
void handleTxInterrupt(uavcan::uint64_t utc_usec);
void handleRxInterrupt(uavcan::uint8_t fifo_index, uavcan::uint64_t utc_usec);
void handleStatusInterrupt();
void discardTimedOutTxMailboxes(uavcan::MonotonicTime current_time);
bool isTxBufferFull() const;
bool isRxBufferEmpty() const { return rx_queue_.getLength() == 0; }
virtual uavcan::uint64_t getErrorCount() const;
uavcan::uint8_t yieldLastHardwareErrorCode();
};
/**
* CAN driver, incorporates all available CAN ifaces.
*/
class CanDriver : public uavcan::ICanDriver, uavcan::Noncopyable
{
Event update_event_;
CanIface if0_;
#if UAVCAN_STM32_NUM_IFACES > 1
CanIface if1_;
#endif
virtual uavcan::int16_t select(uavcan::CanSelectMasks& inout_masks, uavcan::MonotonicTime blocking_deadline);
public:
template <unsigned RxQueueCapacity>
CanDriver(CanRxItem (&rx_queue_storage)[UAVCAN_STM32_NUM_IFACES][RxQueueCapacity])
: if0_(CAN1, update_event_, rx_queue_storage[0], RxQueueCapacity)
#if UAVCAN_STM32_NUM_IFACES > 1
, if1_(CAN2, update_event_, rx_queue_storage[1], RxQueueCapacity)
#endif
{
uavcan::StaticAssert<(RxQueueCapacity <= CanIface::MaxRxQueueCapacity)>::check();
}
int init(uavcan::uint32_t bitrate);
virtual CanIface* getIface(uavcan::uint8_t iface_index);
virtual uavcan::uint8_t getNumIfaces() const { return UAVCAN_STM32_NUM_IFACES; }
};
/**
* Helper class.
* Normally only this class should be used by the application.
* 145 usec per Extended CAN frame @ 1 Mbps, e.g. 16 RX slots * 145 usec --> 2320 usec before RX queue overruns.
*/
template <unsigned RxQueueCapacity = 16>
class CanInitHelper
{
CanRxItem queue_storage_[UAVCAN_STM32_NUM_IFACES][RxQueueCapacity];
public:
CanDriver driver;
CanInitHelper()
: driver(queue_storage_)
{ }
int init(uavcan::uint32_t bitrate)
{
return driver.init(bitrate);
}
};
}
libuavcan_drivers/stm32/driver/src/internal.hpp
+51 -2
View File
@@ -10,17 +10,32 @@
# error "Unknown OS"
#endif
#if UAVCAN_STM32_DEBUG
# include <cstdio>
# include <cstdarg>
#endif
/**
* IRQ handler macros
*/
#if UAVCAN_STM32_CHIBIOS
# 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()
# define UAVCAN_STM32_IRQ_PROLOGUE() \
CH_IRQ_PROLOGUE(); \
chSysLockFromIsr()
# define UAVCAN_STM32_IRQ_EPILOGUE() \
chSysUnlockFromIsr(); \
CH_IRQ_EPILOGUE()
#else
# define UAVCAN_STM32_IRQ_HANDLER(id) void id(void)
# define UAVCAN_STM32_IRQ_PROLOGUE()
# define UAVCAN_STM32_IRQ_EPILOGUE()
#endif
/**
@@ -39,9 +54,43 @@
# error UAVCAN_STM32_TIMER_NUMBER
#endif
/**
* Driver debug output
*/
#if UAVCAN_STM32_DEBUG
# if __GNUC__
__attribute__ ((format(printf, 1, 2)))
# endif
static void UAVCAN_STM32_TRACE(const char* fmt, ...)
{
(void)UAVCAN_STM32_TRACE;
va_list args;
(void)std::printf("UAVCAN: STM32: ");
va_start(args, fmt);
(void)std::vprintf(fmt, args);
va_end(args);
(void)std::puts("");
}
#else
# define UAVCAN_STM32_TRACE(...) ((void)0)
#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 uavcan_stm32
{
struct CriticalSectionLock
{
CriticalSectionLock() { chSysSuspend(); }
~CriticalSectionLock() { chSysEnable(); }
};
}
libuavcan_drivers/stm32/driver/src/uc_stm32_can.cpp
+597
View File
@@ -2,4 +2,601 @@
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include <cassert>
#include <cstring>
#include <uavcan_stm32/can.hpp>
#include <uavcan_stm32/clock.hpp>
#include "internal.hpp"
namespace uavcan_stm32
{
namespace
{
CanIface* ifaces[UAVCAN_STM32_NUM_IFACES] =
{
NULL
#if UAVCAN_STM32_NUM_IFACES > 1
, NULL
#endif
};
inline void handleTxInterrupt(uavcan::uint8_t iface_index)
{
uavcan::uint64_t utc_usec = clock::getUtcUSecFromInterrupt();
if (utc_usec > 0)
{
utc_usec--;
}
if (iface_index < UAVCAN_STM32_NUM_IFACES &&
ifaces[iface_index] != NULL)
{
ifaces[iface_index]->handleTxInterrupt(utc_usec);
}
else
{
assert(0);
}
}
inline void handleRxInterrupt(uavcan::uint8_t iface_index, uavcan::uint8_t fifo_index)
{
uavcan::uint64_t utc_usec = clock::getUtcUSecFromInterrupt();
if (utc_usec > 0)
{
utc_usec--;
}
if (iface_index < UAVCAN_STM32_NUM_IFACES &&
ifaces[iface_index] != NULL)
{
ifaces[iface_index]->handleRxInterrupt(fifo_index, utc_usec);
}
else
{
assert(0);
}
}
inline void handleStatusInterrupt(uavcan::uint8_t iface_index)
{
if (iface_index < UAVCAN_STM32_NUM_IFACES &&
ifaces[iface_index] != NULL)
{
ifaces[iface_index]->handleStatusInterrupt();
}
else
{
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 { assert(0); }
}
/*
* CanIface
*/
int CanIface::computeTimings(const uavcan::uint32_t target_bitrate, Timings& out_timings)
{
/*
* BITRATE = 1 / (PRESCALER * (1 / PCLK) * (1 + BS1 + BS2))
* BITRATE = PCLK / (PRESCALER * (1 + BS1 + BS2))
* let:
* BS = 1 + BS1 + BS2
* PRESCALER_BS = PRESCALER * BS
* ==>
* PRESCALER_BS = PCLK / BITRATE
*/
if (target_bitrate < 20000 || target_bitrate > 1000000)
{
return -1;
}
const uavcan::uint32_t pclk = STM32_PCLK1;
const uavcan::uint32_t prescaler_bs = pclk / target_bitrate;
// Initial guess:
uavcan::int8_t bs1 = 10; // max 15
uavcan::int8_t bs2 = 5; // max 8
uavcan::uint16_t prescaler = 0;
while (1)
{
prescaler = prescaler_bs / (1 + bs1 + bs2);
// Check result:
if ((prescaler >= 1) && (prescaler <= 1024))
{
const uavcan::uint32_t current_bitrate = pclk / (prescaler * (1 + bs1 + bs2));
if (current_bitrate == target_bitrate)
{
break;
}
}
if (bs1 > bs2)
{
bs1--;
}
else
{
bs2--;
}
if (bs1 <= 0 || bs2 <= 0)
{
return -1;
}
}
if (!((prescaler >= 1) && (prescaler <= 1024)))
{
return -1;
}
out_timings.prescaler = prescaler - 1;
out_timings.sjw = 1;
out_timings.bs1 = bs1 - 1;
out_timings.bs2 = bs2 - 1;
UAVCAN_STM32_TRACE("CAN pclk=%lu bitrt=%lu presc=%u sjw=%u bs1=%u bs2=%u",
static_cast<unsigned long>(pclk), static_cast<unsigned long>(target_bitrate),
static_cast<unsigned>(out_timings.prescaler), static_cast<unsigned>(out_timings.sjw),
static_cast<unsigned>(out_timings.bs1), static_cast<unsigned>(out_timings.bs2));
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 -1; // WTF man how to handle that
}
CriticalSectionLock lock;
/*
* Seeking for an empty slot
*/
uavcan::uint8_t txmailbox = 0xFF;
if ((can_->TSR & CAN_TSR_TME0) == CAN_TSR_TME0)
{
txmailbox = 0;
}
else if ((can_->TSR & CAN_TSR_TME1) == CAN_TSR_TME1)
{
txmailbox = 1;
}
else if ((can_->TSR & CAN_TSR_TME2) == CAN_TSR_TME2)
{
txmailbox = 2;
}
else { return 0; } // No way
/*
* Setting up the mailbox
*/
CAN_TxMailBox_TypeDef& mb = can_->sTxMailBox[txmailbox];
if (frame.isExtended())
{
mb.TIR = ((frame.id & uavcan::CanFrame::MaskExtID) << 3) | CAN_TI0R_IDE;
}
else
{
mb.TIR = ((frame.id & uavcan::CanFrame::MaskStdID) << 21);
}
if (frame.isRemoteTransmissionRequest())
{
mb.TIR |= CAN_TI0R_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 |= CAN_TI0R_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) == uavcan::CanIOFlagLoopback;
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)
{
CriticalSectionLock lock;
(void)out_frame;
(void)out_ts_monotonic;
(void)out_ts_utc;
(void)out_flags;
return -1;
}
uavcan::int16_t CanIface::configureFilters(const uavcan::CanFilterConfig* filter_configs,
uavcan::uint16_t num_configs)
{
CriticalSectionLock lock;
(void)filter_configs;
(void)num_configs;
return -1;
}
int CanIface::init(uavcan::uint32_t bitrate)
{
int res = 0;
/*
* CAN timings for this bitrate
*/
Timings timings;
res = computeTimings(bitrate, timings);
if (res < 0)
{
goto leave;
}
/*
* Hardware initialization
*/
can_->MCR &= ~CAN_MCR_SLEEP; // Exit sleep mode
can_->MCR |= CAN_MCR_INRQ; // Request init
for (unsigned wait_ack = 0; wait_ack < 1000000; wait_ack++)
{
if ((can_->MSR & CAN_MSR_INAK) == CAN_MSR_INAK)
{
break;
}
}
if ((can_->MSR & CAN_MSR_INAK) != CAN_MSR_INAK)
{
res = -1;
goto leave;
}
can_->MCR = CAN_MCR_ABOM | CAN_MCR_AWUM | CAN_MCR_INRQ; // RM page 648
can_->BTR = ((timings.sjw & 3) << 24) |
((timings.bs1 & 15) << 16) |
((timings.bs2 & 7) << 20) |
(timings.prescaler & 1023);
can_->IER = CAN_IER_TMEIE | // TX mailbox empty
CAN_IER_ERRIE | // Error set in ESR register
CAN_IER_LECIE | // LEC in ESR updated
CAN_IER_FOVIE0 | // RX FIFO 0 overrun
CAN_IER_FOVIE1 | // RX FIFO 1 overrun
CAN_IER_FMPIE0 | // RX FIFO 0 is not empty
CAN_IER_FMPIE1; // RX FIFO 1 is not empty
can_->MCR &= ~CAN_MCR_INRQ; // Leave init mode
for (unsigned wait_ack = 0; wait_ack < 1000000; wait_ack++)
{
if ((can_->MSR & CAN_MSR_INAK) != CAN_MSR_INAK)
{
break;
}
}
if ((can_->MSR & CAN_MSR_INAK) == CAN_MSR_INAK)
{
res = -1;
goto leave;
}
leave:
return res;
}
void CanIface::handleTxMailboxInterrupt(uavcan::uint8_t mailbox_index, bool txok, const uavcan::uint64_t utc_usec)
{
assert(mailbox_index < NumTxMailboxes);
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 & CAN_TSR_RQCP0)
{
const bool txok = can_->TSR & CAN_TSR_TXOK0;
can_->TSR = CAN_TSR_RQCP0;
handleTxMailboxInterrupt(0, txok, utc_usec);
}
if (can_->TSR & CAN_TSR_RQCP1)
{
const bool txok = can_->TSR & CAN_TSR_TXOK1;
can_->TSR = CAN_TSR_RQCP1;
handleTxMailboxInterrupt(1, txok, utc_usec);
}
if (can_->TSR & CAN_TSR_RQCP2)
{
const bool txok = can_->TSR & CAN_TSR_TXOK2;
can_->TSR = CAN_TSR_RQCP2;
handleTxMailboxInterrupt(2, txok, utc_usec);
}
update_event_.signalFromInterrupt();
}
void CanIface::handleRxInterrupt(uavcan::uint8_t fifo_index, uavcan::uint64_t utc_usec)
{
assert(fifo_index == 0 || fifo_index == 1);
(void)fifo_index;
(void)utc_usec;
update_event_.signalFromInterrupt();
}
void CanIface::handleStatusInterrupt()
{
last_hw_error_code_ = (can_->ESR & CAN_ESR_LEC) >> 4;
can_->ESR = 0;
can_->MSR = CAN_MSR_ERRI | CAN_MSR_WKUI | CAN_MSR_SLAKI;
error_cnt_++;
}
void CanIface::discardTimedOutTxMailboxes(uavcan::MonotonicTime current_time)
{
static const uavcan::uint32_t AbortFlags[NumTxMailboxes] = { CAN_TSR_ABRQ0, CAN_TSR_ABRQ1, CAN_TSR_ABRQ2 };
CriticalSectionLock lock;
for (int i = 0; i < NumTxMailboxes; i++)
{
TxItem& txi = pending_tx_[i];
if (txi.pending && txi.deadline < current_time)
{
can_->TSR = AbortFlags[i]; // Goodnight sweet transmission
txi.pending = false;
error_cnt_++;
}
}
}
bool CanIface::isTxBufferFull() const
{
return (can_->TSR & CAN_TSR_TME) == 0;
}
uavcan::uint64_t CanIface::getErrorCount() const
{
CriticalSectionLock lock;
return error_cnt_ + rx_queue_.getOverflowCount();
}
uavcan::uint8_t CanIface::yieldLastHardwareErrorCode()
{
CriticalSectionLock lock;
const uavcan::uint8_t val = last_hw_error_code_;
last_hw_error_code_ = 0;
return val;
}
/*
* CanDriver
*/
uavcan::int16_t CanDriver::select(uavcan::CanSelectMasks& inout_masks, uavcan::MonotonicTime blocking_deadline)
{
(void)inout_masks;
(void)blocking_deadline;
return -1;
}
int CanDriver::init(uavcan::uint32_t bitrate)
{
int res = 0;
CriticalSectionLock lock;
/*
* CAN1
*/
RCC->APB1ENR |= RCC_APB1ENR_CAN1EN;
RCC->APB1RSTR |= RCC_APB1RSTR_CAN1RST;
RCC->APB1RSTR &= ~RCC_APB1RSTR_CAN1RST;
res = if0_.init(bitrate);
if (res < 0)
{
goto fail;
}
ifaces[0] = &if0_;
/*
* CAN2
*/
#if UAVCAN_STM32_NUM_IFACES > 1
RCC->APB1ENR |= RCC_APB1ENR_CAN2EN;
RCC->APB1RSTR |= RCC_APB1RSTR_CAN2RST;
RCC->APB1RSTR &= ~RCC_APB1RSTR_CAN2RST;
res = if1_.init(bitrate);
if (res < 0)
{
goto fail;
}
ifaces[1] = &if1_;
#endif
/*
* IRQ
*/
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);
nvicEnableVector(CAN1_SCE_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);
nvicEnableVector(CAN2_SCE_IRQn, UAVCAN_STM32_IRQ_PRIORITY_MASK);
#endif
assert(res >= 0);
return res;
fail:
assert(res < 0);
RCC->APB1ENR &= ~RCC_APB1ENR_CAN1EN;
#if UAVCAN_STM32_NUM_IFACES > 1
RCC->APB1ENR &= ~RCC_APB1ENR_CAN2EN;
#endif
return res;
}
CanIface* CanDriver::getIface(uavcan::uint8_t iface_index)
{
if (iface_index < UAVCAN_STM32_NUM_IFACES)
{
return ifaces[iface_index];
}
return NULL;
}
} // namespace uavcan_stm32
/*
* Interrupt handlers
*/
#if !(defined(STM32F10X_CL) || defined(STM32F2XX) || 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
extern "C"
{
UAVCAN_STM32_IRQ_HANDLER(CAN1_TX_IRQHandler)
{
UAVCAN_STM32_IRQ_PROLOGUE();
uavcan_stm32::handleTxInterrupt(0);
UAVCAN_STM32_IRQ_EPILOGUE();
}
UAVCAN_STM32_IRQ_HANDLER(CAN1_RX0_IRQHandler)
{
UAVCAN_STM32_IRQ_PROLOGUE();
uavcan_stm32::handleRxInterrupt(0, 0);
UAVCAN_STM32_IRQ_EPILOGUE();
}
UAVCAN_STM32_IRQ_HANDLER(CAN1_RX1_IRQHandler)
{
UAVCAN_STM32_IRQ_PROLOGUE();
uavcan_stm32::handleRxInterrupt(0, 1);
UAVCAN_STM32_IRQ_EPILOGUE();
}
UAVCAN_STM32_IRQ_HANDLER(CAN1_SCE_IRQHandler)
{
UAVCAN_STM32_IRQ_PROLOGUE();
uavcan_stm32::handleStatusInterrupt(0);
UAVCAN_STM32_IRQ_EPILOGUE();
}
#if UAVCAN_STM32_NUM_IFACES > 1
UAVCAN_STM32_IRQ_HANDLER(CAN2_TX_IRQHandler)
{
UAVCAN_STM32_IRQ_PROLOGUE();
uavcan_stm32::handleTxInterrupt(1);
UAVCAN_STM32_IRQ_EPILOGUE();
}
UAVCAN_STM32_IRQ_HANDLER(CAN2_RX0_IRQHandler)
{
UAVCAN_STM32_IRQ_PROLOGUE();
uavcan_stm32::handleRxInterrupt(1, 0);
UAVCAN_STM32_IRQ_EPILOGUE();
}
UAVCAN_STM32_IRQ_HANDLER(CAN2_RX1_IRQHandler)
{
UAVCAN_STM32_IRQ_PROLOGUE();
uavcan_stm32::handleRxInterrupt(1, 1);
UAVCAN_STM32_IRQ_EPILOGUE();
}
UAVCAN_STM32_IRQ_HANDLER(CAN2_SCE_IRQHandler)
{
UAVCAN_STM32_IRQ_PROLOGUE();
uavcan_stm32::handleStatusInterrupt(1);
UAVCAN_STM32_IRQ_EPILOGUE();
}
#endif
} // extern "C"
libuavcan_drivers/stm32/test_stm32f107/Makefile
+1 -1
View File
@@ -10,7 +10,7 @@ PROJECT = uavcan_test_stm32f107
CPPSRC = src/main.cpp src/dummy.cpp
UDEFS = -DUAVCAN_STM32_CHIBIOS=1
UDEFS = -DUAVCAN_STM32_CHIBIOS=1 -DUAVCAN_STM32_DEBUG=1
#
# UAVCAN library
libuavcan_drivers/stm32/test_stm32f107/src/main.cpp
+16 -1
View File
@@ -11,6 +11,8 @@ namespace app
namespace
{
uavcan_stm32::CanInitHelper<> can;
void ledSet(bool state)
{
palWritePad(GPIO_PORT_LED, GPIO_PIN_LED, state);
@@ -22,7 +24,7 @@ int init()
chibios_rt::System::init();
sdStart(&STDOUT_SD, NULL);
return 0;
return can.init(1000000);
}
__attribute__((noreturn))
@@ -87,5 +89,18 @@ int main()
{
uavcan_stm32::SystemClock::instance().adjustUtc(uavcan::UtcDuration::fromMSec(10000));
}
uavcan::CanFrame frame;
const uavcan::MonotonicTime deadline =
uavcan_stm32::clock::getMonotonic() + uavcan::MonotonicDuration::fromMSec(10);
frame.id = 123 | uavcan::CanFrame::FlagEFF;
frame.data[0] = 42;
frame.dlc = 2;
const int send_res = static_cast<uavcan::ICanIface*>(app::can.driver.getIface(0))->send(frame, deadline, 0);
printf("send_res=%i errcnt=%lu hwerr=%i\n", send_res,
static_cast<unsigned long>(app::can.driver.getIface(0)->getErrorCount()),
static_cast<int>(app::can.driver.getIface(0)->yieldLastHardwareErrorCode()));
}
}