diff --git a/libuavcan_drivers/lpc11c24/driver/src/c_can.hpp b/libuavcan_drivers/lpc11c24/driver/src/c_can.hpp index e5f1987ee1..9354941f88 100644 --- a/libuavcan_drivers/lpc11c24/driver/src/c_can.hpp +++ b/libuavcan_drivers/lpc11c24/driver/src/c_can.hpp @@ -98,7 +98,7 @@ static_assert(offsetof(Type, IF[0].DB2) == 0x048, "C_CAN offset"); static_assert(offsetof(Type, IF[1].DB2) == 0x0A8, "C_CAN offset"); -Type& Can = *reinterpret_cast(0x40050000); +Type& CAN = *reinterpret_cast(0x40050000); /* @@ -125,10 +125,10 @@ static constexpr std::uint16_t TEST_TX_SHIFT = 5; enum class TestTx : std::uint16_t { - Controller = 0, - SamplePoint = 1, - Low = 2, - High = 3 + Controller = 0, + SamplePoint = 1, + LowDominant = 2, + HighRecessive = 3 }; /* @@ -142,6 +142,18 @@ static constexpr std::uint16_t STAT_TXOK = 1 << 3; static constexpr std::uint16_t STAT_LEC_MASK = 7; static constexpr std::uint16_t STAT_LEC_SHIFT = 0; +enum class StatLec : std::uint16_t +{ + NoError = 0, + StuffError = 1, + FormError = 2, + AckError = 3, + Bit1Error = 4, + Bit0Error = 5, + CRCError = 6, + Unused = 7 +}; + /* * IF.MCTRL */ diff --git a/libuavcan_drivers/lpc11c24/driver/src/can.cpp b/libuavcan_drivers/lpc11c24/driver/src/can.cpp index c60ead46ab..2b263f9572 100644 --- a/libuavcan_drivers/lpc11c24/driver/src/can.cpp +++ b/libuavcan_drivers/lpc11c24/driver/src/can.cpp @@ -183,28 +183,46 @@ uavcan::uint32_t CanDriver::detectBitRate(void (*idle_callback)()) { CriticalSectionLocker locker; - c_can::Can.CNTL = c_can::CNTL_DAR | c_can::CNTL_CCE; + c_can::CAN.CNTL = c_can::CNTL_INIT | c_can::CNTL_DAR | c_can::CNTL_CCE | c_can::CNTL_TEST; - c_can::Can.BT = bit_timings.canbtr; - c_can::Can.CLKDIV = bit_timings.canclkdiv; + c_can::CAN.BT = bit_timings.canbtr; + c_can::CAN.CLKDIV = bit_timings.canclkdiv; - c_can::Can.TEST = c_can::TEST_SILENT; + c_can::CAN.TEST = c_can::TEST_SILENT | (unsigned(c_can::TestTx::HighRecessive) << c_can::TEST_TX_SHIFT); - c_can::Can.CNTL = c_can::CNTL_DAR; + c_can::CAN.STAT = (unsigned(c_can::StatLec::Unused) << c_can::STAT_LEC_SHIFT); + + c_can::CAN.CNTL = c_can::CNTL_DAR | c_can::CNTL_TEST; } // Listening const auto deadline = clock::getMonotonic() + ListeningDuration; + bool match_detected = false; while (clock::getMonotonic() < deadline) { if (idle_callback != nullptr) { idle_callback(); } + + if ((c_can::CAN.STAT >> c_can::STAT_LEC_SHIFT) == unsigned(c_can::StatLec::NoError)) + { + match_detected = true; + break; + } + } + + // De-configuring the CAN controller back to reset state + c_can::CAN.CNTL = c_can::CNTL_INIT; + + // Termination condition + if (match_detected) + { + return bitrate; } } - return 0; + return 0; // No match } int CanDriver::init(uavcan::uint32_t bitrate) diff --git a/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/main.cpp b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/main.cpp index 03f84f331f..a80f832a79 100644 --- a/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/main.cpp +++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/main.cpp @@ -13,11 +13,42 @@ namespace { -typedef uavcan::Node<2800> Node; +static constexpr unsigned NodeMemoryPoolSize = 2800; -Node& getNode() +/** + * This is a compact, reentrant and thread-safe replacement to standard llto(). + * It returns the string by value, no extra storage is needed. + */ +typename uavcan::MakeString<22>::Type intToString(long long n) { - static Node node(uavcan_lpc11c24::CanDriver::instance(), uavcan_lpc11c24::SystemClock::instance()); + char buf[24] = {}; + const short sign = (n < 0) ? -1 : 1; + if (sign < 0) + { + n = -n; + } + unsigned pos = 0; + do + { + buf[pos++] = char(n % 10 + '0'); + } + while ((n /= 10) > 0); + if (sign < 0) + { + buf[pos++] = '-'; + } + buf[pos] = '\0'; + for (unsigned i = 0, j = pos - 1U; i < j; i++, j--) + { + std::swap(buf[i], buf[j]); + } + return static_cast(buf); +} + +uavcan::Node& getNode() +{ + static uavcan::Node node(uavcan_lpc11c24::CanDriver::instance(), + uavcan_lpc11c24::SystemClock::instance()); return node; } @@ -33,14 +64,6 @@ uavcan::Logger& getLogger() return logger; } -#if __GNUC__ -__attribute__((noreturn)) -#endif -void die() -{ - while (true) { } -} - #if __GNUC__ __attribute__((noinline)) #endif @@ -49,15 +72,38 @@ void init() board::resetWatchdog(); board::syslog("Boot\r\n"); - if (uavcan_lpc11c24::CanDriver::instance().init(1000000) < 0) + board::setErrorLed(false); + board::setStatusLed(true); + + /* + * Configuring the clock - this must be done before the CAN controller is initialized + */ + uavcan_lpc11c24::clock::init(); + + /* + * Configuring the CAN controller + */ + std::uint32_t bit_rate = 0; + while (bit_rate == 0) { - die(); + board::syslog("CAN bitrate detection...\r\n"); + bit_rate = uavcan_lpc11c24::CanDriver::detectBitRate(&board::resetWatchdog); + } + board::syslog("CAN bitrate: "); + board::syslog(intToString(bit_rate).c_str()); + board::syslog("\r\n"); + + if (uavcan_lpc11c24::CanDriver::instance().init(bit_rate) < 0) + { + board::die(); } board::syslog("CAN init ok\r\n"); board::resetWatchdog(); - getNode().setNodeID(72); + /* + * Configuring the node + */ getNode().setName("org.uavcan.lpc11c24_test"); uavcan::protocol::SoftwareVersion swver; @@ -75,6 +121,14 @@ void init() board::resetWatchdog(); + /* + * Performing dynamic node ID allocation + */ + getNode().setNodeID(72); // TODO + + /* + * Starting the node + */ while (getNode().start() < 0) { } @@ -93,37 +147,6 @@ void init() board::resetWatchdog(); } -void reverse(char* s) -{ - for (int i = 0, j = int(std::strlen(s)) - 1; i < j; i++, j--) - { - const char c = s[i]; - s[i] = s[j]; - s[j] = c; - } -} - -void lltoa(long long n, char buf[24]) -{ - const short sign = (n < 0) ? -1 : 1; - if (sign < 0) - { - n = -n; - } - unsigned i = 0; - do - { - buf[i++] = char(n % 10 + '0'); - } - while ((n /= 10) > 0); - if (sign < 0) - { - buf[i++] = '-'; - } - buf[i] = '\0'; - reverse(buf); -} - } int main() @@ -144,17 +167,12 @@ int main() if ((ts - prev_log_at).toMSec() >= 1000) { prev_log_at = ts; - // We don't want to use formatting functions provided by libuavcan because they rely on std::snprintf() - char buf[24]; - lltoa(uavcan_lpc11c24::clock::getPrevUtcAdjustment().toUSec(), buf); - buf[sizeof(buf) - 1] = '\0'; - - // ...hence we need to construct the message manually: + // hence we need to construct the message manually: uavcan::protocol::debug::LogMessage logmsg; logmsg.level.value = uavcan::protocol::debug::LogLevel::INFO; logmsg.source = "app"; - logmsg.text = buf; + logmsg.text = intToString(uavcan_lpc11c24::clock::getPrevUtcAdjustment().toUSec()).c_str(); (void)getLogger().log(logmsg); } diff --git a/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/sys/board.cpp b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/sys/board.cpp index 169dd35758..63d4f8ac54 100644 --- a/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/sys/board.cpp +++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/sys/board.cpp @@ -132,6 +132,11 @@ void init() } // namespace +void die() +{ + while (true) { } +} + #if __GNUC__ __attribute__((optimize(0))) // Optimization must be disabled lest it hardfaults in the IAP call #endif diff --git a/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/sys/board.hpp b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/sys/board.hpp index ab58c7da0a..cdb84fe331 100644 --- a/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/sys/board.hpp +++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/sys/board.hpp @@ -7,6 +7,11 @@ namespace board { +#if __GNUC__ +__attribute__((noreturn)) +#endif +void die(); + static constexpr unsigned UniqueIDSize = 16; void readUniqueID(std::uint8_t out_uid[UniqueIDSize]);