fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-17 22:53:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

STM32: Driver multiplexing, fixed ChibiOS Event

Browse Source
This commit is contained in:
Pavel Kirienko
2014-04-05 13:19:00 +04:00
parent 9e197f129b
commit 941f936197
4 changed files with 98 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libuavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp
+2
View File
@@ -176,6 +176,8 @@ class CanDriver : public uavcan::ICanDriver, uavcan::Noncopyable
CanIface if1_;
#endif
uavcan::CanSelectMasks makeSelectMasks() const;
virtual uavcan::int16_t select(uavcan::CanSelectMasks& inout_masks, uavcan::MonotonicTime blocking_deadline);
public:
libuavcan_drivers/stm32/driver/src/uc_stm32_can.cpp
+39 -4
View File
@@ -525,11 +525,46 @@ uavcan::uint8_t CanIface::yieldLastHardwareErrorCode()
/*
* CanDriver
*/
uavcan::int16_t CanDriver::select(uavcan::CanSelectMasks& inout_masks, uavcan::MonotonicTime blocking_deadline)
uavcan::CanSelectMasks CanDriver::makeSelectMasks() const
{
(void)inout_masks;
(void)blocking_deadline;
return -1;
uavcan::CanSelectMasks msk;
// Iface 0
msk.read = if0_.isRxBufferEmpty() ? 0 : 1;
msk.write = if0_.isTxBufferFull() ? 0 : 1;
// Iface 1
#if UAVCAN_STM32_NUM_IFACES > 1
if (!if1_.isRxBufferEmpty())
{
msk.read |= 1 << 1;
}
if (!if1_.isTxBufferFull())
{
msk.write |= 1 << 1;
}
#endif
return msk;
}
uavcan::int16_t CanDriver::select(uavcan::CanSelectMasks& inout_masks, const uavcan::MonotonicTime blocking_deadline)
{
const uavcan::CanSelectMasks in_masks = inout_masks;
const uavcan::MonotonicTime time = clock::getMonotonic();
if0_.discardTimedOutTxMailboxes(time); // Check TX timeouts - this may release some TX slots
#if UAVCAN_STM32_NUM_IFACES > 1
if1_.discardTimedOutTxMailboxes(time);
#endif
inout_masks = makeSelectMasks(); // Check if we already have some of the requested events
if ((inout_masks.read & in_masks.read) != 0 ||
(inout_masks.write & in_masks.write) != 0)
{
return 1;
}
(void)update_event_.wait(blocking_deadline - time); // Block until timeout expires or any iface updates
inout_masks = makeSelectMasks(); // Return what we got even if none of the requested events became signaled
return 1; // Return value doesn't matter as long as it is non-negative
}
int CanDriver::init(uavcan::uint32_t bitrate)
libuavcan_drivers/stm32/driver/src/uc_stm32_thread.cpp
+2 -2
View File
@@ -14,13 +14,13 @@ namespace uavcan_stm32
bool Event::wait(uavcan::MonotonicDuration duration)
{
msg_t ret = msg_t();
if (duration.isZero())
if (!duration.isPositive())
{
sem_.waitTimeout(TIME_IMMEDIATE);
}
else
{
sem_.waitTimeout(US2ST(duration.toUSec()));
sem_.waitTimeout(MS2ST(duration.toMSec()));
}
return ret == RDY_OK;
}
libuavcan_drivers/stm32/test_stm32f107/src/main.cpp
+55 -31
View File
@@ -71,13 +71,10 @@ int main()
puts("Hello world");
unsigned iter_counter = 0;
while (true)
{
sleep(1);
app::ledSet(true);
sleep(1);
app::led_turn_off_event.signal();
iter_counter++;
printf("Mono clock: %lu %lu\n",
static_cast<unsigned long>(uavcan_stm32::SystemClock::instance().getMonotonic().toUSec()),
static_cast<unsigned long>(uavcan_stm32::SystemClock::instance().getMonotonic().toMSec()));
@@ -90,38 +87,65 @@ 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);
app::led_turn_off_event.signal();
frame.id = 123 | uavcan::CanFrame::FlagEFF;
frame.data[0] = 42;
frame.dlc = 2;
sleep(1);
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,
const uavcan::MonotonicTime before_select = uavcan_stm32::clock::getMonotonic();
uavcan::CanSelectMasks masks; // Zero masks - always blocks
(void)static_cast<uavcan::ICanDriver&>(app::can.driver).select(masks,
uavcan_stm32::clock::getMonotonic() + uavcan::MonotonicDuration::fromMSec(10000));
const uavcan::MonotonicDuration select_duration = uavcan_stm32::clock::getMonotonic() - before_select;
printf("Select duration: %lu msec errs=%lu/%lu\n",
static_cast<unsigned long>(select_duration.toMSec()),
static_cast<unsigned long>(app::can.driver.getIface(0)->getErrorCount()),
static_cast<int>(app::can.driver.getIface(0)->yieldLastHardwareErrorCode()));
static_cast<unsigned long>(app::can.driver.getIface(1)->getErrorCount()));
while (true)
app::ledSet(true);
for (int i = 0; i < 2; i++)
{
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)
if ((iter_counter % 3 == 0) && (masks.write & (1 << i)))
{
break;
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,
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()));
}
if (masks.read & (1 << i))
{
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;
}
}
}
}
}