diff --git a/Core/Inc/ClockSync.h b/Core/Inc/ClockSync.h
index 903fd9e..bc22d64 100644
--- a/Core/Inc/ClockSync.h
+++ b/Core/Inc/ClockSync.h
@@ -16,12 +16,15 @@
 #define MASTER_HEARTBEAT_DESYNC_THRESH                                         \
   (2 * MASTER_HEARTBEAT_SANITY_INTERVAL_MAX)
 #define FREQ_HOPPING_TRIM_STEPS 2
+#define FREQ_HOPPING_STAGE2_FRAMES 3
+#define FREQ_HOPPING_STAGE2_MAX_ATTEMPTS 6
 #define RCC_CR_HSITRIM_MAX 31
 #define CLOCK_SYNC_MAX_TRANSMIT_ERRORS 64
 
 typedef enum {
   CLOCK_SYNC_NORMAL_OPERATION,
-  CLOCK_SYNC_FREQ_HOPPING
+  CLOCK_SYNC_FREQ_HOPPING_STAGE1,
+  CLOCK_SYNC_FREQ_HOPPING_STAGE2
 } ClockSyncState;
 
 extern ClockSyncState clock_sync_state;
@@ -29,9 +32,11 @@ extern ClockSyncState clock_sync_state;
 void clock_sync_init(CAN_HandleTypeDef* can_handle);
 void clock_sync_update();
 ClockSyncState clock_sync_update_normal_operation();
-ClockSyncState clock_sync_update_freq_hopping();
+ClockSyncState clock_sync_update_freq_hopping_stage1();
+ClockSyncState clock_sync_update_freq_hopping_stage2();
 void clock_sync_start_normal_operation();
-void clock_sync_start_freq_hopping();
+void clock_sync_start_freq_hopping_stage1();
+void clock_sync_start_freq_hopping_stage2();
 void clock_sync_handle_clock_sync_frame(uint8_t counter);
 void clock_sync_handle_master_heartbeat();
 void clock_sync_startup_check();
@@ -39,4 +44,6 @@ uint8_t get_hsi_trim();
 void set_hsi_trim(uint8_t trim);
 void trim_hsi_by(int32_t delta);
 
+uint8_t calculate_freq_hopping_trim(uint32_t freq_hopping_iteration);
+
 #endif // INC_CLOCK_SYNC_H_
diff --git a/Core/Src/ClockSync.c b/Core/Src/ClockSync.c
index 2f4dba6..53d5af6 100644
--- a/Core/Src/ClockSync.c
+++ b/Core/Src/ClockSync.c
@@ -8,13 +8,17 @@
 
 #include <stdint.h>
 
-ClockSyncState clock_sync_state = CLOCK_SYNC_FREQ_HOPPING;
+ClockSyncState clock_sync_state = CLOCK_SYNC_FREQ_HOPPING_STAGE1;
 
 static uint32_t last_clock_sync_frame_time = 0;
 static uint32_t last_master_heartbeat_time = 0;
+static uint32_t master_heartbeat_counter = 0;
 
 static uint32_t freq_hopping_start_trim = 0;
 static uint32_t freq_hopping_iteration = 0;
+static uint32_t freq_hopping_stage2_start_time = 0;
+static uint32_t freq_hopping_stage2_start_counter = 0;
+static uint32_t freq_hopping_stage2_attempts = 0;
 
 void clock_sync_update() {
   ClockSyncState next_state;
@@ -22,12 +26,15 @@ void clock_sync_update() {
   case CLOCK_SYNC_NORMAL_OPERATION:
     next_state = clock_sync_update_normal_operation();
     break;
-  case CLOCK_SYNC_FREQ_HOPPING:
-    next_state = clock_sync_update_freq_hopping();
+  case CLOCK_SYNC_FREQ_HOPPING_STAGE1:
+    next_state = clock_sync_update_freq_hopping_stage1();
+    break;
+  case CLOCK_SYNC_FREQ_HOPPING_STAGE2:
+    next_state = clock_sync_update_freq_hopping_stage2();
     break;
   default:
     // Shouldn't ever happen?
-    next_state = CLOCK_SYNC_FREQ_HOPPING;
+    next_state = CLOCK_SYNC_FREQ_HOPPING_STAGE1;
   }
 
   if (next_state != clock_sync_state) {
@@ -35,8 +42,11 @@ void clock_sync_update() {
     case CLOCK_SYNC_NORMAL_OPERATION:
       clock_sync_start_normal_operation();
       break;
-    case CLOCK_SYNC_FREQ_HOPPING:
-      clock_sync_start_freq_hopping();
+    case CLOCK_SYNC_FREQ_HOPPING_STAGE1:
+      clock_sync_start_freq_hopping_stage1();
+      break;
+    case CLOCK_SYNC_FREQ_HOPPING_STAGE2:
+      clock_sync_start_freq_hopping_stage2();
       break;
     }
   }
@@ -45,42 +55,39 @@ void clock_sync_update() {
 
 void clock_sync_start_normal_operation() {}
 
-void clock_sync_start_freq_hopping() {
+void clock_sync_start_freq_hopping_stage1() {
   freq_hopping_start_trim = get_hsi_trim();
   freq_hopping_iteration = 0;
 }
 
+void clock_sync_start_freq_hopping_stage2() {
+  freq_hopping_start_trim = get_hsi_trim();
+  freq_hopping_stage2_start_time = HAL_GetTick();
+  freq_hopping_stage2_start_counter = master_heartbeat_counter;
+  freq_hopping_stage2_attempts = 0;
+}
+
 ClockSyncState clock_sync_update_normal_operation() {
   uint32_t now = HAL_GetTick();
   uint8_t transmit_errors =
       (ams_can_handle->Instance->ESR & CAN_ESR_TEC_Msk) >> CAN_ESR_TEC_Pos;
   if (now - last_master_heartbeat_time > MASTER_HEARTBEAT_DESYNC_THRESH ||
       transmit_errors > CLOCK_SYNC_MAX_TRANSMIT_ERRORS) {
-    return CLOCK_SYNC_FREQ_HOPPING;
+    return CLOCK_SYNC_FREQ_HOPPING_STAGE1;
   }
 
   return CLOCK_SYNC_NORMAL_OPERATION;
 }
 
-ClockSyncState clock_sync_update_freq_hopping() {
+ClockSyncState clock_sync_update_freq_hopping_stage1() {
   uint32_t now = HAL_GetTick();
   if (now - last_clock_sync_frame_time < CLOCK_SYNC_SANITY_INTERVAL_MAX) {
-    // We've re-sync'd!
-    return CLOCK_SYNC_NORMAL_OPERATION;
+    // We are at least close to re-sync'ing, go to stage 2
+    return CLOCK_SYNC_FREQ_HOPPING_STAGE2;
   }
 
   if (now - last_master_heartbeat_time > MASTER_HEARTBEAT_SANITY_INTERVAL_MAX) {
-    int32_t trim_delta = (freq_hopping_iteration + 1) * FREQ_HOPPING_TRIM_STEPS;
-    if (freq_hopping_iteration % 2 == 0) {
-      trim_delta = -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;
-    }
+    uint8_t new_trim = calculate_freq_hopping_trim(freq_hopping_iteration);
     set_hsi_trim(new_trim);
 
     freq_hopping_iteration++;
@@ -90,7 +97,33 @@ ClockSyncState clock_sync_update_freq_hopping() {
       freq_hopping_iteration = 0;
     }
   }
-  return CLOCK_SYNC_FREQ_HOPPING;
+  return CLOCK_SYNC_FREQ_HOPPING_STAGE1;
+}
+
+ClockSyncState clock_sync_update_freq_hopping_stage2() {
+  if (master_heartbeat_counter - freq_hopping_stage2_start_counter >
+      FREQ_HOPPING_STAGE2_FRAMES) {
+    // We've re-sync'd!
+    return CLOCK_SYNC_NORMAL_OPERATION;
+  }
+
+  uint32_t now = HAL_GetTick();
+  if (now - freq_hopping_stage2_start_time >
+      FREQ_HOPPING_STAGE2_FRAMES * MASTER_HEARTBEAT_SANITY_INTERVAL_MAX) {
+    freq_hopping_stage2_attempts++;
+    if (freq_hopping_stage2_attempts > FREQ_HOPPING_STAGE2_MAX_ATTEMPTS) {
+      // Looks like we're not really close to sync'ing, go back to stage 1
+      return CLOCK_SYNC_FREQ_HOPPING_STAGE1;
+    }
+    // We haven't received all heartbeats, trim further
+    uint8_t new_trim =
+        calculate_freq_hopping_trim(freq_hopping_stage2_attempts);
+    set_hsi_trim(new_trim);
+    freq_hopping_stage2_start_counter = master_heartbeat_counter;
+    freq_hopping_stage2_start_time = now;
+  }
+
+  return CLOCK_SYNC_FREQ_HOPPING_STAGE2;
 }
 
 void clock_sync_handle_clock_sync_frame(uint8_t counter) {
@@ -142,6 +175,7 @@ void clock_sync_handle_clock_sync_frame(uint8_t counter) {
 
 void clock_sync_handle_master_heartbeat() {
   last_master_heartbeat_time = HAL_GetTick();
+  master_heartbeat_counter++;
 }
 
 uint8_t get_hsi_trim() {
@@ -167,3 +201,17 @@ void trim_hsi_by(int32_t delta) {
   }
   set_hsi_trim(trim);
 }
+
+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 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;
+  }
+}