FaSTTUBe/ams-slave-22
61
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Trim HSI until time sync packets are received

Browse Source
This commit is contained in:
jazzpi 2022-07-31 12:09:55 +02:00
parent 171d3e40cd
commit 29e15fc0f7
3 changed files with 42 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
Core/Inc/TimeSync.h
View File

@ -1,6 +1,8 @@
#ifndef INC_TIME_SYNC_H_
#define INC_TIME_SYNC_H_
#include <stdint.h>
#define TARGET_FREQ 16000000 // Hz
#define TRIM_FREQ 48000 // Hz
#define TIME_SYNC_INTERVAL 1000 // ms
@ -9,5 +11,7 @@
#define STARTUP_TIME_SYNC_TIME 5000 // ms
void time_sync_handle_frame();
void time_sync_startup_check();
void trim_hsi(int32_t delta);
#endif // INC_TIME_SYNC_H_

55
Core/Src/TimeSync.c
View File

@ -5,8 +5,9 @@
#include <stdint.h>
static uint32_t last_time_sync_frame = 0;
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;
@ -26,28 +27,44 @@ void time_sync_handle_frame() {
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;
}
int32_t trim_delta = (now < STARTUP_TIME_SYNC_TIME) ? 2 : 1;
if (delta_quants < 0) {
trim_delta = -trim_delta;
}
// 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;
trim_hsi(trim_delta);
f_pre_trim = f_real;
trimmed_last_frame = trim_delta;
}
}
last_time_sync_frame = now;
}
void time_sync_startup_check() {
static uint32_t last_startup_trim = 0;
uint32_t now = HAL_GetTick();
if (now - last_time_sync_frame > TIME_SYNC_INTERVAL &&
now - last_startup_trim > TIME_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;
}

2
Core/Src/main.c
View File

@ -218,6 +218,8 @@ int main(void) {
// Only start sending CAN frames once the clock is somewhat synchronized
if (HAL_GetTick() > STARTUP_TIME_SYNC_TIME) {
ams_can_send_heartbeat();
} else {
time_sync_startup_check();
}
delay_period();
}