54 lines
1.6 KiB
C
54 lines
1.6 KiB
C
#include "TimeSync.h"
|
|
|
|
#include "stm32f412rx.h"
|
|
#include "stm32f4xx_hal.h"
|
|
|
|
#include <stdint.h>
|
|
|
|
void time_sync_handle_frame() {
|
|
static uint32_t last_time_sync_frame = 0;
|
|
static uint32_t f_pre_trim = TARGET_FREQ;
|
|
static uint8_t trimmed_last_frame = 0;
|
|
static int32_t last_trim_delta = TRIM_FREQ;
|
|
|
|
uint32_t now = HAL_GetTick();
|
|
if (last_time_sync_frame != 0) {
|
|
uint32_t n_measured = now - last_time_sync_frame;
|
|
uint32_t f_real = n_measured * (TARGET_FREQ / TIME_SYNC_INTERVAL);
|
|
if (trimmed_last_frame) {
|
|
last_trim_delta = (f_pre_trim - f_real) / trimmed_last_frame;
|
|
if (last_trim_delta < 0) {
|
|
last_trim_delta = -last_trim_delta;
|
|
}
|
|
trimmed_last_frame = 0;
|
|
}
|
|
|
|
int32_t delta_f = TARGET_FREQ - f_real;
|
|
int32_t delta_quants = delta_f / last_trim_delta;
|
|
if (delta_quants != 0) {
|
|
uint32_t trim_delta = (now < STARTUP_TIME_SYNC_TIME) ? 2 : 1;
|
|
uint32_t rcc_cr = RCC->CR;
|
|
// Determine current trim
|
|
int32_t trim = (rcc_cr & RCC_CR_HSITRIM_Msk) >> RCC_CR_HSITRIM_Pos;
|
|
if (delta_quants > 0) {
|
|
trim += trim_delta;
|
|
if (trim > RCC_CR_HSITRIM_MAX) {
|
|
trim = RCC_CR_HSITRIM_MAX;
|
|
}
|
|
} else if (delta_quants < 0) {
|
|
trim -= trim_delta;
|
|
if (trim < 0) {
|
|
trim = 0;
|
|
}
|
|
}
|
|
// Clear current trim and overwrite with new trim
|
|
rcc_cr = (rcc_cr & ~RCC_CR_HSITRIM_Msk) |
|
|
((trim << RCC_CR_HSITRIM_Pos) & RCC_CR_HSITRIM_Msk);
|
|
RCC->CR = rcc_cr;
|
|
f_pre_trim = f_real;
|
|
trimmed_last_frame = trim_delta;
|
|
}
|
|
}
|
|
last_time_sync_frame = now;
|
|
}
|