From 941f936197762a61adb89b011f8848398be4147f Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 5 Apr 2014 13:19:00 +0400 Subject: [PATCH] STM32: Driver multiplexing, fixed ChibiOS Event --- .../stm32/driver/include/uavcan_stm32/can.hpp | 2 + .../stm32/driver/src/uc_stm32_can.cpp | 43 +++++++++- .../stm32/driver/src/uc_stm32_thread.cpp | 4 +- .../stm32/test_stm32f107/src/main.cpp | 86 ++++++++++++------- 4 files changed, 98 insertions(+), 37 deletions(-) diff --git a/libuavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp b/libuavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp index 1bf3ab3f57..8ddbc9e719 100644 --- a/libuavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp +++ b/libuavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp @@ -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: diff --git a/libuavcan_drivers/stm32/driver/src/uc_stm32_can.cpp b/libuavcan_drivers/stm32/driver/src/uc_stm32_can.cpp index d7116bcc0c..bdf1ee3678 100644 --- a/libuavcan_drivers/stm32/driver/src/uc_stm32_can.cpp +++ b/libuavcan_drivers/stm32/driver/src/uc_stm32_can.cpp @@ -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) diff --git a/libuavcan_drivers/stm32/driver/src/uc_stm32_thread.cpp b/libuavcan_drivers/stm32/driver/src/uc_stm32_thread.cpp index 380f2e3513..374f1cf4b0 100644 --- a/libuavcan_drivers/stm32/driver/src/uc_stm32_thread.cpp +++ b/libuavcan_drivers/stm32/driver/src/uc_stm32_thread.cpp @@ -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; } diff --git a/libuavcan_drivers/stm32/test_stm32f107/src/main.cpp b/libuavcan_drivers/stm32/test_stm32f107/src/main.cpp index 263052df09..6daa0e5cab 100644 --- a/libuavcan_drivers/stm32/test_stm32f107/src/main.cpp +++ b/libuavcan_drivers/stm32/test_stm32f107/src/main.cpp @@ -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(uavcan_stm32::SystemClock::instance().getMonotonic().toUSec()), static_cast(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(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(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(select_duration.toMSec()), static_cast(app::can.driver.getIface(0)->getErrorCount()), - static_cast(app::can.driver.getIface(0)->yieldLastHardwareErrorCode())); + static_cast(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(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(app::can.driver.getIface(0))->send(frame, deadline, + uavcan::CanIOFlagLoopback); + printf("send_res=%i errcnt=%lu hwerr=%i\n", send_res, + static_cast(app::can.driver.getIface(0)->getErrorCount()), + static_cast(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(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; + } + } } } }