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

STM32: RX

Browse Source
This commit is contained in:
Pavel Kirienko 2014-04-05 01:39:51 +04:00
parent 38f57f343d
commit e04a32662e
3 changed files with 130 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
libuavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp
View File

@ -111,6 +111,7 @@ class CanIface : public uavcan::ICanIface, uavcan::Noncopyable
Event& update_event_;
TxItem pending_tx_[NumTxMailboxes];
uavcan::uint8_t last_hw_error_code_;
const uavcan::uint8_t self_index_;
int computeTimings(uavcan::uint32_t target_bitrate, Timings& out_timings);
@ -130,13 +131,17 @@ class CanIface : public uavcan::ICanIface, uavcan::Noncopyable
public:
enum { MaxRxQueueCapacity = 254 };
CanIface(CAN_TypeDef* can, Event& update_event, CanRxItem* rx_queue_buffer, uavcan::uint8_t rx_queue_capacity)
CanIface(CAN_TypeDef* can, Event& update_event, uavcan::uint8_t self_index,
CanRxItem* rx_queue_buffer, uavcan::uint8_t rx_queue_capacity)
: rx_queue_(rx_queue_buffer, rx_queue_capacity)
, can_(can)
, error_cnt_(0)
, update_event_(update_event)
, last_hw_error_code_(0)
{ }
, self_index_(self_index)
{
assert(self_index_ < UAVCAN_STM32_NUM_IFACES);
}
/**
* Assumes:
@ -176,9 +181,9 @@ class CanDriver : public uavcan::ICanDriver, uavcan::Noncopyable
public:
template <unsigned RxQueueCapacity>
CanDriver(CanRxItem (&rx_queue_storage)[UAVCAN_STM32_NUM_IFACES][RxQueueCapacity])
: if0_(CAN1, update_event_, rx_queue_storage[0], RxQueueCapacity)
: if0_(CAN1, update_event_, 0, rx_queue_storage[0], RxQueueCapacity)
#if UAVCAN_STM32_NUM_IFACES > 1
, if1_(CAN2, update_event_, rx_queue_storage[1], RxQueueCapacity)
, if1_(CAN2, update_event_, 1, rx_queue_storage[1], RxQueueCapacity)
#endif
{
uavcan::StaticAssert<(RxQueueCapacity <= CanIface::MaxRxQueueCapacity)>::check();

109
libuavcan_drivers/stm32/driver/src/uc_stm32_can.cpp
View File

@ -264,17 +264,24 @@ uavcan::int16_t CanIface::send(const uavcan::CanFrame& frame, uavcan::MonotonicT
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;
out_ts_monotonic = clock::getMonotonic(); // High precision is not required for monotonic timestamps
uavcan::uint64_t utc_usec = 0;
{
CriticalSectionLock lock;
if (rx_queue_.getLength() == 0)
{
return 0;
}
rx_queue_.pop(out_frame, utc_usec, out_flags);
}
out_ts_utc = uavcan::UtcTime::fromUSec(utc_usec);
return 1;
}
uavcan::int16_t CanIface::configureFilters(const uavcan::CanFilterConfig* filter_configs,
uavcan::uint16_t num_configs)
{
// TODO: Hardware filter support
CriticalSectionLock lock;
(void)filter_configs;
(void)num_configs;
@ -324,8 +331,6 @@ int CanIface::init(uavcan::uint32_t bitrate)
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
@ -344,6 +349,36 @@ int CanIface::init(uavcan::uint32_t bitrate)
goto leave;
}
/*
* Default filter configuration
*/
if (self_index_ == 0)
{
can_->FMR |= CAN_FMR_FINIT;
can_->FMR &= 0xFFFFC0F1;
can_->FMR |= static_cast<uavcan::uint32_t>(NumFilters) << 8; // Slave (CAN2) gets half of the filters
can_->FFA1R = 0; // All assigned to FIFO0 by default
can_->FM1R = 0; // Indentifier Mask mode
#if UAVCAN_STM32_NUM_IFACES > 1
can_->FS1R = 0x7ffffff; // Single 32-bit for all
can_->sFilterRegister[0].FR1 = 0; // CAN1 accepts everything
can_->sFilterRegister[0].FR2 = 0;
can_->sFilterRegister[NumFilters].FR1 = 0; // CAN2 accepts everything
can_->sFilterRegister[NumFilters].FR2 = 0;
can_->FA1R = 1 | (1 << NumFilters); // One filter per each iface
#else
can_->FS1R = 0x1fff;
can_->sFilterRegister[0].FR1 = 0;
can_->sFilterRegister[0].FR2 = 0;
can_->FA1R = 1;
#endif
can_->FMR &= ~CAN_FMR_FINIT;
}
leave:
return res;
}
@ -385,9 +420,61 @@ void CanIface::handleTxInterrupt(const uavcan::uint64_t utc_usec)
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;
assert(fifo_index < 2);
volatile uavcan::uint32_t* const rfr_reg = (fifo_index == 0) ? &can_->RF0R : &can_->RF1R;
if ((*rfr_reg & CAN_RF0R_FMP0) == 0)
{
assert(0); // Weird, IRQ is here but no data to read
return;
}
/*
* Register overflow as a hardware error
*/
if ((*rfr_reg & CAN_RF0R_FOVR0) != 0)
{
error_cnt_++;
}
/*
* Read the frame contents
*/
uavcan::CanFrame frame;
const CAN_FIFOMailBox_TypeDef& rf = can_->sFIFOMailBox[fifo_index];
if ((rf.RIR & CAN_RI0R_IDE) == 0)
{
frame.id = uavcan::CanFrame::MaskStdID & (rf.RIR >> 21);
}
else
{
frame.id = uavcan::CanFrame::MaskExtID & (rf.RIR >> 3);
frame.id |= uavcan::CanFrame::FlagEFF;
}
if ((rf.RIR & CAN_RI0R_RTR) != 0)
{
frame.id |= uavcan::CanFrame::FlagRTR;
}
frame.dlc = rf.RDTR & 15;
frame.data[0] = 0xFF & (rf.RDLR >> 0);
frame.data[1] = 0xFF & (rf.RDLR >> 8);
frame.data[2] = 0xFF & (rf.RDLR >> 16);
frame.data[3] = 0xFF & (rf.RDLR >> 24);
frame.data[4] = 0xFF & (rf.RDHR >> 0);
frame.data[5] = 0xFF & (rf.RDHR >> 8);
frame.data[6] = 0xFF & (rf.RDHR >> 16);
frame.data[7] = 0xFF & (rf.RDHR >> 24);
*rfr_reg = CAN_RF0R_RFOM0 | CAN_RF0R_FOVR0 | CAN_RF0R_FULL0; // Release FIFO entry we just read
/*
* Store with timeout into the FIFO buffer and signal update event
*/
rx_queue_.push(frame, utc_usec, 0);
update_event_.signalFromInterrupt();
}

24
libuavcan_drivers/stm32/test_stm32f107/src/main.cpp
View File

@ -98,9 +98,31 @@ int main()
frame.data[0] = 42;
frame.dlc = 2;
const int send_res = static_cast<uavcan::ICanIface*>(app::can.driver.getIface(0))->send(frame, deadline, 0);
const int send_res =
static_cast<uavcan::ICanIface*>(app::can.driver.getIface(0))->send(frame, deadline,
uavcan::CanIOFlagLoopback);
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()));
while (true)
{
uavcan::CanRxFrame rx_frame;
uavcan::CanIOFlags flags = 0;
const int recv_res =
static_cast<uavcan::ICanIface*>(app::can.driver.getIface(0))->receive(rx_frame, rx_frame.ts_mono,
rx_frame.ts_utc, flags);
printf("recv_res=%i flg=%u canid=%lu dlc=%u data=[%02x %02x %02x %02x %02x %02x %02x %02x]\n",
recv_res, unsigned(flags), rx_frame.id & uavcan::CanFrame::MaskExtID, int(rx_frame.dlc),
int(rx_frame.data[0]), int(rx_frame.data[1]), int(rx_frame.data[2]), int(rx_frame.data[3]),
int(rx_frame.data[4]), int(rx_frame.data[5]), int(rx_frame.data[6]), int(rx_frame.data[7]));
if (recv_res <= 0)
{
break;
}
}
}
}