Руслан Урядинский / libuavcan

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependents:   UAVCAN UAVCAN_Subscriber

libuavcan_drivers/lpc11c24/driver/src/clock.cpp@0:dfe6edabb8ec, 2018-04-14 (annotated)

Committer:
RuslanUrya
Date:
Sat Apr 14 10:25:32 2018 +0000
Revision:
0:dfe6edabb8ec
Initial commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
RuslanUrya 0:dfe6edabb8ec 1 /*
RuslanUrya 0:dfe6edabb8ec 2 * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
RuslanUrya 0:dfe6edabb8ec 3 */
RuslanUrya 0:dfe6edabb8ec 4
RuslanUrya 0:dfe6edabb8ec 5 #include <uavcan_lpc11c24/clock.hpp>
RuslanUrya 0:dfe6edabb8ec 6 #include <uavcan/util/templates.hpp>
RuslanUrya 0:dfe6edabb8ec 7 #include <chip.h>
RuslanUrya 0:dfe6edabb8ec 8 #include "internal.hpp"
RuslanUrya 0:dfe6edabb8ec 9
RuslanUrya 0:dfe6edabb8ec 10 namespace uavcan_lpc11c24
RuslanUrya 0:dfe6edabb8ec 11 {
RuslanUrya 0:dfe6edabb8ec 12 namespace clock
RuslanUrya 0:dfe6edabb8ec 13 {
RuslanUrya 0:dfe6edabb8ec 14 namespace
RuslanUrya 0:dfe6edabb8ec 15 {
RuslanUrya 0:dfe6edabb8ec 16
RuslanUrya 0:dfe6edabb8ec 17 bool initialized = false;
RuslanUrya 0:dfe6edabb8ec 18 bool utc_set = false;
RuslanUrya 0:dfe6edabb8ec 19
RuslanUrya 0:dfe6edabb8ec 20 std::int32_t utc_correction_usec_per_overflow_x16 = 0;
RuslanUrya 0:dfe6edabb8ec 21 std::int64_t prev_adjustment = 0;
RuslanUrya 0:dfe6edabb8ec 22
RuslanUrya 0:dfe6edabb8ec 23 std::uint64_t time_mono = 0;
RuslanUrya 0:dfe6edabb8ec 24 std::uint64_t time_utc = 0;
RuslanUrya 0:dfe6edabb8ec 25
RuslanUrya 0:dfe6edabb8ec 26 /**
RuslanUrya 0:dfe6edabb8ec 27 * If this value is too large for the given core clock, reload value will be out of the 24-bit integer range.
RuslanUrya 0:dfe6edabb8ec 28 * This will be detected at run time during timer initialization - refer to SysTick_Config().
RuslanUrya 0:dfe6edabb8ec 29 */
RuslanUrya 0:dfe6edabb8ec 30 constexpr std::uint32_t USecPerOverflow = 65536 * 2;
RuslanUrya 0:dfe6edabb8ec 31 constexpr std::int32_t MaxUtcSpeedCorrectionX16 = 100 * 16;
RuslanUrya 0:dfe6edabb8ec 32
RuslanUrya 0:dfe6edabb8ec 33 }
RuslanUrya 0:dfe6edabb8ec 34
RuslanUrya 0:dfe6edabb8ec 35 #if __GNUC__
RuslanUrya 0:dfe6edabb8ec 36 __attribute__((noreturn))
RuslanUrya 0:dfe6edabb8ec 37 #endif
RuslanUrya 0:dfe6edabb8ec 38 static void fail()
RuslanUrya 0:dfe6edabb8ec 39 {
RuslanUrya 0:dfe6edabb8ec 40 while (true) { }
RuslanUrya 0:dfe6edabb8ec 41 }
RuslanUrya 0:dfe6edabb8ec 42
RuslanUrya 0:dfe6edabb8ec 43 void init()
RuslanUrya 0:dfe6edabb8ec 44 {
RuslanUrya 0:dfe6edabb8ec 45 CriticalSectionLocker lock;
RuslanUrya 0:dfe6edabb8ec 46 if (!initialized)
RuslanUrya 0:dfe6edabb8ec 47 {
RuslanUrya 0:dfe6edabb8ec 48 initialized = true;
RuslanUrya 0:dfe6edabb8ec 49
RuslanUrya 0:dfe6edabb8ec 50 if ((SystemCoreClock % 1000000) != 0) // Core clock frequency validation
RuslanUrya 0:dfe6edabb8ec 51 {
RuslanUrya 0:dfe6edabb8ec 52 fail();
RuslanUrya 0:dfe6edabb8ec 53 }
RuslanUrya 0:dfe6edabb8ec 54
RuslanUrya 0:dfe6edabb8ec 55 if (SysTick_Config((SystemCoreClock / 1000000) * USecPerOverflow) != 0)
RuslanUrya 0:dfe6edabb8ec 56 {
RuslanUrya 0:dfe6edabb8ec 57 fail();
RuslanUrya 0:dfe6edabb8ec 58 }
RuslanUrya 0:dfe6edabb8ec 59 }
RuslanUrya 0:dfe6edabb8ec 60 }
RuslanUrya 0:dfe6edabb8ec 61
RuslanUrya 0:dfe6edabb8ec 62 static std::uint64_t sampleFromCriticalSection(const volatile std::uint64_t* const value)
RuslanUrya 0:dfe6edabb8ec 63 {
RuslanUrya 0:dfe6edabb8ec 64 const std::uint32_t reload = SysTick->LOAD + 1; // SysTick counts downwards, hence the value subtracted from reload
RuslanUrya 0:dfe6edabb8ec 65
RuslanUrya 0:dfe6edabb8ec 66 volatile std::uint64_t time = *value;
RuslanUrya 0:dfe6edabb8ec 67 volatile std::uint32_t cycles = reload - SysTick->VAL;
RuslanUrya 0:dfe6edabb8ec 68
RuslanUrya 0:dfe6edabb8ec 69 if ((SCB->ICSR & SCB_ICSR_PENDSTSET_Msk) == SCB_ICSR_PENDSTSET_Msk)
RuslanUrya 0:dfe6edabb8ec 70 {
RuslanUrya 0:dfe6edabb8ec 71 cycles = reload - SysTick->VAL;
RuslanUrya 0:dfe6edabb8ec 72 time += USecPerOverflow;
RuslanUrya 0:dfe6edabb8ec 73 }
RuslanUrya 0:dfe6edabb8ec 74 const std::uint32_t cycles_per_usec = SystemCoreClock / 1000000;
RuslanUrya 0:dfe6edabb8ec 75 return time + (cycles / cycles_per_usec);
RuslanUrya 0:dfe6edabb8ec 76 }
RuslanUrya 0:dfe6edabb8ec 77
RuslanUrya 0:dfe6edabb8ec 78 std::uint64_t getUtcUSecFromCanInterrupt()
RuslanUrya 0:dfe6edabb8ec 79 {
RuslanUrya 0:dfe6edabb8ec 80 return utc_set ? sampleFromCriticalSection(&time_utc) : 0;
RuslanUrya 0:dfe6edabb8ec 81 }
RuslanUrya 0:dfe6edabb8ec 82
RuslanUrya 0:dfe6edabb8ec 83 uavcan::MonotonicTime getMonotonic()
RuslanUrya 0:dfe6edabb8ec 84 {
RuslanUrya 0:dfe6edabb8ec 85 if (!initialized)
RuslanUrya 0:dfe6edabb8ec 86 {
RuslanUrya 0:dfe6edabb8ec 87 fail();
RuslanUrya 0:dfe6edabb8ec 88 }
RuslanUrya 0:dfe6edabb8ec 89 std::uint64_t usec = 0;
RuslanUrya 0:dfe6edabb8ec 90 {
RuslanUrya 0:dfe6edabb8ec 91 CriticalSectionLocker locker;
RuslanUrya 0:dfe6edabb8ec 92 usec = sampleFromCriticalSection(&time_mono);
RuslanUrya 0:dfe6edabb8ec 93 }
RuslanUrya 0:dfe6edabb8ec 94 return uavcan::MonotonicTime::fromUSec(usec);
RuslanUrya 0:dfe6edabb8ec 95 }
RuslanUrya 0:dfe6edabb8ec 96
RuslanUrya 0:dfe6edabb8ec 97 uavcan::UtcTime getUtc()
RuslanUrya 0:dfe6edabb8ec 98 {
RuslanUrya 0:dfe6edabb8ec 99 if (!initialized)
RuslanUrya 0:dfe6edabb8ec 100 {
RuslanUrya 0:dfe6edabb8ec 101 fail();
RuslanUrya 0:dfe6edabb8ec 102 }
RuslanUrya 0:dfe6edabb8ec 103 std::uint64_t usec = 0;
RuslanUrya 0:dfe6edabb8ec 104 if (utc_set)
RuslanUrya 0:dfe6edabb8ec 105 {
RuslanUrya 0:dfe6edabb8ec 106 CriticalSectionLocker locker;
RuslanUrya 0:dfe6edabb8ec 107 usec = sampleFromCriticalSection(&time_utc);
RuslanUrya 0:dfe6edabb8ec 108 }
RuslanUrya 0:dfe6edabb8ec 109 return uavcan::UtcTime::fromUSec(usec);
RuslanUrya 0:dfe6edabb8ec 110 }
RuslanUrya 0:dfe6edabb8ec 111
RuslanUrya 0:dfe6edabb8ec 112 uavcan::UtcDuration getPrevUtcAdjustment()
RuslanUrya 0:dfe6edabb8ec 113 {
RuslanUrya 0:dfe6edabb8ec 114 return uavcan::UtcDuration::fromUSec(prev_adjustment);
RuslanUrya 0:dfe6edabb8ec 115 }
RuslanUrya 0:dfe6edabb8ec 116
RuslanUrya 0:dfe6edabb8ec 117 void adjustUtc(uavcan::UtcDuration adjustment)
RuslanUrya 0:dfe6edabb8ec 118 {
RuslanUrya 0:dfe6edabb8ec 119 const std::int64_t adj_delta = adjustment.toUSec() - prev_adjustment; // This is the P term
RuslanUrya 0:dfe6edabb8ec 120 prev_adjustment = adjustment.toUSec();
RuslanUrya 0:dfe6edabb8ec 121
RuslanUrya 0:dfe6edabb8ec 122 utc_correction_usec_per_overflow_x16 += adjustment.isPositive() ? 1 : -1; // I
RuslanUrya 0:dfe6edabb8ec 123 utc_correction_usec_per_overflow_x16 += (adj_delta > 0) ? 1 : -1; // P
RuslanUrya 0:dfe6edabb8ec 124
RuslanUrya 0:dfe6edabb8ec 125 utc_correction_usec_per_overflow_x16 =
RuslanUrya 0:dfe6edabb8ec 126 uavcan::max(utc_correction_usec_per_overflow_x16, -MaxUtcSpeedCorrectionX16);
RuslanUrya 0:dfe6edabb8ec 127 utc_correction_usec_per_overflow_x16 =
RuslanUrya 0:dfe6edabb8ec 128 uavcan::min(utc_correction_usec_per_overflow_x16, MaxUtcSpeedCorrectionX16);
RuslanUrya 0:dfe6edabb8ec 129
RuslanUrya 0:dfe6edabb8ec 130 if (adjustment.getAbs().toMSec() > 9 || !utc_set)
RuslanUrya 0:dfe6edabb8ec 131 {
RuslanUrya 0:dfe6edabb8ec 132 const std::int64_t adj_usec = adjustment.toUSec();
RuslanUrya 0:dfe6edabb8ec 133 {
RuslanUrya 0:dfe6edabb8ec 134 CriticalSectionLocker locker;
RuslanUrya 0:dfe6edabb8ec 135 if ((adj_usec < 0) && std::uint64_t(-adj_usec) > time_utc)
RuslanUrya 0:dfe6edabb8ec 136 {
RuslanUrya 0:dfe6edabb8ec 137 time_utc = 1;
RuslanUrya 0:dfe6edabb8ec 138 }
RuslanUrya 0:dfe6edabb8ec 139 else
RuslanUrya 0:dfe6edabb8ec 140 {
RuslanUrya 0:dfe6edabb8ec 141 time_utc = std::uint64_t(std::int64_t(time_utc) + adj_usec);
RuslanUrya 0:dfe6edabb8ec 142 }
RuslanUrya 0:dfe6edabb8ec 143 }
RuslanUrya 0:dfe6edabb8ec 144 if (!utc_set)
RuslanUrya 0:dfe6edabb8ec 145 {
RuslanUrya 0:dfe6edabb8ec 146 utc_set = true;
RuslanUrya 0:dfe6edabb8ec 147 utc_correction_usec_per_overflow_x16 = 0;
RuslanUrya 0:dfe6edabb8ec 148 }
RuslanUrya 0:dfe6edabb8ec 149 }
RuslanUrya 0:dfe6edabb8ec 150 }
RuslanUrya 0:dfe6edabb8ec 151
RuslanUrya 0:dfe6edabb8ec 152 } // namespace clock
RuslanUrya 0:dfe6edabb8ec 153
RuslanUrya 0:dfe6edabb8ec 154 SystemClock SystemClock::self;
RuslanUrya 0:dfe6edabb8ec 155
RuslanUrya 0:dfe6edabb8ec 156 SystemClock& SystemClock::instance()
RuslanUrya 0:dfe6edabb8ec 157 {
RuslanUrya 0:dfe6edabb8ec 158 clock::init();
RuslanUrya 0:dfe6edabb8ec 159 return self;
RuslanUrya 0:dfe6edabb8ec 160 }
RuslanUrya 0:dfe6edabb8ec 161
RuslanUrya 0:dfe6edabb8ec 162 }
RuslanUrya 0:dfe6edabb8ec 163
RuslanUrya 0:dfe6edabb8ec 164 /*
RuslanUrya 0:dfe6edabb8ec 165 * Timer interrupt handler
RuslanUrya 0:dfe6edabb8ec 166 */
RuslanUrya 0:dfe6edabb8ec 167 extern "C"
RuslanUrya 0:dfe6edabb8ec 168 {
RuslanUrya 0:dfe6edabb8ec 169
RuslanUrya 0:dfe6edabb8ec 170 void SysTick_Handler();
RuslanUrya 0:dfe6edabb8ec 171
RuslanUrya 0:dfe6edabb8ec 172 void SysTick_Handler()
RuslanUrya 0:dfe6edabb8ec 173 {
RuslanUrya 0:dfe6edabb8ec 174 using namespace uavcan_lpc11c24::clock;
RuslanUrya 0:dfe6edabb8ec 175 if (initialized)
RuslanUrya 0:dfe6edabb8ec 176 {
RuslanUrya 0:dfe6edabb8ec 177 time_mono += USecPerOverflow;
RuslanUrya 0:dfe6edabb8ec 178 if (utc_set)
RuslanUrya 0:dfe6edabb8ec 179 {
RuslanUrya 0:dfe6edabb8ec 180 // Values below 16 are ignored
RuslanUrya 0:dfe6edabb8ec 181 time_utc += std::uint64_t(std::int32_t(USecPerOverflow) + (utc_correction_usec_per_overflow_x16 / 16));
RuslanUrya 0:dfe6edabb8ec 182 }
RuslanUrya 0:dfe6edabb8ec 183 }
RuslanUrya 0:dfe6edabb8ec 184 else
RuslanUrya 0:dfe6edabb8ec 185 {
RuslanUrya 0:dfe6edabb8ec 186 fail();
RuslanUrya 0:dfe6edabb8ec 187 }
RuslanUrya 0:dfe6edabb8ec 188 }
RuslanUrya 0:dfe6edabb8ec 189
RuslanUrya 0:dfe6edabb8ec 190 }