fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-04 11:20:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Somewhat improved clock sync stability (the algorithm still has to be reimplemented from scratch)

Browse Source
This commit is contained in:
Pavel Kirienko
2014-04-25 10:45:39 +04:00
parent f28b00c5db
commit 32671fe574
1 changed files with 23 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp
+23 -5
View File
@@ -37,6 +37,7 @@ bool initialized = false;
bool utc_set = false;
// TODO: Clock speed adjustment is suboptimal, shall be reimplemented.
uavcan::uint32_t utc_jump_cnt = 0;
uavcan::int32_t utc_correction_usec_per_overflow_x16 = 0;
uavcan::int64_t prev_adjustment = 0;
@@ -47,7 +48,7 @@ uavcan::uint64_t time_mono = 0;
uavcan::uint64_t time_utc = 0;
const uavcan::uint32_t USecPerOverflow = 65536;
const uavcan::int32_t MaxUtcSpeedCorrectionX16 = 100 * 16;
const uavcan::int32_t MaxUtcSpeedCorrectionX16 = 30 * 16;
}
@@ -150,13 +151,12 @@ void adjustUtc(uavcan::UtcDuration adjustment)
/*
* Naive speed adjustment - discrete PI controller.
* TODO: More reliable clock speed adjustment algorithm
*/
const uavcan::int64_t adj_delta = adjustment.toUSec() - prev_adjustment; // This is the P term
const uavcan::int64_t adj_delta = adjustment.toUSec() - prev_adjustment;
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 += adjustment.isPositive() ? 1 : -1;
utc_correction_usec_per_overflow_x16 += (adj_delta > 0) ? 1 : -1;
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);
@@ -267,6 +267,24 @@ UAVCAN_STM32_IRQ_HANDLER(TIMX_IRQHandler)
{
// Values below 16 are ignored
time_utc += USecPerOverflow + (utc_correction_usec_per_overflow_x16 / 16);
// Correction slowly decays
static uavcan::uint8_t reductor;
if (reductor++ == 0)
{
if (utc_correction_usec_per_overflow_x16 > 0)
{
utc_correction_usec_per_overflow_x16--;
}
else if (utc_correction_usec_per_overflow_x16 < 0)
{
utc_correction_usec_per_overflow_x16++;
}
else
{
; // Nothing to do
}
}
}
UAVCAN_STM32_IRQ_EPILOGUE();