71 lines
2.0 KiB
C
71 lines
2.0 KiB
C
#include "ClockSync.h"
|
|
|
|
#include "stm32f412rx.h"
|
|
#include "stm32f4xx_hal.h"
|
|
|
|
#include <stdint.h>
|
|
|
|
static uint32_t last_clock_sync_frame = 0;
|
|
|
|
void clock_sync_handle_frame() {
|
|
static uint32_t f_pre_trim = CLOCK_TARGET_FREQ;
|
|
static int32_t trimmed_last_frame = 0;
|
|
static int32_t last_trim_delta = HSI_TRIM_FREQ;
|
|
|
|
uint32_t now = HAL_GetTick();
|
|
if (last_clock_sync_frame != 0) {
|
|
uint32_t n_measured = now - last_clock_sync_frame;
|
|
uint32_t f_real = n_measured * (CLOCK_TARGET_FREQ / CLOCK_SYNC_INTERVAL);
|
|
if (trimmed_last_frame) {
|
|
last_trim_delta = ((int32_t)(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 = CLOCK_TARGET_FREQ - f_real;
|
|
int32_t delta_quants = delta_f / last_trim_delta;
|
|
if (delta_quants != 0) {
|
|
int32_t trim_delta = (now < STARTUP_CLOCK_SYNC_TIME) ? 2 : 1;
|
|
if (delta_quants < 0) {
|
|
trim_delta = -trim_delta;
|
|
}
|
|
trim_hsi(trim_delta);
|
|
f_pre_trim = f_real;
|
|
trimmed_last_frame = trim_delta;
|
|
}
|
|
}
|
|
last_clock_sync_frame = now;
|
|
}
|
|
|
|
void clock_sync_startup_check() {
|
|
static uint32_t last_startup_trim = 0;
|
|
|
|
uint32_t now = HAL_GetTick();
|
|
if (now - last_clock_sync_frame > CLOCK_SYNC_INTERVAL &&
|
|
now - last_startup_trim > CLOCK_SYNC_INTERVAL) {
|
|
// The slave is probably warm, which increases the clock frequency. If the
|
|
// frequency is too high, we won't receive any CAN frames anymore, so let's
|
|
// try trimming down a bit.
|
|
trim_hsi(-2);
|
|
last_startup_trim = now;
|
|
}
|
|
}
|
|
|
|
void trim_hsi(int32_t delta) {
|
|
uint32_t rcc_cr = RCC->CR;
|
|
// Determine current trim
|
|
int32_t trim = (rcc_cr & RCC_CR_HSITRIM_Msk) >> RCC_CR_HSITRIM_Pos;
|
|
trim += delta;
|
|
if (trim > RCC_CR_HSITRIM_MAX) {
|
|
trim = RCC_CR_HSITRIM_MAX;
|
|
} else 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;
|
|
}
|