Trim clock downwards during frequency hopping

Core/Inc/BQ_Abstraction_Layer.h

@@ -51,6 +51,7 @@ extern BQ_Error_Description bq_error;
 extern uint32_t lastmeasurementtime;
 
 void afe_init(UART_HandleTypeDef* uarthandle);
+void afe_soft_reset();
 void afe_shutdown();
 void afe_measure();
 void afe_selftest();

Core/Inc/BQ_Register_Definitions.h

@@ -188,6 +188,8 @@
 #define EE_BURN_SIZE 0x01
 #define MAGIC2_SIZE 0x04
 
+#define DEV_CNTRL_SOFT_RESET (1 << 7)
+
 #define GPI_FAULT_SUM (1 << 6)
 #define CHIP_FAULT_SUM (1 << 7)
 #define SYS_FAULT_SUM (1 << 8)

Core/Src/BQ_Abstraction_Layer.c

@@ -44,8 +44,15 @@ void afe_init(UART_HandleTypeDef* uarthandle) {
   afe_wakeup();
   HAL_Delay(10);
 
+  afe_soft_reset();
+}
+
+void afe_soft_reset() {
   bq_status = BQ_INIT_PHASE;
 
+  BQ_Write_Register(DEV_CNTRL, DEV_CNTRL_SIZE, DEV_CNTRL_SOFT_RESET);
+  HAL_Delay(10);
+
   afe_config_communication();
   afe_config_measurement_channels();
 
@@ -57,13 +64,13 @@ void afe_init(UART_HandleTypeDef* uarthandle) {
 
   afe_init_fault_thresholds();
 
-  HAL_Delay(1000);
+  HAL_Delay(100);
 
   afe_update_Checksum();
 
   afe_clear_all_faults();
 
-  HAL_Delay(100);
+  HAL_Delay(50);
 
   afe_check_faults();
   lastmeasurementtime = 0;

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
+ * received. The frequency first goes down until it reaches 0, then goes up from
- * the trim was initially 16, it will go through the following values:
+ * 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;
+  int32_t trim_delta =
-  if (freq_hopping_iteration % 2 == 0) {
+      ((freq_hopping_iteration + 1) * FREQ_HOPPING_TRIM_STEPS) %
-    trim_delta = -trim_delta;
+      (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;
+  return new_trim;
-  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;
-  }
 }

Core/Src/TMP144.c

@@ -154,14 +154,16 @@ HAL_StatusTypeDef tmp144_recv_temps(TMP144Bus* bus) {
     return HAL_ERROR;
   }
 
+  size_t temperatures_offset =
+      (bus == &tmp144_bus_busbar) ? 0 : N_TEMP_SENSORS / 2;
+
   bus->state = TMP144_IDLE;
   size_t headerlen = sizeof(TMP144_SEQ_READ_TEMPS);
   if (memcmp(bus->rxbuf, TMP144_SEQ_READ_TEMPS, headerlen) != 0) {
+    memset(&temperatures[temperatures_offset], 0, sizeof(temperatures) / 2);
     return HAL_ERROR;
   }
 
-  size_t temperatures_offset =
-      (bus == &tmp144_bus_busbar) ? 0 : N_TEMP_SENSORS / 2;
   for (size_t i = 0; i < bus->n_sensors; i++) {
     size_t buf_offset = headerlen + 2 * i;
     uint16_t temp =

Core/Src/main.c

@@ -209,15 +209,16 @@ int main(void) {
 
     /* USER CODE BEGIN 3 */
     update_status_leds();
+    fan_ctrl_update();
+    clock_sync_update();
+    // Only use communication interfaces (BQ UART, TMP144 UARTs, CAN) once the
+    // clock is synchronized
+    if (clock_sync_state == CLOCK_SYNC_NORMAL_OPERATION) {
       afe_measure();
       tmp144_read_temps();
       if (HAL_GetTick() - main_loop_start > ERROR_CHECK_START) {
         check_error_conditions();
       }
-    fan_ctrl_update();
-    clock_sync_update();
-    // Only start sending CAN frames once the clock is synchronized
-    if (clock_sync_state == CLOCK_SYNC_NORMAL_OPERATION) {
       ams_can_send_heartbeat();
     }
     delay_period();