Use HSI with CAN-based time synchronization

Core/Inc/AMS_CAN.h

@@ -17,6 +17,7 @@
 #include <stdint.h>
 
 #define CAN_ID_SLAVE_ERROR 0x001
+#define CAN_ID_TIME_SYNC 0x002
 #define CAN_ID_AMS_SLAVE_HEARTBEAT_BASE 0x600
 #define CAN_HEARTBEAT_TX_TIMEOUT 10 /* ms */
 

Core/Inc/TimeSync.h

@@ -0,0 +1,13 @@
+#ifndef INC_TIME_SYNC_H_
+#define INC_TIME_SYNC_H_
+
+#define TARGET_FREQ 16000000    // Hz
+#define TRIM_FREQ 48000         // Hz
+#define TIME_SYNC_INTERVAL 1000 // ms
+#define RCC_CR_HSITRIM_MAX 31
+
+#define STARTUP_TIME_SYNC_TIME 5000 // ms
+
+void time_sync_handle_frame();
+
+#endif // INC_TIME_SYNC_H_

Core/Src/AMS_CAN.c

@@ -9,6 +9,7 @@
 
 #include "BQ_Abstraction_Layer.h"
 #include "TMP144.h"
+#include "TimeSync.h"
 #include "common_defs.h"
 #include "main.h"
 
@@ -32,6 +33,30 @@ void ams_can_init(CAN_HandleTypeDef* ams_handle,
     }
   }
 
+  // Config filter
+  CAN_FilterTypeDef can_filter;
+  can_filter.FilterActivation = CAN_FILTER_ENABLE;
+  can_filter.FilterBank = 0;
+  can_filter.FilterFIFOAssignment = CAN_FILTER_FIFO0;
+  /* Message ID is in the MSBs of the FilterId register */
+  can_filter.FilterIdHigh = CAN_ID_TIME_SYNC << (16 - 11);
+  can_filter.FilterIdLow = 0;
+  /* Filter the 11 MSBs (i.e. a StdId) */
+  can_filter.FilterMaskIdHigh = 0xFFE0;
+  can_filter.FilterMaskIdLow = 0;
+  can_filter.FilterMode = CAN_FILTERMODE_IDMASK;
+  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) {
+    can_filter.SlaveStartFilterBank = 1;
+  } else {
+    can_filter.SlaveStartFilterBank = 0;
+  }
+  if (HAL_CAN_ConfigFilter(handle_ams, &can_filter) != HAL_OK) {
+    Error_Handler();
+  }
+
   // Activate RX notifications
   if (HAL_CAN_ActivateNotification(handle_ams, CAN_IT_RX_FIFO0_MSG_PENDING) !=
       HAL_OK) {
@@ -57,7 +82,17 @@ void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef* handle) {
   }
 }
 
-void ams_can_handle_ams_msg(CAN_RxHeaderTypeDef* header, uint8_t* data) {}
+void ams_can_handle_ams_msg(CAN_RxHeaderTypeDef* header, uint8_t* data) {
+  if (header->IDE != CAN_ID_STD) {
+    return;
+  }
+
+  switch (header->StdId) {
+  case CAN_ID_TIME_SYNC:
+    time_sync_handle_frame();
+    break;
+  }
+}
 
 void ams_can_send_heartbeat() {
   static CAN_TxHeaderTypeDef header;

Core/Src/TimeSync.c

@@ -0,0 +1,53 @@
+#include "TimeSync.h"
+
+#include "stm32f412rx.h"
+#include "stm32f4xx_hal.h"
+
+#include <stdint.h>
+
+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;
+
+  uint32_t now = HAL_GetTick();
+  if (last_time_sync_frame != 0) {
+    uint32_t n_measured = now - last_time_sync_frame;
+    uint32_t f_real = n_measured * (TARGET_FREQ / TIME_SYNC_INTERVAL);
+    if (trimmed_last_frame) {
+      last_trim_delta = (f_pre_trim - f_real) / trimmed_last_frame;
+      if (last_trim_delta < 0) {
+        last_trim_delta = -last_trim_delta;
+      }
+      trimmed_last_frame = 0;
+    }
+
+    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;
+        }
+      }
+      // 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;
+      f_pre_trim = f_real;
+      trimmed_last_frame = trim_delta;
+    }
+  }
+  last_time_sync_frame = now;
+}

Core/Src/main.c

@@ -26,6 +26,7 @@
 #include "EEPROM.h"
 #include "FanControl.h"
 #include "TMP144.h"
+#include "TimeSync.h"
 #include "common_defs.h"
 
 #include "stm32f4xx_hal.h"
@@ -214,7 +215,10 @@ int main(void) {
       check_error_conditions();
     }
     fan_ctrl_update();
-    ams_can_send_heartbeat();
+    // Only start sending CAN frames once the clock is somewhat synchronized
+    if (HAL_GetTick() > STARTUP_TIME_SYNC_TIME) {
+      ams_can_send_heartbeat();
+    }
     delay_period();
   }
   /* USER CODE END 3 */
@@ -236,8 +240,9 @@ void SystemClock_Config(void) {
   /** Initializes the RCC Oscillators according to the specified parameters
    * in the RCC_OscInitTypeDef structure.
    */
-  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
-  RCC_OscInitStruct.HSEState = RCC_HSE_ON;
+  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;
+  RCC_OscInitStruct.HSIState = RCC_HSI_ON;
+  RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
   RCC_OscInitStruct.PLL.PLLState = RCC_PLL_NONE;
   if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK) {
     Error_Handler();
@@ -247,7 +252,7 @@ void SystemClock_Config(void) {
    */
   RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK | RCC_CLOCKTYPE_SYSCLK |
                                 RCC_CLOCKTYPE_PCLK1 | RCC_CLOCKTYPE_PCLK2;
-  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_HSE;
+  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_HSI;
   RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
   RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV1;
   RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;
@@ -255,10 +260,6 @@ void SystemClock_Config(void) {
   if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_0) != HAL_OK) {
     Error_Handler();
   }
-
-  /** Enables the Clock Security System
-   */
-  HAL_RCC_EnableCSS();
 }
 
 /**
@@ -551,7 +552,6 @@ static void MX_GPIO_Init(void) {
   GPIO_InitTypeDef GPIO_InitStruct = {0};
 
   /* GPIO Ports Clock Enable */
-  __HAL_RCC_GPIOH_CLK_ENABLE();
   __HAL_RCC_GPIOC_CLK_ENABLE();
   __HAL_RCC_GPIOA_CLK_ENABLE();
   __HAL_RCC_GPIOB_CLK_ENABLE();

Core/Src/stm32f4xx_it.c

@@ -77,7 +77,6 @@ void NMI_Handler(void)
   /* USER CODE BEGIN NonMaskableInt_IRQn 0 */
 
   /* USER CODE END NonMaskableInt_IRQn 0 */
-  HAL_RCC_NMI_IRQHandler();
   /* USER CODE BEGIN NonMaskableInt_IRQn 1 */
   while (1) {
   }