From 4f02c21bd98a893be335da61884d242a82011621 Mon Sep 17 00:00:00 2001
From: jazzpi <jasper@mezzo.de>
Date: Thu, 4 Aug 2022 15:44:46 +0200
Subject: [PATCH] Trim clock downwards during frequency hopping

---
 Core/Src/ClockSync.c | 27 ++++++++++++++-------------
 1 file changed, 14 insertions(+), 13 deletions(-)

diff --git a/Core/Src/ClockSync.c b/Core/Src/ClockSync.c
index 7a2c227..c41d5be 100644
--- a/Core/Src/ClockSync.c
+++ b/Core/Src/ClockSync.c
@@ -70,10 +70,11 @@
  * STAGE 1
  * -------
  * Stage 1 trims the HSI until at least one MASTER_HEARTBEAT frame has been
- * received. The frequency alternates between lower and higher values, i.e. if
- * the trim was initially 16, it will go through the following values:
+ * received. The frequency first goes down until it reaches 0, then goes up from
+ * the initial value. I.e. if the trim was initially 5, it will go through the
+ * following values:
  *
- * 16 -> 14 -> 18 -> 12 -> 20 -> 10 -> 22 -> ...
+ * 5 -> 3 -> 1 -> 6 -> 8 -> ...
  *
  * Once a MASTER_HEARTBEAT frame has been received, the slave transitions to
  * stage 2.
@@ -81,7 +82,7 @@
  * STAGE 2
  * -------
  * Stage 2 trims the HSI further until at least three consecutive
- * MASTER_HEARTBEAT frames have been received. The frequency alternates in the
+ * MASTER_HEARTBEAT frames have been received. The frequency is trimmed in the
  * same fashion as in stage 1, but now around the frequency where a
  * MASTER_HEARTBEAT frame was received in stage 1, and more slowly.
  *
@@ -314,15 +315,15 @@ void trim_hsi_by(int32_t delta) {
 }
 
 uint8_t calculate_freq_hopping_trim(uint32_t freq_hopping_iteration) {
-  int32_t trim_delta = (freq_hopping_iteration + 1) * FREQ_HOPPING_TRIM_STEPS;
-  if (freq_hopping_iteration % 2 == 0) {
-    trim_delta = -trim_delta;
+  int32_t trim_delta =
+      ((freq_hopping_iteration + 1) * FREQ_HOPPING_TRIM_STEPS) %
+      (RCC_CR_HSITRIM_MAX + 1);
+  int32_t new_trim = freq_hopping_start_trim - trim_delta;
+
+  if (new_trim < 0) {
+    // Trim upwards from the start trim
+    new_trim = trim_delta;
   }
 
-  int32_t new_trim = freq_hopping_start_trim + trim_delta;
-  if (new_trim < 0) {
-    new_trim += RCC_CR_HSITRIM_MAX + 1;
-  } else if (new_trim > RCC_CR_HSITRIM_MAX) {
-    new_trim -= RCC_CR_HSITRIM_MAX + 1;
-  }
+  return new_trim;
 }