Frequency hopping using master heartbeats

Core/Inc/AMS_CAN.h

@@ -18,9 +18,12 @@
 
 #define CAN_ID_SLAVE_ERROR 0x001
 #define CAN_ID_CLOCK_SYNC 0x002
+#define CAN_ID_MASTER_HEARTBEAT 0x010
 #define CAN_ID_AMS_SLAVE_HEARTBEAT_BASE 0x600
 #define CAN_HEARTBEAT_TX_TIMEOUT 10 /* ms */
 
+extern CAN_HandleTypeDef* ams_can_handle;
+
 void ams_can_init(CAN_HandleTypeDef* ams, CAN_HandleTypeDef* car);
 
 void ams_can_handle_ams_msg(CAN_RxHeaderTypeDef* header, uint8_t* data);

Core/Inc/ClockSync.h

@@ -1,17 +1,42 @@
 #ifndef INC_CLOCK_SYNC_H_
 #define INC_CLOCK_SYNC_H_
 
+#include "stm32f4xx_hal.h"
+#include "stm32f4xx_hal_can.h"
+
 #include <stdint.h>
 
 #define CLOCK_TARGET_FREQ 16000000 // Hz
 #define HSI_TRIM_FREQ 48000        // Hz
 #define CLOCK_SYNC_INTERVAL 1000   // ms
+#define CLOCK_SYNC_SANITY_INTERVAL_MIN 900
+#define CLOCK_SYNC_SANITY_INTERVAL_MAX 1100
+#define MASTER_HEARTBEAT_INTERVAL 100 // ms
+#define MASTER_HEARTBEAT_SANITY_INTERVAL_MAX 110
+#define MASTER_HEARTBEAT_DESYNC_THRESH                                         \
+  (2 * MASTER_HEARTBEAT_SANITY_INTERVAL_MAX)
+#define FREQ_HOPPING_TRIM_STEPS 2
 #define RCC_CR_HSITRIM_MAX 31
+#define CLOCK_SYNC_MAX_TRANSMIT_ERRORS 64
 
-#define STARTUP_CLOCK_SYNC_TIME 5000 // ms
+typedef enum {
+  CLOCK_SYNC_NORMAL_OPERATION,
+  CLOCK_SYNC_FREQ_HOPPING
+} ClockSyncState;
 
-void clock_sync_handle_frame();
+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();
+void clock_sync_start_normal_operation();
+void clock_sync_start_freq_hopping();
+void clock_sync_handle_clock_sync_frame(uint8_t counter);
+void clock_sync_handle_master_heartbeat();
 void clock_sync_startup_check();
-void trim_hsi(int32_t delta);
+uint8_t get_hsi_trim();
+void set_hsi_trim(uint8_t trim);
+void trim_hsi_by(int32_t delta);
 
 #endif // INC_CLOCK_SYNC_H_

Core/Src/AMS_CAN.c

@@ -19,16 +19,16 @@
 
 #include <stdint.h>
 
-static CAN_HandleTypeDef* handle_ams;
+CAN_HandleTypeDef* ams_can_handle;
 
 void ams_can_init(CAN_HandleTypeDef* ams_handle,
                   CAN_HandleTypeDef* car_handle) {
-  handle_ams = ams_handle;
+  ams_can_handle = ams_handle;
 
   // Start peripheral
-  if (HAL_CAN_Start(handle_ams) != HAL_OK) {
+  if (HAL_CAN_Start(ams_can_handle) != HAL_OK) {
-    handle_ams = car_handle;
+    ams_can_handle = car_handle;
-    if (HAL_CAN_Start(handle_ams) != HAL_OK) {
+    if (HAL_CAN_Start(ams_can_handle) != HAL_OK) {
       Error_Handler();
     }
   }
@@ -48,18 +48,24 @@ void ams_can_init(CAN_HandleTypeDef* ams_handle,
   can_filter.FilterScale = CAN_FILTERSCALE_32BIT;
   // If we use CAN1, the slave filter should start after our filter bank. If we
   // use CAN2, it should start at our filter bank.
-  if (handle_ams == ams_handle) {
+  if (ams_can_handle == ams_handle) {
-    can_filter.SlaveStartFilterBank = 1;
+    can_filter.SlaveStartFilterBank = 14;
   } else {
     can_filter.SlaveStartFilterBank = 0;
   }
-  if (HAL_CAN_ConfigFilter(handle_ams, &can_filter) != HAL_OK) {
+  if (HAL_CAN_ConfigFilter(ams_can_handle, &can_filter) != HAL_OK) {
+    Error_Handler();
+  }
+  can_filter.FilterBank++;
+  can_filter.FilterIdHigh = CAN_ID_MASTER_HEARTBEAT << (16 - 11);
+  can_filter.FilterIdLow = 0;
+  if (HAL_CAN_ConfigFilter(ams_can_handle, &can_filter) != HAL_OK) {
     Error_Handler();
   }
 
   // Activate RX notifications
-  if (HAL_CAN_ActivateNotification(handle_ams, CAN_IT_RX_FIFO0_MSG_PENDING) !=
+  if (HAL_CAN_ActivateNotification(ams_can_handle,
-      HAL_OK) {
+                                   CAN_IT_RX_FIFO0_MSG_PENDING) != HAL_OK) {
     Error_Handler();
   }
 }
@@ -75,7 +81,7 @@ void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef* handle) {
     Error_Handler();
   }
 
-  if (handle == handle_ams) {
+  if (handle == ams_can_handle) {
     ams_can_handle_ams_msg(&header, data);
   } else {
     Error_Handler();
@@ -89,7 +95,10 @@ void ams_can_handle_ams_msg(CAN_RxHeaderTypeDef* header, uint8_t* data) {
 
   switch (header->StdId) {
   case CAN_ID_CLOCK_SYNC:
-    clock_sync_handle_frame();
+    clock_sync_handle_clock_sync_frame(data[0]);
+    break;
+  case CAN_ID_MASTER_HEARTBEAT:
+    clock_sync_handle_master_heartbeat();
     break;
   }
 }
@@ -112,10 +121,10 @@ void ams_can_send_heartbeat() {
       data[2 * i + 0] = v & 0xFF;
       data[2 * i + 1] = v >> 8;
     }
-    if (ams_can_wait_for_free_mailboxes(handle_ams, 1,
+    if (ams_can_wait_for_free_mailboxes(ams_can_handle, 1,
                                         CAN_HEARTBEAT_TX_TIMEOUT) == HAL_OK) {
       uint32_t mailbox;
-      HAL_CAN_AddTxMessage(handle_ams, &header, data, &mailbox);
+      HAL_CAN_AddTxMessage(ams_can_handle, &header, data, &mailbox);
     }
   }
 
@@ -129,10 +138,10 @@ void ams_can_send_heartbeat() {
       data[2 * i + 0] = temp & 0xFF;
       data[2 * i + 1] = temp >> 8;
     }
-    if (ams_can_wait_for_free_mailboxes(handle_ams, 1,
+    if (ams_can_wait_for_free_mailboxes(ams_can_handle, 1,
                                         CAN_HEARTBEAT_TX_TIMEOUT) == HAL_OK) {
       uint32_t mailbox;
-      HAL_CAN_AddTxMessage(handle_ams, &header, data, &mailbox);
+      HAL_CAN_AddTxMessage(ams_can_handle, &header, data, &mailbox);
     }
   }
 }
@@ -150,11 +159,11 @@ void ams_can_send_error(AMS_ErrorCode error_code,
   data[0] = slave_id;
   data[1] = error_code;
 
-  HAL_CAN_AbortTxRequest(handle_ams,
+  HAL_CAN_AbortTxRequest(ams_can_handle,
                          CAN_TX_MAILBOX0 | CAN_TX_MAILBOX1 | CAN_TX_MAILBOX2);
   uint32_t mailbox;
-  HAL_CAN_AddTxMessage(handle_ams, &header, data, &mailbox);
+  HAL_CAN_AddTxMessage(ams_can_handle, &header, data, &mailbox);
-  ams_can_wait_for_free_mailboxes(handle_ams, 3, transmission_timeout);
+  ams_can_wait_for_free_mailboxes(ams_can_handle, 3, transmission_timeout);
 }
 
 HAL_StatusTypeDef ams_can_wait_for_free_mailboxes(CAN_HandleTypeDef* handle,

Core/Src/ClockSync.c

@@ -1,24 +1,125 @@
 #include "ClockSync.h"
 
+#include "AMS_CAN.h"
+
 #include "stm32f412rx.h"
 #include "stm32f4xx_hal.h"
+#include "stm32f4xx_hal_can.h"
 
 #include <stdint.h>
 
-static uint32_t last_clock_sync_frame = 0;
+ClockSyncState clock_sync_state = CLOCK_SYNC_FREQ_HOPPING;
 
-void clock_sync_handle_frame() {
+static uint32_t last_clock_sync_frame_time = 0;
+static uint32_t last_master_heartbeat_time = 0;
+
+static uint32_t freq_hopping_start_trim = 0;
+static uint32_t freq_hopping_iteration = 0;
+
+void clock_sync_update() {
+  ClockSyncState next_state;
+  switch (clock_sync_state) {
+  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();
+    break;
+  default:
+    // Shouldn't ever happen?
+    next_state = CLOCK_SYNC_FREQ_HOPPING;
+  }
+
+  if (next_state != clock_sync_state) {
+    switch (next_state) {
+    case CLOCK_SYNC_NORMAL_OPERATION:
+      clock_sync_start_normal_operation();
+      break;
+    case CLOCK_SYNC_FREQ_HOPPING:
+      clock_sync_start_freq_hopping();
+      break;
+    }
+  }
+  clock_sync_state = next_state;
+}
+
+void clock_sync_start_normal_operation() {}
+
+void clock_sync_start_freq_hopping() {
+  freq_hopping_start_trim = get_hsi_trim();
+  freq_hopping_iteration = 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_NORMAL_OPERATION;
+}
+
+ClockSyncState clock_sync_update_freq_hopping() {
+  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;
+  }
+
+  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;
+    }
+    set_hsi_trim(new_trim);
+
+    freq_hopping_iteration++;
+    if ((freq_hopping_iteration + 1) * FREQ_HOPPING_TRIM_STEPS >
+        RCC_CR_HSITRIM_MAX) {
+      // The next delta would be too large, start again
+      freq_hopping_iteration = 0;
+    }
+  }
+  return CLOCK_SYNC_FREQ_HOPPING;
+}
+
+void clock_sync_handle_clock_sync_frame(uint8_t counter) {
   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;
+  static uint8_t last_clock_sync_frame_counter = 0;
 
   uint32_t now = HAL_GetTick();
-  if (last_clock_sync_frame != 0) {
+  uint32_t n_measured = now - last_clock_sync_frame_time;
-    uint32_t n_measured = now - last_clock_sync_frame;
+  uint8_t expected_counter = last_clock_sync_frame_counter + 1;
+  /* Sanity checks:
+   * - Are we actually in normal operation mode?
+   * - Have we received a sync frame before?
+   * - Did the counter increment by one (mod 2^8)? I.e., did we miss a frame?
+   * - Is the measured time elapsed within feasible bounds?
+   */
+  if (clock_sync_state == CLOCK_SYNC_NORMAL_OPERATION &&
+      last_clock_sync_frame_time != 0 && counter == expected_counter &&
+      n_measured >= CLOCK_SYNC_SANITY_INTERVAL_MIN &&
+      n_measured <= CLOCK_SYNC_SANITY_INTERVAL_MAX) {
     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;
+      // Update trim delta
-      if (last_trim_delta < 0) {
+      last_trim_delta = f_pre_trim - f_real;
+      if (last_trim_delta == 0) {
+        last_trim_delta = HSI_TRIM_FREQ;
+      } else if (last_trim_delta < 0) {
         last_trim_delta = -last_trim_delta;
       }
       trimmed_last_frame = 0;
@@ -27,44 +128,42 @@ void clock_sync_handle_frame() {
     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;
+      // We were able to receive the frame, so we should be reasonably close. It
-      if (delta_quants < 0) {
+      // should thus be enough to trim by -1 or 1.
-        trim_delta = -trim_delta;
+      int32_t trim_delta = (delta_quants < 0) ? -1 : 1;
-      }
+      trim_hsi_by(trim_delta);
-      trim_hsi(trim_delta);
       f_pre_trim = f_real;
-      trimmed_last_frame = trim_delta;
+      trimmed_last_frame = 1;
     }
   }
-  last_clock_sync_frame = now;
+  last_clock_sync_frame_time = now;
+  last_clock_sync_frame_counter = counter;
 }
 
-void clock_sync_startup_check() {
+void clock_sync_handle_master_heartbeat() {
-  static uint32_t last_startup_trim = 0;
+  last_master_heartbeat_time = HAL_GetTick();
-
-  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) {
+uint8_t get_hsi_trim() {
+  return (RCC->CR & RCC_CR_HSITRIM_Msk) >> RCC_CR_HSITRIM_Pos;
+}
+
+void set_hsi_trim(uint8_t trim) {
   uint32_t rcc_cr = RCC->CR;
+  // 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;
+}
+
+void trim_hsi_by(int32_t delta) {
   // Determine current trim
-  int32_t trim = (rcc_cr & RCC_CR_HSITRIM_Msk) >> RCC_CR_HSITRIM_Pos;
+  int32_t trim = get_hsi_trim();
   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
+  set_hsi_trim(trim);
-  rcc_cr = (rcc_cr & ~RCC_CR_HSITRIM_Msk) |
-           ((trim << RCC_CR_HSITRIM_Pos) & RCC_CR_HSITRIM_Msk);
-  RCC->CR = rcc_cr;
 }

Core/Src/main.c

@@ -215,11 +215,10 @@ int main(void) {
       check_error_conditions();
     }
     fan_ctrl_update();
-    // Only start sending CAN frames once the clock is somewhat synchronized
+    clock_sync_update();
-    if (HAL_GetTick() > STARTUP_CLOCK_SYNC_TIME) {
+    // Only start sending CAN frames once the clock is synchronized
+    if (clock_sync_state == CLOCK_SYNC_NORMAL_OPERATION) {
       ams_can_send_heartbeat();
-    } else {
-      clock_sync_startup_check();
     }
     delay_period();
   }

Makefile

@@ -1,5 +1,5 @@
 ##########################################################################################################################
-# File automatically-generated by tool: [projectgenerator] version: [3.17.1] date: [Sat Jul 30 20:29:35 CEST 2022] 
+# File automatically-generated by tool: [projectgenerator] version: [3.17.1] date: [Tue Aug 02 21:11:16 CEST 2022] 
 ##########################################################################################################################
 
 # ------------------------------------------------