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

STM32: Clock speed PPM limit

Browse Source
This commit is contained in:
Pavel Kirienko 2014-04-25 15:23:22 +04:00
parent 32671fe574
commit 430776469f
2 changed files with 15 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libuavcan_drivers/stm32/driver/include/uavcan_stm32/clock.hpp
View File

@ -43,6 +43,12 @@ void adjustUtc(uavcan::UtcDuration adjustment);
*/
uavcan::int32_t getUtcSpeedCorrectionPPM();
/**
* Sets maximum absolute UTC speed correction in ppm.
* This function is thread safe.
*/
void setMaxUtcSpeedCorrectionPPM(uavcan::uint32_t ppm);
/**
* Number of non-gradual adjustments performed so far.
* Ideally should be zero.

12
libuavcan_drivers/stm32/driver/src/uc_stm32_clock.cpp
View File

@ -43,12 +43,12 @@ uavcan::int32_t utc_correction_usec_per_overflow_x16 = 0;
uavcan::int64_t prev_adjustment = 0;
uavcan::UtcDuration min_utc_jump = uavcan::UtcDuration::fromMSec(10);
uavcan::int32_t max_utc_speed_correction_x16 = 20 * 16;
uavcan::uint64_t time_mono = 0;
uavcan::uint64_t time_utc = 0;
const uavcan::uint32_t USecPerOverflow = 65536;
const uavcan::int32_t MaxUtcSpeedCorrectionX16 = 30 * 16;
}
@ -158,8 +158,8 @@ void adjustUtc(uavcan::UtcDuration adjustment)
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);
utc_correction_usec_per_overflow_x16 = std::max(utc_correction_usec_per_overflow_x16,-max_utc_speed_correction_x16);
utc_correction_usec_per_overflow_x16 = std::min(utc_correction_usec_per_overflow_x16, max_utc_speed_correction_x16);
/*
* Clock value adjustment
@ -199,6 +199,12 @@ uavcan::int32_t getUtcSpeedCorrectionPPM()
return uavcan::int64_t((utc_correction_usec_per_overflow_x16 * 1000000) / 16) / USecPerOverflow;
}
void setMaxUtcSpeedCorrectionPPM(uavcan::uint32_t ppm)
{
MutexLocker mlocker(mutex);
max_utc_speed_correction_x16 = (USecPerOverflow * 16LL * uavcan::int64_t(ppm)) / 1000000;
}
uavcan::uint32_t getUtcAjdustmentJumpCount()
{
MutexLocker mlocker(mutex);