fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-27 08:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

STM32: clock sync: Simple PI controller for speed adjustments; converges to +-100 usec in few minutes

Browse Source
This commit is contained in:
Pavel Kirienko
2014-04-12 21:21:02 +04:00
parent 67f92628a8
commit d8c37584c0
3 changed files with 28 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp
+19 -30
View File
@@ -39,6 +39,7 @@ bool utc_set = false;
uavcan::uint32_t utc_jump_cnt = 0;
uavcan::int32_t utc_correction_usec_per_overflow_x16 = 0;
uavcan::int64_t prev_adjustment = 0;
uavcan::uint64_t time_mono = 0;
uavcan::uint64_t time_utc = 0;
@@ -147,43 +148,25 @@ void adjustUtc(uavcan::UtcDuration adjustment)
MutexLocker mlocker(mutex);
assert(initialized);
if (adjustment.isZero() && utc_set)
{
return; // Perfect sync
}
/*
* Naive speed adjustment (proof of concept)
* TODO: Reliable clock speed adjustment algorithm
* Naive speed adjustment - discrete PI controller.
* TODO: More reliable clock speed adjustment algorithm
*/
if (adjustment.isPositive())
{
if (utc_correction_usec_per_overflow_x16 < 0)
{
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 > 0)
{
utc_correction_usec_per_overflow_x16 -= 4;
}
else if (utc_correction_usec_per_overflow_x16 > -MaxUtcSpeedCorrectionX16)
{
utc_correction_usec_per_overflow_x16 -= 1;
}
}
const uavcan::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);
/*
* Clock value adjustment
* For small adjustments we will rely only on speed change
* For small adjustments (less than 3 msec) we will rely only on speed change
*/
if (adjustment.getAbs().toMSec() > 1 || !utc_set)
if (adjustment.getAbs().toMSec() > 2 || !utc_set)
{
const uavcan::int64_t adj_usec = adjustment.toUSec();
@@ -223,6 +206,12 @@ uavcan::uint32_t getUtcAjdustmentJumpCount()
return utc_jump_cnt;
}
uavcan::UtcDuration getPrevUtcAdjustment()
{
MutexLocker mlocker(mutex);
return uavcan::UtcDuration::fromUSec(prev_adjustment);
}
} // namespace clock
SystemClock& SystemClock::instance()