diff --git a/libuavcan_drivers/stm32/driver/include/uavcan_stm32/clock.hpp b/libuavcan_drivers/stm32/driver/include/uavcan_stm32/clock.hpp index 6835231b59..ca28bc5d0e 100644 --- a/libuavcan_drivers/stm32/driver/include/uavcan_stm32/clock.hpp +++ b/libuavcan_drivers/stm32/driver/include/uavcan_stm32/clock.hpp @@ -57,6 +57,13 @@ uavcan::uint32_t getUtcAjdustmentJumpCount(); */ uavcan::UtcDuration getPrevUtcAdjustment(); +/** + * Sets minimum absolute time error to perform non-gradual jump adjustment rather than speed change. + * The parameter must be positive. + * This function is thread safe. + */ +void setMinJump(uavcan::UtcDuration adj); + } /** diff --git a/libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp b/libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp index 9d783eeff8..5a8d0b8685 100644 --- a/libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp +++ b/libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp @@ -41,6 +41,8 @@ uavcan::uint32_t utc_jump_cnt = 0; uavcan::int32_t utc_correction_usec_per_overflow_x16 = 0; uavcan::int64_t prev_adjustment = 0; +uavcan::UtcDuration min_jump = uavcan::UtcDuration::fromMSec(3); + uavcan::uint64_t time_mono = 0; uavcan::uint64_t time_utc = 0; @@ -161,9 +163,9 @@ void adjustUtc(uavcan::UtcDuration adjustment) /* * Clock value adjustment - * For small adjustments (less than 3 msec) we will rely only on speed change + * For small adjustments we will rely only on speed change */ - if (adjustment.getAbs().toMSec() > 2 || !utc_set) + if (adjustment.getAbs() > min_jump || !utc_set) { const uavcan::int64_t adj_usec = adjustment.toUSec(); @@ -209,6 +211,19 @@ uavcan::UtcDuration getPrevUtcAdjustment() return uavcan::UtcDuration::fromUSec(prev_adjustment); } +void setMinJump(uavcan::UtcDuration adj) +{ + MutexLocker mlocker(mutex); + if (adj.isPositive()) + { + min_jump = adj; + } + else + { + assert(0); + } +} + } // namespace clock SystemClock& SystemClock::instance()