From e205c2e441d8679ca286e8e84cca22aa9b330732 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 15 Apr 2014 22:12:19 +0400 Subject: [PATCH] LPC11C24 clock driver. Not well tested yet, but generally seems to be OK --- .../driver/include/uavcan_lpc11c24/clock.hpp | 66 ++++++ libuavcan_drivers/lpc11c24/driver/src/can.cpp | 9 +- .../lpc11c24/driver/src/clock.cpp | 191 ++++++++++++++++++ .../lpc11c24/driver/src/internal.hpp | 12 ++ .../test_olimex_lpc_p11c24/src/main.cpp | 12 +- 5 files changed, 285 insertions(+), 5 deletions(-) create mode 100644 libuavcan_drivers/lpc11c24/driver/include/uavcan_lpc11c24/clock.hpp create mode 100644 libuavcan_drivers/lpc11c24/driver/src/clock.cpp diff --git a/libuavcan_drivers/lpc11c24/driver/include/uavcan_lpc11c24/clock.hpp b/libuavcan_drivers/lpc11c24/driver/include/uavcan_lpc11c24/clock.hpp new file mode 100644 index 0000000000..211f53cd86 --- /dev/null +++ b/libuavcan_drivers/lpc11c24/driver/include/uavcan_lpc11c24/clock.hpp @@ -0,0 +1,66 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#include + +namespace uavcan_lpc11c24 +{ +namespace clock +{ +/** + * Starts the clock. + * Can be called multiple times, only the first call will be effective. + */ +void init(); + +/** + * Returns current monotonic time passed since the moment when clock::init() was called. + * Note that both monotonic and UTC clocks are implemented using SysTick timer. + */ +uavcan::MonotonicTime getMonotonic(); + +/** + * Returns UTC time if it has been set, otherwise returns zero time. + * Note that both monotonic and UTC clocks are implemented using SysTick timer. + */ +uavcan::UtcTime getUtc(); + +/** + * Performs UTC time adjustment. + * The UTC time will be zero until first adjustment has been performed. + */ +void adjustUtc(uavcan::UtcDuration adjustment); + +/** + * Returns clock error sampled at previous UTC adjustment. + * Positive if the hardware timer is slower than reference time. + */ +uavcan::UtcDuration getPrevUtcAdjustment(); + +} + +/** + * Adapter for uavcan::ISystemClock. + */ +class SystemClock : public uavcan::ISystemClock, uavcan::Noncopyable +{ + static SystemClock self; + + SystemClock() { } + + virtual uavcan::MonotonicTime getMonotonic() const { return clock::getMonotonic(); } + virtual uavcan::UtcTime getUtc() const { return clock::getUtc(); } + virtual void adjustUtc(uavcan::UtcDuration adjustment) { clock::adjustUtc(adjustment); } + +public: + /** + * Calls clock::init() as needed. + * This function is thread safe. + */ + static SystemClock& instance(); +}; + +} diff --git a/libuavcan_drivers/lpc11c24/driver/src/can.cpp b/libuavcan_drivers/lpc11c24/driver/src/can.cpp index 1202f9d3ab..67d96d6704 100644 --- a/libuavcan_drivers/lpc11c24/driver/src/can.cpp +++ b/libuavcan_drivers/lpc11c24/driver/src/can.cpp @@ -3,6 +3,7 @@ */ #include +#include #include #include "internal.hpp" @@ -264,8 +265,8 @@ uavcan::int16_t CanDriver::send(const uavcan::CanFrame& frame, uavcan::Monotonic uavcan::int16_t CanDriver::receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic, uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags) { - out_ts_monotonic = uavcan::MonotonicTime(); // TODO: read monotonic - out_flags = 0; // We don't support any flags + out_ts_monotonic = uavcan_lpc11c24::clock::getMonotonic(); + out_flags = 0; // We don't support any IO flags CriticalSectionLocker locker; if (rx_queue.getLength() == 0) @@ -368,9 +369,9 @@ void canErrorCallback(uint32_t error_info) } } -void CAN_IRQHandler(void) +void CAN_IRQHandler() { - uavcan_lpc11c24::last_irq_utc_timestamp = 0; // TODO: Read UTC timestamp + uavcan_lpc11c24::last_irq_utc_timestamp = uavcan_lpc11c24::clock::getUtcUSecFromCanInterrupt(); LPC_CCAN_API->isr(); } diff --git a/libuavcan_drivers/lpc11c24/driver/src/clock.cpp b/libuavcan_drivers/lpc11c24/driver/src/clock.cpp new file mode 100644 index 0000000000..b2b4d528af --- /dev/null +++ b/libuavcan_drivers/lpc11c24/driver/src/clock.cpp @@ -0,0 +1,191 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include "internal.hpp" + +namespace uavcan_lpc11c24 +{ +namespace clock +{ +namespace +{ + +bool initialized = false; +bool utc_set = false; + +int32_t utc_correction_usec_per_overflow_x16 = 0; +int64_t prev_adjustment = 0; + +uint64_t time_mono = 0; +uint64_t time_utc = 0; + +/** + * If this value is too large for the given core clock, reload value will be out of the 24-bit integer range. + * This will be detected at run time during timer initialization - refer to SysTick_Config(). + */ +const uint32_t USecPerOverflow = 65536 * 2; +const int32_t MaxUtcSpeedCorrectionX16 = 100 * 16; + +} + +__attribute((noreturn)) +void fail() +{ + while (true) { } +} + +void init() +{ + CriticalSectionLocker lock; + if (!initialized) + { + initialized = true; + + if ((SystemCoreClock % 1000000) != 0) // Core clock frequency validation + { + fail(); + } + + if (SysTick_Config((SystemCoreClock / 1000000) * USecPerOverflow) != 0) + { + fail(); + } + } +} + +// Optimizer breaks this function. +__attribute__((optimize("0"))) +static uint64_t sampleFromCriticalSection(const volatile uint64_t* const value) +{ + const uint32_t reload = SysTick->LOAD; // SysTick counts downwards, hence the value subtracted from reload + volatile uint64_t time = *value; + volatile uint32_t cycles = reload - SysTick->VAL; + + if (NVIC_GetPendingIRQ(SysTick_IRQn)) + { + /* + * The timer has overflowed either before or after CNT sample was obtained. + * We need to sample it once more to be sure that the obtained + * counter value has wrapped over zero. + */ + cycles = reload - SysTick->VAL; + /* + * The timer interrupt was set, but not handled yet. + * Thus we need to adjust the tick counter manually. + */ + time += USecPerOverflow; + } + /* + * Raw counter value must be converted from core cycles to microseconds + */ + return time + (cycles / (SystemCoreClock / 1000000)); +} + +uint64_t getUtcUSecFromCanInterrupt() +{ + return utc_set ? sampleFromCriticalSection(&time_utc) : 0; +} + +uavcan::MonotonicTime getMonotonic() +{ + uint64_t usec = 0; + { + CriticalSectionLocker locker; + usec = sampleFromCriticalSection(&time_mono); + } + return uavcan::MonotonicTime::fromUSec(usec); +} + +uavcan::UtcTime getUtc() +{ + if (utc_set) + { + uint64_t usec = 0; + { + CriticalSectionLocker locker; + usec = sampleFromCriticalSection(&time_utc); + } + return uavcan::UtcTime::fromUSec(usec); + } + return uavcan::UtcTime(); +} + +uavcan::UtcDuration getPrevUtcAdjustment() +{ + return uavcan::UtcDuration::fromUSec(prev_adjustment); +} + +void adjustUtc(uavcan::UtcDuration adjustment) +{ + const int64_t adj_delta = adjustment.toUSec() - prev_adjustment; // This is the P term + prev_adjustment = adjustment.toUSec(); + + utc_correction_usec_per_overflow_x16 += adjustment.isPositive() ? 1 : -1; // I + utc_correction_usec_per_overflow_x16 += (adj_delta > 0) ? 1 : -1; // P + + utc_correction_usec_per_overflow_x16 = std::max(utc_correction_usec_per_overflow_x16, -MaxUtcSpeedCorrectionX16); + utc_correction_usec_per_overflow_x16 = std::min(utc_correction_usec_per_overflow_x16, MaxUtcSpeedCorrectionX16); + + if (adjustment.getAbs().toMSec() > 2 || !utc_set) + { + const int64_t adj_usec = adjustment.toUSec(); + { + CriticalSectionLocker locker; + if ((adj_usec < 0) && uint64_t(-adj_usec) > time_utc) + { + time_utc = 1; + } + else + { + time_utc += adj_usec; + } + } + if (!utc_set) + { + utc_set = true; + utc_correction_usec_per_overflow_x16 = 0; + } + } +} + +} // namespace clock + +SystemClock SystemClock::self; + +SystemClock& SystemClock::instance() +{ + clock::init(); + return self; +} + +} + +/* + * Timer interrupt handler + */ +extern "C" +{ + +__attribute__((optimize("0"))) +void SysTick_Handler() +{ + using namespace uavcan_lpc11c24::clock; + if (initialized) + { + time_mono += USecPerOverflow; + if (utc_set) + { + // Values below 16 are ignored + time_utc += USecPerOverflow + (utc_correction_usec_per_overflow_x16 / 16); + } + } + else + { + fail(); + } +} + +} diff --git a/libuavcan_drivers/lpc11c24/driver/src/internal.hpp b/libuavcan_drivers/lpc11c24/driver/src/internal.hpp index 649f290b62..29a458bf08 100644 --- a/libuavcan_drivers/lpc11c24/driver/src/internal.hpp +++ b/libuavcan_drivers/lpc11c24/driver/src/internal.hpp @@ -4,6 +4,8 @@ #pragma once +#include + namespace uavcan_lpc11c24 { @@ -23,4 +25,14 @@ struct CriticalSectionLocker } }; +/** + * Internal for the driver + */ +namespace clock +{ + +uint64_t getUtcUSecFromCanInterrupt(); + +} + } 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 1d4830a57f..28d24863f6 100644 --- a/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/main.cpp +++ b/libuavcan_drivers/lpc11c24/test_olimex_lpc_p11c24/src/main.cpp @@ -5,6 +5,7 @@ #include #include #include +#include #define ENFORCE(x) \ if ((x) == 0) { \ @@ -18,9 +19,18 @@ int main() while (true) { } } + uavcan_lpc11c24::clock::init(); + + uavcan::MonotonicTime prev_mono; + while (true) { - //for (volatile int i = 0; i < 2000000; i++) { __asm volatile ("nop"); } + const uavcan::MonotonicTime ts_mono = uavcan_lpc11c24::clock::getMonotonic(); + + while (prev_mono >= ts_mono) + { + } + prev_mono = ts_mono; board::setErrorLed(uavcan_lpc11c24::CanDriver::instance().getErrorCount() > 0);