diff --git a/libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp b/libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp index 1efd057a54..3be68d8fc3 100644 --- a/libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp +++ b/libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp @@ -158,16 +158,24 @@ void adjustUtc(uavcan::UtcDuration adjustment) */ if (adjustment.isPositive()) { - if (utc_correction_usec_per_overflow_x16 < MaxUtcSpeedCorrectionX16) + if (utc_correction_usec_per_overflow_x16 < 0) { - utc_correction_usec_per_overflow_x16++; + utc_correction_usec_per_overflow_x16 += 4; + } + else if (utc_correction_usec_per_overflow_x16 < MaxUtcSpeedCorrectionX16) + { + utc_correction_usec_per_overflow_x16 += 1; } } else { - if (utc_correction_usec_per_overflow_x16 > -MaxUtcSpeedCorrectionX16) + if (utc_correction_usec_per_overflow_x16 > 0) { - utc_correction_usec_per_overflow_x16--; + utc_correction_usec_per_overflow_x16 -= 4; + } + else if (utc_correction_usec_per_overflow_x16 > -MaxUtcSpeedCorrectionX16) + { + utc_correction_usec_per_overflow_x16 -= 1; } }