From 29e15fc0f7f8a2cbe75d591a8165255ec0c785ae Mon Sep 17 00:00:00 2001
From: jazzpi <jasper@mezzo.de>
Date: Sun, 31 Jul 2022 12:09:55 +0200
Subject: [PATCH] Trim HSI until time sync packets are received

---
 Core/Inc/TimeSync.h |  4 ++++
 Core/Src/TimeSync.c | 55 +++++++++++++++++++++++++++++----------------
 Core/Src/main.c     |  2 ++
 3 files changed, 42 insertions(+), 19 deletions(-)

diff --git a/Core/Inc/TimeSync.h b/Core/Inc/TimeSync.h
index 842c7ee..6247e43 100644
--- a/Core/Inc/TimeSync.h
+++ b/Core/Inc/TimeSync.h
@@ -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_
diff --git a/Core/Src/TimeSync.c b/Core/Src/TimeSync.c
index dd79ee9..2b61cfa 100644
--- a/Core/Src/TimeSync.c
+++ b/Core/Src/TimeSync.c
@@ -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;
+}
diff --git a/Core/Src/main.c b/Core/Src/main.c
index 8b7fc19..7fd7fd2 100644
--- a/Core/Src/main.c
+++ b/Core/Src/main.c
@@ -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();
   }