From 8526ee709c3c526fc35e9cd824db64a10684e7da Mon Sep 17 00:00:00 2001
From: boringethan <ethan@openwater.cc>
Date: Fri, 20 Mar 2026 11:32:51 -0700
Subject: [PATCH 01/11] compression initial commit - works ok on 2 cameras

---
 Core/Inc/camera_manager.h |   2 +
 Core/Inc/common.h         |   1 +
 Core/Src/camera_manager.c | 151 +++++++++++++++++++++++++++++++++++++-
 Core/Src/logging.c        |   5 +-
 4 files changed, 155 insertions(+), 4 deletions(-)

diff --git a/Core/Inc/camera_manager.h b/Core/Inc/camera_manager.h
index 9963b78..1ef6426 100644
--- a/Core/Inc/camera_manager.h
+++ b/Core/Inc/camera_manager.h
@@ -49,6 +49,7 @@ typedef struct {
 #define HISTO_EOH  0xEE
 #define HISTO_EOF  0xDD
 #define TYPE_HISTO 0x00
+#define TYPE_HISTO_CMP 0x01
 #define HISTO_HEADER_SIZE 6
 #define HISTO_TRAILER_SIZE 3
 
@@ -77,6 +78,7 @@ _Bool abort_data_reception(uint8_t cam_id);
 _Bool send_data(void);
 _Bool send_fake_data(void);
 _Bool send_histogram_data(void);
+_Bool send_histogram_data_cmp(void);
 _Bool enable_camera_stream(uint8_t cam_id);
 _Bool disable_camera_stream(uint8_t cam_id);
 uint8_t get_camera_status(uint8_t cam_id);
diff --git a/Core/Inc/common.h b/Core/Inc/common.h
index a155ec9..6eba3d9 100644
--- a/Core/Inc/common.h
+++ b/Core/Inc/common.h
@@ -30,6 +30,7 @@
 #define DEBUG_FLAG_HISTO_SPARSE (1u << 3)  /* Send histogram data in small chunks over ~15s to reduce EMI */
 #define DEBUG_FLAG_COMM_VERBOSE (1u << 4)  /* Enable cmd id and "." response prints in uart_comms */
 #define DEBUG_FLAG_CMD_VERBOSE (1u << 5)  /* Enable printf in command handlers (if_commands.c) */
+#define DEBUG_FLAG_HISTO_CMP  (1u << 6)  /* Send compressed histogram packets (TYPE_HISTO_CMP) */
 
 
 #define I2C_IRQ_PRIORITY 0
diff --git a/Core/Src/camera_manager.c b/Core/Src/camera_manager.c
index d7fe675..00483be 100644
--- a/Core/Src/camera_manager.c
+++ b/Core/Src/camera_manager.c
@@ -36,7 +36,8 @@ static int _active_cam_idx = 0;
 volatile bool usb_failed = false;
 
 __ALIGN_BEGIN volatile uint8_t frame_buffer[1][CAMERA_COUNT * HISTOGRAM_DATA_SIZE] __ALIGN_END; // Double buffer
-__ALIGN_BEGIN uint8_t packet_buffer[HISTO_JSON_BUFFER_SIZE] __ALIGN_END; 
+__ALIGN_BEGIN uint8_t packet_buffer[HISTO_JSON_BUFFER_SIZE] __ALIGN_END;
+__ALIGN_BEGIN uint8_t uncmp_payload[HISTO_JSON_BUFFER_SIZE] __ALIGN_END;  // Staging buffer for compression
 
 static uint8_t _active_buffer = 0; // Index of the buffer currently being written to
 volatile uint8_t frame_id = 0;
@@ -1152,8 +1153,11 @@ _Bool send_data(void) {
 	check_camera_failures();
 	
 	bool success = false;
-	if ((logging_get_debug_flags() & DEBUG_FLAG_FAKE_DATA) != 0u) {
+	uint32_t dflags = logging_get_debug_flags();
+	if ((dflags & DEBUG_FLAG_FAKE_DATA) != 0u) {
 		success = send_fake_data();
+	} else if ((dflags & DEBUG_FLAG_HISTO_CMP) != 0u) {
+		success = send_histogram_data_cmp();
 	} else {
 		success = send_histogram_data();
 	}
@@ -1293,6 +1297,149 @@ _Bool send_histogram_data(void) {
 	return status;
 }
 
+/*
+ * PackBits-style byte-level RLE compressor.
+ * Control byte < 0x80: literal run of (ctrl + 1) bytes follow (1–128).
+ * Control byte >= 0x80: repeat run – next byte repeated (ctrl - 0x80 + 3) times (3–130).
+ * Returns compressed size, or -1 if dst_max would be exceeded.
+ */
+static int rle_compress(const uint8_t *src, int src_len, uint8_t *dst, int dst_max) {
+	int si = 0, di = 0;
+	while (si < src_len) {
+		/* Try to find a run of identical bytes (min 3) */
+		uint8_t val = src[si];
+		int run_start = si;
+		while (si < src_len && src[si] == val && (si - run_start) < 130) {
+			si++;
+		}
+		int run_len = si - run_start;
+
+		if (run_len >= 3) {
+			/* Encode as repeat run */
+			if (di + 2 > dst_max) return -1;
+			dst[di++] = (uint8_t)(0x80 + (run_len - 3));
+			dst[di++] = val;
+		} else {
+			/* Collect literals until we hit a run of 3+ identical bytes */
+			si = run_start;
+			int lit_start = si;
+			while (si < src_len) {
+				if (si + 2 < src_len && src[si] == src[si + 1] && src[si] == src[si + 2]) {
+					break;
+				}
+				si++;
+				if (si - lit_start >= 128) break;
+			}
+			int lit_len = si - lit_start;
+			if (di + 1 + lit_len > dst_max) return -1;
+			dst[di++] = (uint8_t)(lit_len - 1);
+			memcpy(dst + di, src + lit_start, lit_len);
+			di += lit_len;
+		}
+	}
+	return di;
+}
+
+_Bool send_histogram_data_cmp(void) {
+	_Bool status = true;
+	int p_off = 0;   /* offset into uncmp_payload (uncompressed staging) */
+	uint8_t ready_bits = 0;
+
+	if (event_bits_enabled == 0x00) {
+		return true;
+	}
+	__disable_irq();
+	ready_bits = event_bits;
+	event_bits = 0x00;
+	__enable_irq();
+
+	uint8_t count = 0;
+	bool skip_no_data_log = streaming_first_frame;
+	streaming_first_frame = false;
+	for (int i = 0; i < CAMERA_COUNT; ++i) {
+		if (ready_bits & (1 << i)) {
+			count++;
+		}
+	}
+	if (count == 0) {
+		if (!skip_no_data_log) {
+			printf("No cameras have data to send\r\n");
+		}
+		return false;
+	}
+
+	/* --- Build uncompressed payload into uncmp_payload --- */
+
+	/* Timestamp (4 bytes) */
+	uint32_t timestamp = get_timestamp_ms();
+	uncmp_payload[p_off++] = (uint8_t)(timestamp & 0xFF);
+	uncmp_payload[p_off++] = (uint8_t)((timestamp >> 8) & 0xFF);
+	uncmp_payload[p_off++] = (uint8_t)((timestamp >> 16) & 0xFF);
+	uncmp_payload[p_off++] = (uint8_t)((timestamp >> 24) & 0xFF);
+
+	/* Per-camera data blocks */
+	for (uint8_t cam_id = 0; cam_id < CAMERA_COUNT; ++cam_id) {
+		if ((ready_bits & (0x01 << cam_id)) != 0) {
+			uint32_t *histo_ptr = (uint32_t *)cam_array[cam_id].pRecieveHistoBuffer;
+			uncmp_payload[p_off++] = HISTO_SOH;
+			uncmp_payload[p_off++] = cam_id;
+			memcpy(uncmp_payload + p_off, histo_ptr, HISTO_SIZE_32B * 4);
+			p_off += HISTO_SIZE_32B * 4;
+
+			uint32_t temp_bits;
+			memcpy(&temp_bits, (uint8_t *)&cam_temp[cam_id], 4);
+			uncmp_payload[p_off++] = (uint8_t)(temp_bits & 0xFF);
+			uncmp_payload[p_off++] = (uint8_t)((temp_bits >> 8) & 0xFF);
+			uncmp_payload[p_off++] = (uint8_t)((temp_bits >> 16) & 0xFF);
+			uncmp_payload[p_off++] = (uint8_t)((temp_bits >> 24) & 0xFF);
+
+			uncmp_payload[p_off++] = HISTO_EOH;
+
+			/* Re-arm reception as soon as data is copied */
+			start_data_reception(cam_id);
+		}
+	}
+
+	/* --- Compress payload into packet_buffer (after header) --- */
+	int cmp_len = rle_compress(uncmp_payload, p_off,
+	                           packet_buffer + HISTO_HEADER_SIZE,
+	                           HISTO_JSON_BUFFER_SIZE - HISTO_HEADER_SIZE - HISTO_TRAILER_SIZE);
+	if (cmp_len < 0) {
+		printf("RLE compression failed (output too large)\r\n");
+		return false;
+	}
+
+	/* --- Header --- */
+	uint32_t total_size = HISTO_HEADER_SIZE + (uint32_t)cmp_len + HISTO_TRAILER_SIZE;
+	int offset = 0;
+	packet_buffer[offset++] = HISTO_SOF;
+	packet_buffer[offset++] = TYPE_HISTO_CMP;
+	packet_buffer[offset++] = (uint8_t)(total_size & 0xFF);
+	packet_buffer[offset++] = (uint8_t)((total_size >> 8) & 0xFF);
+	packet_buffer[offset++] = (uint8_t)((total_size >> 16) & 0xFF);
+	packet_buffer[offset++] = (uint8_t)((total_size >> 24) & 0xFF);
+
+	/* Skip over the compressed data we already wrote */
+	offset = HISTO_HEADER_SIZE + cmp_len;
+
+	/* --- Footer --- */
+	uint16_t crc = util_crc16(packet_buffer, offset - 1);
+	packet_buffer[offset++] = crc & 0xFF;
+	packet_buffer[offset++] = (crc >> 8) & 0xFF;
+	packet_buffer[offset++] = HISTO_EOF;
+
+	/* Send data */
+	uint8_t tx_status = USBD_HISTO_SendData(&hUsbDeviceHS, packet_buffer, offset, 0);
+	if (tx_status != USBD_OK) {
+		status = false;
+		printf("USBD_HISTO_SendData failed: %d\r\n", tx_status);
+	}
+
+	frame_id++;
+
+	return status;
+}
+
 _Bool send_fake_data(void) {
 
 	fill_frame_buffers();
diff --git a/Core/Src/logging.c b/Core/Src/logging.c
index 992df91..4042a47 100644
--- a/Core/Src/logging.c
+++ b/Core/Src/logging.c
@@ -102,13 +102,14 @@ void logging_set_debug_flags(uint32_t flags) {
 	uint32_t prev_flags = debug_flags;
 	debug_flags = flags;
 	if (prev_flags != debug_flags) {
-		printf("Debug flags changed: 0x%08lX -> 0x%08lX%s%s%s%s\r\n",
+		printf("Debug flags changed: 0x%08lX -> 0x%08lX%s%s%s%s%s\r\n",
 		       (unsigned long)prev_flags,
 		       (unsigned long)debug_flags,
 		       (debug_flags & DEBUG_FLAG_USB_PRINTF) ? " (USB printf ON)" : " (USB printf OFF)",
 		       (debug_flags & DEBUG_FLAG_HISTO_SPARSE) ? " (HISTO sparse ON)" : "",
 		       (debug_flags & DEBUG_FLAG_COMM_VERBOSE) ? " (comm verbose ON)" : "",
-		       (debug_flags & DEBUG_FLAG_CMD_VERBOSE) ? " (verbose cmd ON)" : "");
+		       (debug_flags & DEBUG_FLAG_CMD_VERBOSE) ? " (verbose cmd ON)" : "",
+		       (debug_flags & DEBUG_FLAG_HISTO_CMP) ? " (histo compress ON)" : "");
 	}
 	if ((debug_flags & DEBUG_FLAG_USB_PRINTF) == 0u) {
 		// Drop any buffered log data so it doesn't flush later.

From 88efa441f9f454d8f7cdd973fe14588d906156e0 Mon Sep 17 00:00:00 2001
From: boringethan <ethan@openwater.cc>
Date: Fri, 20 Mar 2026 12:43:56 -0700
Subject: [PATCH 02/11] Add statistics to expose how well the compression is
 working

---
 Core/Src/camera_manager.c | 60 +++++++++++++++++++++++++++++++++++----
 1 file changed, 55 insertions(+), 5 deletions(-)

diff --git a/Core/Src/camera_manager.c b/Core/Src/camera_manager.c
index 00483be..3c473de 100644
--- a/Core/Src/camera_manager.c
+++ b/Core/Src/camera_manager.c
@@ -51,6 +51,14 @@ extern uint32_t pulse_count;
 volatile uint32_t total_frames_sent = 0;
 volatile uint32_t total_frames_failed = 0;
 
+// Compression statistics (running averages)
+static uint32_t cmp_total_uncompressed = 0;  // Sum of all uncompressed payload sizes
+static uint32_t cmp_total_compressed = 0;    // Sum of all compressed payload sizes
+static uint32_t cmp_frame_count = 0;         // Number of compressed frames sent
+static uint32_t cmp_fail_count = 0;          // Number of compression failures
+static uint32_t cmp_usb_fail_count = 0;      // Number of USB send failures (compressed)
+static uint32_t cmp_max_time_us = 0;         // Worst-case compression time in µs
+
 #define STREAMING_TIMEOUT_MS 150
 volatile uint32_t most_recent_frame_time = 0;
 volatile uint32_t streaming_start_time = 0;
@@ -1191,9 +1199,30 @@ _Bool check_streaming(void){
 			if(total_frames_failed > 0){
 				printf("%lu frames failed\r\n", total_frames_failed);
 			}
+			/* Print compression stats if compression was used */
+			if (cmp_frame_count > 0) {
+				uint32_t avg_ratio = (cmp_total_compressed * 100) / cmp_total_uncompressed;
+				printf("[CMP] Compression stats: %lu frames compressed, avg ratio %lu%% (%lu -> %lu bytes total)\r\n",
+				       (unsigned long)cmp_frame_count, (unsigned long)avg_ratio,
+				       (unsigned long)cmp_total_uncompressed, (unsigned long)cmp_total_compressed);
+				printf("[CMP] Max compress time: %lu us\r\n", (unsigned long)cmp_max_time_us);
+				if (cmp_fail_count > 0) {
+					printf("[CMP] Compression overflows: %lu\r\n", (unsigned long)cmp_fail_count);
+				}
+				if (cmp_usb_fail_count > 0) {
+					printf("[CMP] USB send failures: %lu\r\n", (unsigned long)cmp_usb_fail_count);
+				}
+			}
+			/* Reset all stats */
 			pulse_count = 0;
 			total_frames_sent = 0;
 			total_frames_failed = 0;
+			cmp_total_uncompressed = 0;
+			cmp_total_compressed = 0;
+			cmp_frame_count = 0;
+			cmp_fail_count = 0;
+			cmp_usb_fail_count = 0;
+			cmp_max_time_us = 0;
 			streaming_active = false;
 		}
 	}
@@ -1363,7 +1392,8 @@ _Bool send_histogram_data_cmp(void) {
 	}
 	if (count == 0) {
 		if (!skip_no_data_log) {
-			printf("No cameras have data to send\r\n");
+			printf("[CMP] No cameras have data to send (ready_bits=0x%02X, enabled=0x%02X)\r\n",
+			       ready_bits, event_bits_enabled);
 		}
 		return false;
 	}
@@ -1401,14 +1431,30 @@ _Bool send_histogram_data_cmp(void) {
 	}
 
 	/* --- Compress payload into packet_buffer (after header) --- */
+	int dst_max = HISTO_JSON_BUFFER_SIZE - HISTO_HEADER_SIZE - HISTO_TRAILER_SIZE;
+
+	uint32_t cyc_start = DWT->CYCCNT;
 	int cmp_len = rle_compress(uncmp_payload, p_off,
-	                           packet_buffer + HISTO_HEADER_SIZE,
-	                           HISTO_JSON_BUFFER_SIZE - HISTO_HEADER_SIZE - HISTO_TRAILER_SIZE);
+	                           packet_buffer + HISTO_HEADER_SIZE, dst_max);
+	uint32_t cyc_elapsed = DWT->CYCCNT - cyc_start;
+	uint32_t elapsed_us = cyc_elapsed / (SystemCoreClock / 1000000u);
+
+	if (elapsed_us > cmp_max_time_us) {
+		cmp_max_time_us = elapsed_us;
+	}
+
 	if (cmp_len < 0) {
-		printf("RLE compression failed (output too large)\r\n");
+		cmp_fail_count++;
+		printf("[CMP] FAIL: compression overflow, %d cams, uncmp=%d, dst_max=%d, time=%luus (fail #%lu)\r\n",
+		       count, p_off, dst_max, (unsigned long)elapsed_us, (unsigned long)cmp_fail_count);
 		return false;
 	}
 
+	/* Track compression statistics */
+	cmp_total_uncompressed += (uint32_t)p_off;
+	cmp_total_compressed += (uint32_t)cmp_len;
+	cmp_frame_count++;
+
 	/* --- Header --- */
 	uint32_t total_size = HISTO_HEADER_SIZE + (uint32_t)cmp_len + HISTO_TRAILER_SIZE;
 	int offset = 0;
@@ -1432,7 +1478,11 @@ _Bool send_histogram_data_cmp(void) {
 	uint8_t tx_status = USBD_HISTO_SendData(&hUsbDeviceHS, packet_buffer, offset, 0);
 	if (tx_status != USBD_OK) {
 		status = false;
-		printf("USBD_HISTO_SendData failed: %d\r\n", tx_status);
+		cmp_usb_fail_count++;
+		printf("[CMP] USB FAIL: status=%d, pkt_size=%d, %d cams, cmp_ratio=%d%%, time=%luus (usb_fail #%lu)\r\n",
+		       tx_status, offset, count,
+		       (p_off > 0) ? (cmp_len * 100 / p_off) : 0,
+		       (unsigned long)elapsed_us, (unsigned long)cmp_usb_fail_count);
 	}
 
 	frame_id++;

From b66e9b40e9d64f9d00162fa9f4c96da64df192c3 Mon Sep 17 00:00:00 2001
From: boringethan <ethan@openwater.cc>
Date: Fri, 20 Mar 2026 15:58:46 -0700
Subject: [PATCH 03/11] Improve complier optimization on compression function
 and add some features for brown out detection

---
 Core/Src/camera_manager.c | 21 +++++++++++++
 Core/Src/main.c           | 63 +++++++++++++++++++++++++++++++++++++++
 2 files changed, 84 insertions(+)

diff --git a/Core/Src/camera_manager.c b/Core/Src/camera_manager.c
index 3c473de..0666eac 100644
--- a/Core/Src/camera_manager.c
+++ b/Core/Src/camera_manager.c
@@ -1331,7 +1331,16 @@ _Bool send_histogram_data(void) {
  * Control byte < 0x80: literal run of (ctrl + 1) bytes follow (1–128).
  * Control byte >= 0x80: repeat run – next byte repeated (ctrl - 0x80 + 3) times (3–130).
  * Returns compressed size, or -1 if dst_max would be exceeded.
+ *
+ * NOTE: __attribute__((optimize("O3"))) forces GCC to compile this single
+ * function at -O3 even when the rest of the file is built at -O0 or -Og.
+ * This is critical: rle_compress() runs in interrupt context and must finish
+ * within the ~25 ms frame budget.  Benchmarks on STM32H7 @ 480 MHz:
+ *   -O0 / compressible data:    ~9 ms   (barely OK for zeros/fake data)
+ *   -O0 / incompressible data: ~37 ms   (EXCEEDS budget → SPI overrun!)
+ *   -O3 / incompressible data: ~3–5 ms  (safe margin)
  */
+__attribute__((optimize("O3")))
 static int rle_compress(const uint8_t *src, int src_len, uint8_t *dst, int dst_max) {
 	int si = 0, di = 0;
 	while (si < src_len) {
@@ -1450,6 +1459,18 @@ _Bool send_histogram_data_cmp(void) {
 		return false;
 	}
 
+	/* Warn if compression time is eating into the frame budget.
+	 * Frame period is ~25 ms (40 fps).  Anything above 20 ms risks an SPI
+	 * overrun on the next frame — the symptom is "Camera N stopped posting
+	 * data" followed by cascading overrun errors on SPI3/SPI4. */
+#define CMP_BUDGET_WARNING_US  20000UL  /* 20 ms */
+	if (elapsed_us > CMP_BUDGET_WARNING_US) {
+		printf("[CMP] WARN: compression took %lu us (>20 ms frame budget!), %d cams, "
+		       "%d->%d bytes (ratio %d%%)\r\n",
+		       (unsigned long)elapsed_us, count, p_off, cmp_len,
+		       (p_off > 0) ? (cmp_len * 100 / p_off) : 0);
+	}
+
 	/* Track compression statistics */
 	cmp_total_uncompressed += (uint32_t)p_off;
 	cmp_total_compressed += (uint32_t)cmp_len;
diff --git a/Core/Src/main.c b/Core/Src/main.c
index 213beb3..9048795 100644
--- a/Core/Src/main.c
+++ b/Core/Src/main.c
@@ -165,6 +165,66 @@ static void MX_TIM15_Init(void);
 
 /* Private user code ---------------------------------------------------------*/
 /* USER CODE BEGIN 0 */
+/* Print last reset cause - helps diagnose thermal/brownout (BOR) or watchdog (IWDG) */
+static void print_reset_cause(void)
+{
+	uint8_t first = 1;
+	if (__HAL_RCC_GET_FLAG(RCC_FLAG_BORRST)) {
+		printf("Reset cause: BOR (Brown-Out) - possible thermal/voltage droop\r\n");
+		first = 0;
+	}
+	if (__HAL_RCC_GET_FLAG(RCC_FLAG_PORRST)) {
+		printf("%sReset cause: POR (Power-On)\r\n", first ? "" : "         ");
+		first = 0;
+	}
+	if (__HAL_RCC_GET_FLAG(RCC_FLAG_PINRST)) {
+		printf("%sReset cause: PIN (External NRST)\r\n", first ? "" : "         ");
+		first = 0;
+	}
+	if (__HAL_RCC_GET_FLAG(RCC_FLAG_SFTRST)) {
+		printf("%sReset cause: Software\r\n", first ? "" : "         ");
+		first = 0;
+	}
+#if defined(RCC_FLAG_IWDG1RST)
+	if (__HAL_RCC_GET_FLAG(RCC_FLAG_IWDG1RST)) {
+		printf("%sReset cause: IWDG (Independent Watchdog) - possible lockup/thermal\r\n", first ? "" : "         ");
+		first = 0;
+	}
+#endif
+#if defined(RCC_FLAG_WWDG1RST)
+	if (__HAL_RCC_GET_FLAG(RCC_FLAG_WWDG1RST)) {
+		printf("%sReset cause: WWDG (Window Watchdog)\r\n", first ? "" : "         ");
+		first = 0;
+	}
+#endif
+#if defined(RCC_FLAG_LPWR1RST)
+	if (__HAL_RCC_GET_FLAG(RCC_FLAG_LPWR1RST)) {
+		printf("%sReset cause: LPWR (Low-power)\r\n", first ? "" : "         ");
+		first = 0;
+	}
+#endif
+	if (first) {
+		printf("Reset cause: unknown\r\n");
+	}
+	__HAL_RCC_CLEAR_RESET_FLAGS();
+}
+
+/* Poll MCU junction temperature; print warning if above high threshold */
+#define MCU_TEMP_CHECK_MS  5000u
+static void poll_mcu_temperature(void)
+{
+	static uint32_t last_check_ms = 0;
+	uint32_t now = HAL_GetTick();
+	if ((last_check_ms != 0u) && ((now - last_check_ms) < MCU_TEMP_CHECK_MS)) {
+		return;
+	}
+	last_check_ms = now;
+	uint32_t level = HAL_PWREx_GetTemperatureLevel();
+	if (level == PWR_TEMP_ABOVE_HIGH_THRESHOLD) {
+		printf("[THERMAL] MCU junction temp ABOVE HIGH THRESHOLD - reduce load/cooling!\r\n");
+	}
+}
+
 static void PrintI2CSpeed(I2C_HandleTypeDef *hi2c)
 {
   // Assuming the timing is configured in I2C_TIMINGR register
@@ -268,6 +328,8 @@ int main(void)
        FW_BUILD_TIME_STRING);
   printf("CPU Clock Frequency: %lu MHz\r\n",
          HAL_RCC_GetSysClockFreq() / 1000000);
+  print_reset_cause();
+  HAL_PWREx_EnableMonitoring();  /* Enable junction temp (TEMPH/TEMPL) monitoring */
   printf("Initializing, please wait ...\r\n");
 
   // enable HS USB MUX
@@ -332,6 +394,7 @@ int main(void)
     /* USER CODE BEGIN 3 */
   	comms_host_check_received(); // check comms  
     check_streaming();
+    poll_mcu_temperature();  /* Print warning if MCU junction temp above threshold */
   }
 
   /* USER CODE END 3 */

From f6486e3a00ce53fb7f22a99c813356d23825e3e2 Mon Sep 17 00:00:00 2001
From: boringethan <ethan@openwater.cc>
Date: Mon, 23 Mar 2026 11:16:23 -0700
Subject: [PATCH 04/11] decrease CMP_BUDGET_WARNING_US

---
 Core/Src/camera_manager.c |    7 +-
 Core/Src/main.c           | 4099 +++++++++++++++++++------------------
 Core/Src/stm32h7xx_it.c   |   75 +-
 STM32H743VIHX_FLASH.ld    |    9 +
 4 files changed, 2160 insertions(+), 2030 deletions(-)

diff --git a/Core/Src/camera_manager.c b/Core/Src/camera_manager.c
index 0666eac..7680345 100644
--- a/Core/Src/camera_manager.c
+++ b/Core/Src/camera_manager.c
@@ -1460,10 +1460,9 @@ _Bool send_histogram_data_cmp(void) {
 	}
 
 	/* Warn if compression time is eating into the frame budget.
-	 * Frame period is ~25 ms (40 fps).  Anything above 20 ms risks an SPI
-	 * overrun on the next frame — the symptom is "Camera N stopped posting
-	 * data" followed by cascading overrun errors on SPI3/SPI4. */
-#define CMP_BUDGET_WARNING_US  20000UL  /* 20 ms */
+	 * Frame period is ~25 ms (40 fps).  Anything above 10 ms is a yellow
+	 * flag worth watching; above ~20 ms risks SPI overrun on the next frame. */
+#define CMP_BUDGET_WARNING_US  10000UL  /* 10 ms */
 	if (elapsed_us > CMP_BUDGET_WARNING_US) {
 		printf("[CMP] WARN: compression took %lu us (>20 ms frame budget!), %d cams, "
 		       "%d->%d bytes (ratio %d%%)\r\n",
diff --git a/Core/Src/main.c b/Core/Src/main.c
index 9048795..0cdf17f 100644
--- a/Core/Src/main.c
+++ b/Core/Src/main.c
@@ -1,2022 +1,2077 @@
-/* USER CODE BEGIN Header */
-/**
- ******************************************************************************
- * @file           : main.c
- * @brief          : Main program body
- ******************************************************************************
- * @attention
- *
- * Copyright (c) 2024 STMicroelectronics.
- * All rights reserved.
- *
- * This software is licensed under terms that can be found in the LICENSE file
- * in the root directory of this software component.
- * If no LICENSE file comes with this software, it is provided AS-IS.
- *
- ******************************************************************************
- */
-/* USER CODE END Header */
-/* Includes ------------------------------------------------------------------*/
-#include "main.h"
-
-/* Private includes ----------------------------------------------------------*/
-/* USER CODE BEGIN Includes */
-#include "0X02C1B.h"
-#include "usb_device.h"
-#include "uart_comms.h"
-#include "usbd_histo.h"
-#include "usbd_imu.h"
-#include "usbd_comms.h"
-#include "logging.h"
-#include "utils.h"
-#include "i2c_master.h"
-#include "histo_fake.h"
-#include "ICM20948.h"
-#include "camera_manager.h"
-
-#include <stdio.h>
-#include <string.h>
-#include <stdbool.h>
-
-#include "stm32h7xx_ll_tim.h"
-
-/* USER CODE END Includes */
-
-/* Private typedef -----------------------------------------------------------*/
-/* USER CODE BEGIN PTD */
-
-/* External references to USB endpoint status variables */
-extern volatile uint8_t tx_flag;  // From uart_comms.c
-extern volatile uint8_t comms_ep_data;  // From usbd_comms.c (__IO is volatile)
-extern volatile uint8_t histo_ep_data;  // From usbd_histo.c (__IO is volatile)
-extern volatile uint8_t imu_ep_data;    // From usbd_imu.c (__IO is volatile)
-
-/* USER CODE END PTD */
-
-/* Private define ------------------------------------------------------------*/
-/* USER CODE BEGIN PD */
-/* USER CODE END PD */
-
-/* Private macro -------------------------------------------------------------*/
-/* USER CODE BEGIN PM */
-
-/* USER CODE END PM */
-
-/* Private variables ---------------------------------------------------------*/
-
-CRC_HandleTypeDef hcrc;
-
-I2C_HandleTypeDef hi2c1;
-
-RNG_HandleTypeDef hrng;
-
-SPI_HandleTypeDef hspi2;
-SPI_HandleTypeDef hspi3;
-SPI_HandleTypeDef hspi4;
-SPI_HandleTypeDef hspi6;
-DMA_HandleTypeDef hdma_spi2_rx;
-DMA_HandleTypeDef hdma_spi3_rx;
-DMA_HandleTypeDef hdma_spi4_rx;
-DMA_HandleTypeDef hdma_spi6_rx;
-
-TIM_HandleTypeDef htim2;
-TIM_HandleTypeDef htim4;
-TIM_HandleTypeDef htim5;
-TIM_HandleTypeDef htim8;
-TIM_HandleTypeDef htim14;
-TIM_HandleTypeDef htim15;
-TIM_HandleTypeDef htim16;
-
-UART_HandleTypeDef huart4;
-USART_HandleTypeDef husart1;
-USART_HandleTypeDef husart2;
-USART_HandleTypeDef husart3;
-USART_HandleTypeDef husart6;
-DMA_HandleTypeDef hdma_uart4_rx;
-DMA_HandleTypeDef hdma_uart4_tx;
-DMA_HandleTypeDef hdma_usart1_rx;
-DMA_HandleTypeDef hdma_usart2_rx;
-DMA_HandleTypeDef hdma_usart3_rx;
-DMA_HandleTypeDef hdma_usart6_rx;
-
-DMA_HandleTypeDef hdma_memtomem_dma2_stream1;
-/* USER CODE BEGIN PV */
-
-uint8_t rxBuffer[COMMAND_MAX_SIZE]  __attribute__((aligned(4)));
-uint8_t txBuffer[COMMAND_MAX_SIZE];
-uint32_t bitstream_len;
-__attribute__((section(".RAM_D1"))) uint8_t bitstream_buffer[MAX_BITSTREAM_SIZE]; // 160KB buffer
-
-volatile uint8_t event_bits = 0x00;         // holds the event bits to be flipped
-volatile uint8_t event_bits_enabled = 0x00; // holds the event bits for the cameras to be enabled
-volatile uint16_t pulse_count = 0;
-extern uint32_t imu_frame_counter;
-volatile bool _enter_dfu = false;
-
-ICM_Axis3D a;
-ICM_Axis3D m;
-ICM_Axis3D g;
-float t;
-char usb_buf[128];
-// Debug flags (sensor debug and fake data are controlled via OW_CMD_DEBUG_FLAGS)
-bool uart_stream = false;
-
-uint16_t fail_count = 0;
-
-extern USBD_HandleTypeDef hUsbDeviceHS;
-
-const char *bit_rep[16] = {
-    [ 0] = "0000", [ 1] = "0001", [ 2] = "0010", [ 3] = "0011",
-    [ 4] = "0100", [ 5] = "0101", [ 6] = "0110", [ 7] = "0111",
-    [ 8] = "1000", [ 9] = "1001", [10] = "1010", [11] = "1011",
-    [12] = "1100", [13] = "1101", [14] = "1110", [15] = "1111",
-};
-/* USER CODE END PV */
-
-/* Private function prototypes -----------------------------------------------*/
-void SystemClock_Config(void);
-void PeriphCommonClock_Config(void);
-static void MPU_Config(void);
-static void MX_GPIO_Init(void);
-static void MX_DMA_Init(void);
-static void MX_BDMA_Init(void);
-static void MX_CRC_Init(void);
-static void MX_I2C1_Init(void);
-static void MX_RNG_Init(void);
-static void MX_SPI2_Init(void);
-static void MX_SPI3_Init(void);
-static void MX_SPI4_Init(void);
-static void MX_SPI6_Init(void);
-static void MX_TIM2_Init(void);
-static void MX_UART4_Init(void);
-static void MX_USART6_Init(void);
-static void MX_TIM8_Init(void);
-static void MX_TIM4_Init(void);
-static void MX_USART1_Init(void);
-static void MX_USART2_Init(void);
-static void MX_USART3_Init(void);
-static void MX_TIM14_Init(void);
-static void MX_TIM16_Init(void);
-static void MX_TIM5_Init(void);
-static void MX_TIM15_Init(void);
-/* USER CODE BEGIN PFP */
-
-/* USER CODE END PFP */
-
-/* Private user code ---------------------------------------------------------*/
-/* USER CODE BEGIN 0 */
-/* Print last reset cause - helps diagnose thermal/brownout (BOR) or watchdog (IWDG) */
-static void print_reset_cause(void)
-{
-	uint8_t first = 1;
-	if (__HAL_RCC_GET_FLAG(RCC_FLAG_BORRST)) {
-		printf("Reset cause: BOR (Brown-Out) - possible thermal/voltage droop\r\n");
-		first = 0;
-	}
-	if (__HAL_RCC_GET_FLAG(RCC_FLAG_PORRST)) {
-		printf("%sReset cause: POR (Power-On)\r\n", first ? "" : "         ");
-		first = 0;
-	}
-	if (__HAL_RCC_GET_FLAG(RCC_FLAG_PINRST)) {
-		printf("%sReset cause: PIN (External NRST)\r\n", first ? "" : "         ");
-		first = 0;
-	}
-	if (__HAL_RCC_GET_FLAG(RCC_FLAG_SFTRST)) {
-		printf("%sReset cause: Software\r\n", first ? "" : "         ");
-		first = 0;
-	}
-#if defined(RCC_FLAG_IWDG1RST)
-	if (__HAL_RCC_GET_FLAG(RCC_FLAG_IWDG1RST)) {
-		printf("%sReset cause: IWDG (Independent Watchdog) - possible lockup/thermal\r\n", first ? "" : "         ");
-		first = 0;
-	}
-#endif
-#if defined(RCC_FLAG_WWDG1RST)
-	if (__HAL_RCC_GET_FLAG(RCC_FLAG_WWDG1RST)) {
-		printf("%sReset cause: WWDG (Window Watchdog)\r\n", first ? "" : "         ");
-		first = 0;
-	}
-#endif
-#if defined(RCC_FLAG_LPWR1RST)
-	if (__HAL_RCC_GET_FLAG(RCC_FLAG_LPWR1RST)) {
-		printf("%sReset cause: LPWR (Low-power)\r\n", first ? "" : "         ");
-		first = 0;
-	}
-#endif
-	if (first) {
-		printf("Reset cause: unknown\r\n");
-	}
-	__HAL_RCC_CLEAR_RESET_FLAGS();
-}
-
-/* Poll MCU junction temperature; print warning if above high threshold */
-#define MCU_TEMP_CHECK_MS  5000u
-static void poll_mcu_temperature(void)
-{
-	static uint32_t last_check_ms = 0;
-	uint32_t now = HAL_GetTick();
-	if ((last_check_ms != 0u) && ((now - last_check_ms) < MCU_TEMP_CHECK_MS)) {
-		return;
-	}
-	last_check_ms = now;
-	uint32_t level = HAL_PWREx_GetTemperatureLevel();
-	if (level == PWR_TEMP_ABOVE_HIGH_THRESHOLD) {
-		printf("[THERMAL] MCU junction temp ABOVE HIGH THRESHOLD - reduce load/cooling!\r\n");
-	}
-}
-
-static void PrintI2CSpeed(I2C_HandleTypeDef *hi2c)
-{
-  // Assuming the timing is configured in I2C_TIMINGR register
-  uint32_t timing = hi2c->Init.Timing;
-  uint32_t presc = (timing >> 28) & 0xF;
-  uint32_t scll = (timing >> 0) & 0xFF;
-  uint32_t sclh = (timing >> 8) & 0xFF;
-
-  // Get the I2C clock source frequency
-  uint32_t i2c_clk_freq = HAL_RCC_GetPCLK1Freq();
-
-  // Calculate SCL speed (frequency) in Hz
-  uint32_t scl_freq = i2c_clk_freq / ((presc + 1) * (scll + 1 + sclh + 1));
-
-  // Print I2C speed
-  printf("I2C Speed: %ld Hz\r\n", scl_freq); // Print the I2C speed in kHz
-}
-
-/* USER CODE END 0 */
-
-/**
-  * @brief  The application entry point.
-  * @retval int
-  */
-int main(void)
-{
-
-  /* USER CODE BEGIN 1 */
-
-  /* USER CODE END 1 */
-
-  /* MPU Configuration--------------------------------------------------------*/
-  MPU_Config();
-
-  /* MCU Configuration--------------------------------------------------------*/
-
-  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
-  HAL_Init();
-
-  /* USER CODE BEGIN Init */
-
-  /* USER CODE END Init */
-
-  /* Configure the system clock */
-  SystemClock_Config();
-
-  /* Configure the peripherals common clocks */
-  PeriphCommonClock_Config();
-
-  /* USER CODE BEGIN SysInit */
-
-  /* USER CODE END SysInit */
-
-  /* Initialize all configured peripherals */
-  MX_GPIO_Init();
-  MX_DMA_Init();
-  MX_BDMA_Init();
-  MX_CRC_Init();
-  MX_I2C1_Init();
-  MX_RNG_Init();
-  MX_SPI2_Init();
-  MX_SPI3_Init();
-  MX_SPI4_Init();
-  MX_SPI6_Init();
-  MX_TIM2_Init();
-  MX_UART4_Init();
-  MX_USART6_Init();
-  MX_TIM8_Init();
-  MX_TIM4_Init();
-  MX_USART1_Init();
-  MX_USART2_Init();
-  MX_USART3_Init();
-  MX_TIM14_Init();
-  MX_TIM16_Init();
-  MX_TIM5_Init();
-  MX_TIM15_Init();
-  /* USER CODE BEGIN 2 */
-  
-  if (HAL_TIM_Base_Start(&htim5) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  
-  init_dma_logging();
-
-  DWT_Init();
-
-  // enable I2C MUX
-  HAL_GPIO_WritePin(MUX_RESET_GPIO_Port, MUX_RESET_Pin, GPIO_PIN_RESET);
-
-  // enable USB PHY
-  HAL_GPIO_WritePin(USB_RESET_GPIO_Port, USB_RESET_Pin, GPIO_PIN_RESET);
-
-  printf("\033c");
-  fflush(stdout);
-  delay_ms(500);
-  printf("Openwater open-MOTION Aggregator\r\n\r\n");
-  printf("FW: %s (%s)\r\nDate: %s\r\n",
-       FW_VERSION_STRING,
-       FW_SHA_STRING,
-       FW_BUILD_TIME_STRING);
-  printf("CPU Clock Frequency: %lu MHz\r\n",
-         HAL_RCC_GetSysClockFreq() / 1000000);
-  print_reset_cause();
-  HAL_PWREx_EnableMonitoring();  /* Enable junction temp (TEMPH/TEMPL) monitoring */
-  printf("Initializing, please wait ...\r\n");
-
-  // enable HS USB MUX
-  HAL_GPIO_WritePin(USB_MUX_GPIO_Port, USB_MUX_Pin, GPIO_PIN_RESET);
-
-  // enable I2C MUX
-  HAL_GPIO_WritePin(MUX_RESET_GPIO_Port, MUX_RESET_Pin, GPIO_PIN_SET);
-
-  HAL_GPIO_WritePin(FS_OUT_EN_GPIO_Port, FS_OUT_EN_Pin, GPIO_PIN_RESET);  //enable Framesync output
-
-  // test i2c
-  PrintI2CSpeed(&hi2c1);
-  // I2C_scan(&hi2c1, NULL, 0, true);
-  delay_ms(100);
-  X02C1B_FSIN_EXT_disable();
-  GPIO_SetOutput(FSIN_EN_GPIO_Port, FSIN_EN_Pin, GPIO_PIN_RESET); // disable fsin output
-
-  if (ICM_WHOAMI() == ICM20948_EXPECTED_ID)
-  {
-	if(ICM_Init() != HAL_OK){
-		printf("Failed to initialize IMU\r\n\n");
-	}
-	else
-	{
-		printf("IMU detected\r\n");
-	    delay_ms(100);
-	    if(verbose_on) ICM_DumpRegisters();
-	}
-  }
-  else
-  {
-    printf("IMU NOT detected\r\n\n");
-  }
-
-  delay_ms(10);
-
-  HAL_GPIO_WritePin(USB_RESET_GPIO_Port, USB_RESET_Pin, GPIO_PIN_SET);
-
-  HAL_GPIO_WritePin(ERROR_LED_GPIO_Port, ERROR_LED_Pin, GPIO_PIN_SET);
-
-  init_camera_sensors(); // init structures and camera configs
-
-  // Select default camera
-  TCA9548A_SelectChannel(&hi2c1, 0x70, get_active_cam()->i2c_target);
-
-  delay_ms(250);
-  MX_USB_DEVICE_Init();
-  delay_ms(1000);
-  //GPIO_SetHiZ(GPIOA, GPIO_PIN_2);
-
-  comms_host_start();
-  // fill_frame_buffers();
-  printf("System Running\r\n");
-  /* USER CODE END 2 */
-
-  /* Infinite loop */
-  /* USER CODE BEGIN WHILE */
-  while (1)
-  {
-    /* USER CODE END WHILE */
-
-    /* USER CODE BEGIN 3 */
-  	comms_host_check_received(); // check comms  
-    check_streaming();
-    poll_mcu_temperature();  /* Print warning if MCU junction temp above threshold */
-  }
-
-  /* USER CODE END 3 */
-}
-
-/**
-  * @brief System Clock Configuration
-  * @retval None
-  */
-void SystemClock_Config(void)
-{
-  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
-  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
-
-  /** Supply configuration update enable
-  */
-  HAL_PWREx_ConfigSupply(PWR_LDO_SUPPLY);
-
-  /** Configure the main internal regulator output voltage
-  */
-  __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE0);
-
-  while(!__HAL_PWR_GET_FLAG(PWR_FLAG_VOSRDY)) {}
-
-  /** 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.PLL.PLLState = RCC_PLL_ON;
-  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
-  RCC_OscInitStruct.PLL.PLLM = 3;
-  RCC_OscInitStruct.PLL.PLLN = 120;
-  RCC_OscInitStruct.PLL.PLLP = 2;
-  RCC_OscInitStruct.PLL.PLLQ = 20;
-  RCC_OscInitStruct.PLL.PLLR = 2;
-  RCC_OscInitStruct.PLL.PLLRGE = RCC_PLL1VCIRANGE_3;
-  RCC_OscInitStruct.PLL.PLLVCOSEL = RCC_PLL1VCOWIDE;
-  RCC_OscInitStruct.PLL.PLLFRACN = 0;
-  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
-  {
-    Error_Handler();
-  }
-
-  /** Initializes the CPU, AHB and APB buses clocks
-  */
-  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
-                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2
-                              |RCC_CLOCKTYPE_D3PCLK1|RCC_CLOCKTYPE_D1PCLK1;
-  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
-  RCC_ClkInitStruct.SYSCLKDivider = RCC_SYSCLK_DIV1;
-  RCC_ClkInitStruct.AHBCLKDivider = RCC_HCLK_DIV2;
-  RCC_ClkInitStruct.APB3CLKDivider = RCC_APB3_DIV2;
-  RCC_ClkInitStruct.APB1CLKDivider = RCC_APB1_DIV2;
-  RCC_ClkInitStruct.APB2CLKDivider = RCC_APB2_DIV2;
-  RCC_ClkInitStruct.APB4CLKDivider = RCC_APB4_DIV2;
-
-  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_4) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  __HAL_RCC_PLL2CLKOUT_ENABLE(RCC_PLL2_DIVP);
-  HAL_RCC_MCOConfig(RCC_MCO2, RCC_MCO2SOURCE_PLL2PCLK, RCC_MCODIV_5);
-}
-
-/**
-  * @brief Peripherals Common Clock Configuration
-  * @retval None
-  */
-void PeriphCommonClock_Config(void)
-{
-  RCC_PeriphCLKInitTypeDef PeriphClkInitStruct = {0};
-
-  /** Initializes the peripherals clock
-  */
-  PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_SPI3|RCC_PERIPHCLK_SPI2;
-  PeriphClkInitStruct.PLL2.PLL2M = 6;
-  PeriphClkInitStruct.PLL2.PLL2N = 120;
-  PeriphClkInitStruct.PLL2.PLL2P = 4;
-  PeriphClkInitStruct.PLL2.PLL2Q = 2;
-  PeriphClkInitStruct.PLL2.PLL2R = 2;
-  PeriphClkInitStruct.PLL2.PLL2RGE = RCC_PLL2VCIRANGE_2;
-  PeriphClkInitStruct.PLL2.PLL2VCOSEL = RCC_PLL2VCOWIDE;
-  PeriphClkInitStruct.PLL2.PLL2FRACN = 0;
-  PeriphClkInitStruct.Spi123ClockSelection = RCC_SPI123CLKSOURCE_PLL2;
-  if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK)
-  {
-    Error_Handler();
-  }
-}
-
-/**
-  * @brief CRC Initialization Function
-  * @param None
-  * @retval None
-  */
-static void MX_CRC_Init(void)
-{
-
-  /* USER CODE BEGIN CRC_Init 0 */
-
-  /* USER CODE END CRC_Init 0 */
-
-  /* USER CODE BEGIN CRC_Init 1 */
-
-  /* USER CODE END CRC_Init 1 */
-  hcrc.Instance = CRC;
-  hcrc.Init.DefaultPolynomialUse = DEFAULT_POLYNOMIAL_ENABLE;
-  hcrc.Init.DefaultInitValueUse = DEFAULT_INIT_VALUE_ENABLE;
-  hcrc.Init.InputDataInversionMode = CRC_INPUTDATA_INVERSION_NONE;
-  hcrc.Init.OutputDataInversionMode = CRC_OUTPUTDATA_INVERSION_DISABLE;
-  hcrc.InputDataFormat = CRC_INPUTDATA_FORMAT_BYTES;
-  if (HAL_CRC_Init(&hcrc) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  /* USER CODE BEGIN CRC_Init 2 */
-
-  /* USER CODE END CRC_Init 2 */
-
-}
-
-/**
-  * @brief I2C1 Initialization Function
-  * @param None
-  * @retval None
-  */
-static void MX_I2C1_Init(void)
-{
-
-  /* USER CODE BEGIN I2C1_Init 0 */
-
-  /* USER CODE END I2C1_Init 0 */
-
-  /* USER CODE BEGIN I2C1_Init 1 */
-
-  /* USER CODE END I2C1_Init 1 */
-  hi2c1.Instance = I2C1;
-  hi2c1.Init.Timing = 0x00B03FDB;
-  hi2c1.Init.OwnAddress1 = 0;
-  hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
-  hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
-  hi2c1.Init.OwnAddress2 = 0;
-  hi2c1.Init.OwnAddress2Masks = I2C_OA2_NOMASK;
-  hi2c1.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
-  hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
-  if (HAL_I2C_Init(&hi2c1) != HAL_OK)
-  {
-    Error_Handler();
-  }
-
-  /** Configure Analogue filter
-  */
-  if (HAL_I2CEx_ConfigAnalogFilter(&hi2c1, I2C_ANALOGFILTER_ENABLE) != HAL_OK)
-  {
-    Error_Handler();
-  }
-
-  /** Configure Digital filter
-  */
-  if (HAL_I2CEx_ConfigDigitalFilter(&hi2c1, 0) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  /* USER CODE BEGIN I2C1_Init 2 */
-
-  /* USER CODE END I2C1_Init 2 */
-
-}
-
-/**
-  * @brief RNG Initialization Function
-  * @param None
-  * @retval None
-  */
-static void MX_RNG_Init(void)
-{
-
-  /* USER CODE BEGIN RNG_Init 0 */
-
-  /* USER CODE END RNG_Init 0 */
-
-  /* USER CODE BEGIN RNG_Init 1 */
-
-  /* USER CODE END RNG_Init 1 */
-  hrng.Instance = RNG;
-  hrng.Init.ClockErrorDetection = RNG_CED_ENABLE;
-  if (HAL_RNG_Init(&hrng) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  /* USER CODE BEGIN RNG_Init 2 */
-
-  /* USER CODE END RNG_Init 2 */
-
-}
-
-/**
-  * @brief SPI2 Initialization Function
-  * @param None
-  * @retval None
-  */
-static void MX_SPI2_Init(void)
-{
-
-  /* USER CODE BEGIN SPI2_Init 0 */
-
-  /* USER CODE END SPI2_Init 0 */
-
-  /* USER CODE BEGIN SPI2_Init 1 */
-
-  /* USER CODE END SPI2_Init 1 */
-  /* SPI2 parameter configuration*/
-  hspi2.Instance = SPI2;
-  hspi2.Init.Mode = SPI_MODE_SLAVE;
-  hspi2.Init.Direction = SPI_DIRECTION_2LINES;
-  hspi2.Init.DataSize = SPI_DATASIZE_8BIT;
-  hspi2.Init.CLKPolarity = SPI_POLARITY_HIGH;
-  hspi2.Init.CLKPhase = SPI_PHASE_2EDGE;
-  hspi2.Init.NSS = SPI_NSS_SOFT;
-  hspi2.Init.FirstBit = SPI_FIRSTBIT_LSB;
-  hspi2.Init.TIMode = SPI_TIMODE_DISABLE;
-  hspi2.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
-  hspi2.Init.CRCPolynomial = 0x0;
-  hspi2.Init.NSSPMode = SPI_NSS_PULSE_DISABLE;
-  hspi2.Init.NSSPolarity = SPI_NSS_POLARITY_LOW;
-  hspi2.Init.FifoThreshold = SPI_FIFO_THRESHOLD_01DATA;
-  hspi2.Init.TxCRCInitializationPattern = SPI_CRC_INITIALIZATION_ALL_ZERO_PATTERN;
-  hspi2.Init.RxCRCInitializationPattern = SPI_CRC_INITIALIZATION_ALL_ZERO_PATTERN;
-  hspi2.Init.MasterSSIdleness = SPI_MASTER_SS_IDLENESS_00CYCLE;
-  hspi2.Init.MasterInterDataIdleness = SPI_MASTER_INTERDATA_IDLENESS_00CYCLE;
-  hspi2.Init.MasterReceiverAutoSusp = SPI_MASTER_RX_AUTOSUSP_DISABLE;
-  hspi2.Init.MasterKeepIOState = SPI_MASTER_KEEP_IO_STATE_DISABLE;
-  hspi2.Init.IOSwap = SPI_IO_SWAP_DISABLE;
-  if (HAL_SPI_Init(&hspi2) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  /* USER CODE BEGIN SPI2_Init 2 */
-
-  /* USER CODE END SPI2_Init 2 */
-
-}
-
-/**
-  * @brief SPI3 Initialization Function
-  * @param None
-  * @retval None
-  */
-static void MX_SPI3_Init(void)
-{
-
-  /* USER CODE BEGIN SPI3_Init 0 */
-
-  /* USER CODE END SPI3_Init 0 */
-
-  /* USER CODE BEGIN SPI3_Init 1 */
-
-  /* USER CODE END SPI3_Init 1 */
-  /* SPI3 parameter configuration*/
-  hspi3.Instance = SPI3;
-  hspi3.Init.Mode = SPI_MODE_SLAVE;
-  hspi3.Init.Direction = SPI_DIRECTION_2LINES;
-  hspi3.Init.DataSize = SPI_DATASIZE_8BIT;
-  hspi3.Init.CLKPolarity = SPI_POLARITY_HIGH;
-  hspi3.Init.CLKPhase = SPI_PHASE_2EDGE;
-  hspi3.Init.NSS = SPI_NSS_SOFT;
-  hspi3.Init.FirstBit = SPI_FIRSTBIT_LSB;
-  hspi3.Init.TIMode = SPI_TIMODE_DISABLE;
-  hspi3.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
-  hspi3.Init.CRCPolynomial = 0x0;
-  hspi3.Init.NSSPMode = SPI_NSS_PULSE_DISABLE;
-  hspi3.Init.NSSPolarity = SPI_NSS_POLARITY_LOW;
-  hspi3.Init.FifoThreshold = SPI_FIFO_THRESHOLD_01DATA;
-  hspi3.Init.TxCRCInitializationPattern = SPI_CRC_INITIALIZATION_ALL_ZERO_PATTERN;
-  hspi3.Init.RxCRCInitializationPattern = SPI_CRC_INITIALIZATION_ALL_ZERO_PATTERN;
-  hspi3.Init.MasterSSIdleness = SPI_MASTER_SS_IDLENESS_00CYCLE;
-  hspi3.Init.MasterInterDataIdleness = SPI_MASTER_INTERDATA_IDLENESS_00CYCLE;
-  hspi3.Init.MasterReceiverAutoSusp = SPI_MASTER_RX_AUTOSUSP_DISABLE;
-  hspi3.Init.MasterKeepIOState = SPI_MASTER_KEEP_IO_STATE_DISABLE;
-  hspi3.Init.IOSwap = SPI_IO_SWAP_DISABLE;
-  if (HAL_SPI_Init(&hspi3) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  /* USER CODE BEGIN SPI3_Init 2 */
-
-  /* USER CODE END SPI3_Init 2 */
-
-}
-
-/**
-  * @brief SPI4 Initialization Function
-  * @param None
-  * @retval None
-  */
-static void MX_SPI4_Init(void)
-{
-
-  /* USER CODE BEGIN SPI4_Init 0 */
-
-  /* USER CODE END SPI4_Init 0 */
-
-  /* USER CODE BEGIN SPI4_Init 1 */
-
-  /* USER CODE END SPI4_Init 1 */
-  /* SPI4 parameter configuration*/
-  hspi4.Instance = SPI4;
-  hspi4.Init.Mode = SPI_MODE_SLAVE;
-  hspi4.Init.Direction = SPI_DIRECTION_2LINES;
-  hspi4.Init.DataSize = SPI_DATASIZE_8BIT;
-  hspi4.Init.CLKPolarity = SPI_POLARITY_HIGH;
-  hspi4.Init.CLKPhase = SPI_PHASE_2EDGE;
-  hspi4.Init.NSS = SPI_NSS_SOFT;
-  hspi4.Init.FirstBit = SPI_FIRSTBIT_LSB;
-  hspi4.Init.TIMode = SPI_TIMODE_DISABLE;
-  hspi4.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
-  hspi4.Init.CRCPolynomial = 0x0;
-  hspi4.Init.NSSPMode = SPI_NSS_PULSE_DISABLE;
-  hspi4.Init.NSSPolarity = SPI_NSS_POLARITY_LOW;
-  hspi4.Init.FifoThreshold = SPI_FIFO_THRESHOLD_01DATA;
-  hspi4.Init.TxCRCInitializationPattern = SPI_CRC_INITIALIZATION_ALL_ZERO_PATTERN;
-  hspi4.Init.RxCRCInitializationPattern = SPI_CRC_INITIALIZATION_ALL_ZERO_PATTERN;
-  hspi4.Init.MasterSSIdleness = SPI_MASTER_SS_IDLENESS_00CYCLE;
-  hspi4.Init.MasterInterDataIdleness = SPI_MASTER_INTERDATA_IDLENESS_00CYCLE;
-  hspi4.Init.MasterReceiverAutoSusp = SPI_MASTER_RX_AUTOSUSP_DISABLE;
-  hspi4.Init.MasterKeepIOState = SPI_MASTER_KEEP_IO_STATE_DISABLE;
-  hspi4.Init.IOSwap = SPI_IO_SWAP_DISABLE;
-  if (HAL_SPI_Init(&hspi4) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  /* USER CODE BEGIN SPI4_Init 2 */
-
-  /* USER CODE END SPI4_Init 2 */
-
-}
-
-/**
-  * @brief SPI6 Initialization Function
-  * @param None
-  * @retval None
-  */
-static void MX_SPI6_Init(void)
-{
-
-  /* USER CODE BEGIN SPI6_Init 0 */
-
-  /* USER CODE END SPI6_Init 0 */
-
-  /* USER CODE BEGIN SPI6_Init 1 */
-
-  /* USER CODE END SPI6_Init 1 */
-  /* SPI6 parameter configuration*/
-  hspi6.Instance = SPI6;
-  hspi6.Init.Mode = SPI_MODE_SLAVE;
-  hspi6.Init.Direction = SPI_DIRECTION_2LINES;
-  hspi6.Init.DataSize = SPI_DATASIZE_8BIT;
-  hspi6.Init.CLKPolarity = SPI_POLARITY_HIGH;
-  hspi6.Init.CLKPhase = SPI_PHASE_2EDGE;
-  hspi6.Init.NSS = SPI_NSS_SOFT;
-  hspi6.Init.FirstBit = SPI_FIRSTBIT_LSB;
-  hspi6.Init.TIMode = SPI_TIMODE_DISABLE;
-  hspi6.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
-  hspi6.Init.CRCPolynomial = 0x0;
-  hspi6.Init.NSSPMode = SPI_NSS_PULSE_DISABLE;
-  hspi6.Init.NSSPolarity = SPI_NSS_POLARITY_LOW;
-  hspi6.Init.FifoThreshold = SPI_FIFO_THRESHOLD_01DATA;
-  hspi6.Init.TxCRCInitializationPattern = SPI_CRC_INITIALIZATION_ALL_ZERO_PATTERN;
-  hspi6.Init.RxCRCInitializationPattern = SPI_CRC_INITIALIZATION_ALL_ZERO_PATTERN;
-  hspi6.Init.MasterSSIdleness = SPI_MASTER_SS_IDLENESS_00CYCLE;
-  hspi6.Init.MasterInterDataIdleness = SPI_MASTER_INTERDATA_IDLENESS_00CYCLE;
-  hspi6.Init.MasterReceiverAutoSusp = SPI_MASTER_RX_AUTOSUSP_DISABLE;
-  hspi6.Init.MasterKeepIOState = SPI_MASTER_KEEP_IO_STATE_DISABLE;
-  hspi6.Init.IOSwap = SPI_IO_SWAP_DISABLE;
-  if (HAL_SPI_Init(&hspi6) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  /* USER CODE BEGIN SPI6_Init 2 */
-
-  /* USER CODE END SPI6_Init 2 */
-
-}
-
-/**
-  * @brief TIM2 Initialization Function
-  * @param None
-  * @retval None
-  */
-static void MX_TIM2_Init(void)
-{
-
-  /* USER CODE BEGIN TIM2_Init 0 */
-
-  /* USER CODE END TIM2_Init 0 */
-
-  TIM_ClockConfigTypeDef sClockSourceConfig = {0};
-  TIM_MasterConfigTypeDef sMasterConfig = {0};
-  TIM_OC_InitTypeDef sConfigOC = {0};
-
-  /* USER CODE BEGIN TIM2_Init 1 */
-
-  /* USER CODE END TIM2_Init 1 */
-  htim2.Instance = TIM2;
-  htim2.Init.Prescaler = 0;
-  htim2.Init.CounterMode = TIM_COUNTERMODE_UP;
-  htim2.Init.Period = 4294967295;
-  htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
-  htim2.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
-  if (HAL_TIM_Base_Init(&htim2) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
-  if (HAL_TIM_ConfigClockSource(&htim2, &sClockSourceConfig) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  if (HAL_TIM_PWM_Init(&htim2) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
-  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
-  if (HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  sConfigOC.OCMode = TIM_OCMODE_PWM1;
-  sConfigOC.Pulse = 0;
-  sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
-  sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
-  if (HAL_TIM_PWM_ConfigChannel(&htim2, &sConfigOC, TIM_CHANNEL_2) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  /* USER CODE BEGIN TIM2_Init 2 */
-
-  /* USER CODE END TIM2_Init 2 */
-  HAL_TIM_MspPostInit(&htim2);
-
-}
-
-/**
-  * @brief TIM4 Initialization Function
-  * @param None
-  * @retval None
-  */
-static void MX_TIM4_Init(void)
-{
-
-  /* USER CODE BEGIN TIM4_Init 0 */
-
-  /* USER CODE END TIM4_Init 0 */
-
-  TIM_MasterConfigTypeDef sMasterConfig = {0};
-  TIM_OC_InitTypeDef sConfigOC = {0};
-
-  /* USER CODE BEGIN TIM4_Init 1 */
-
-  /* USER CODE END TIM4_Init 1 */
-  htim4.Instance = TIM4;
-  htim4.Init.Prescaler = 1000;
-  htim4.Init.CounterMode = TIM_COUNTERMODE_UP;
-  htim4.Init.Period = 5999;
-  htim4.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
-  htim4.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
-  if (HAL_TIM_PWM_Init(&htim4) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  if (HAL_TIM_OC_Init(&htim4) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
-  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
-  if (HAL_TIMEx_MasterConfigSynchronization(&htim4, &sMasterConfig) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  sConfigOC.OCMode = TIM_OCMODE_PWM1;
-  sConfigOC.Pulse = 3000;
-  sConfigOC.OCPolarity = TIM_OCPOLARITY_LOW;
-  sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
-  if (HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_2) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  sConfigOC.OCMode = TIM_OCMODE_TIMING;
-  sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
-  if (HAL_TIM_OC_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_3) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  /* USER CODE BEGIN TIM4_Init 2 */
-  LL_TIM_EnableIT_CC1(TIM4);
-  /* USER CODE END TIM4_Init 2 */
-  HAL_TIM_MspPostInit(&htim4);
-
-}
-
-/**
-  * @brief TIM5 Initialization Function
-  * @param None
-  * @retval None
-  */
-static void MX_TIM5_Init(void)
-{
-
-  /* USER CODE BEGIN TIM5_Init 0 */
-
-  /* USER CODE END TIM5_Init 0 */
-
-  TIM_ClockConfigTypeDef sClockSourceConfig = {0};
-  TIM_MasterConfigTypeDef sMasterConfig = {0};
-
-  /* USER CODE BEGIN TIM5_Init 1 */
-
-  /* USER CODE END TIM5_Init 1 */
-  htim5.Instance = TIM5;
-  htim5.Init.Prescaler = 2400-1;
-  htim5.Init.CounterMode = TIM_COUNTERMODE_UP;
-  htim5.Init.Period = 0xFFFFFFFF;
-  htim5.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
-  htim5.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
-  if (HAL_TIM_Base_Init(&htim5) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
-  if (HAL_TIM_ConfigClockSource(&htim5, &sClockSourceConfig) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
-  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
-  if (HAL_TIMEx_MasterConfigSynchronization(&htim5, &sMasterConfig) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  /* USER CODE BEGIN TIM5_Init 2 */
-
-  /* USER CODE END TIM5_Init 2 */
-
-}
-
-/**
-  * @brief TIM8 Initialization Function
-  * @param None
-  * @retval None
-  */
-static void MX_TIM8_Init(void)
-{
-
-  /* USER CODE BEGIN TIM8_Init 0 */
-
-  /* USER CODE END TIM8_Init 0 */
-
-  TIM_ClockConfigTypeDef sClockSourceConfig = {0};
-  TIM_MasterConfigTypeDef sMasterConfig = {0};
-
-  /* USER CODE BEGIN TIM8_Init 1 */
-
-  /* USER CODE END TIM8_Init 1 */
-  htim8.Instance = TIM8;
-  htim8.Init.Prescaler = 0;
-  htim8.Init.CounterMode = TIM_COUNTERMODE_UP;
-  htim8.Init.Period = 10-1;
-  htim8.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
-  htim8.Init.RepetitionCounter = 0;
-  htim8.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
-  if (HAL_TIM_Base_Init(&htim8) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
-  if (HAL_TIM_ConfigClockSource(&htim8, &sClockSourceConfig) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
-  sMasterConfig.MasterOutputTrigger2 = TIM_TRGO2_RESET;
-  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
-  if (HAL_TIMEx_MasterConfigSynchronization(&htim8, &sMasterConfig) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  /* USER CODE BEGIN TIM8_Init 2 */
-
-  /* USER CODE END TIM8_Init 2 */
-
-}
-
-/**
-  * @brief TIM14 Initialization Function
-  * @param None
-  * @retval None
-  */
-static void MX_TIM14_Init(void)
-{
-
-  /* USER CODE BEGIN TIM14_Init 0 */
-
-  /* USER CODE END TIM14_Init 0 */
-
-  /* USER CODE BEGIN TIM14_Init 1 */
-
-  /* USER CODE END TIM14_Init 1 */
-  htim14.Instance = TIM14;
-  htim14.Init.Prescaler = 240-1;
-  htim14.Init.CounterMode = TIM_COUNTERMODE_UP;
-  htim14.Init.Period = 5000-1;
-  htim14.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
-  htim14.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
-  if (HAL_TIM_Base_Init(&htim14) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  /* USER CODE BEGIN TIM14_Init 2 */
-
-  /* USER CODE END TIM14_Init 2 */
-
-}
-
-/**
-  * @brief TIM15 Initialization Function
-  * @param None
-  * @retval None
-  */
-static void MX_TIM15_Init(void)
-{
-
-  /* USER CODE BEGIN TIM15_Init 0 */
-
-  /* USER CODE END TIM15_Init 0 */
-
-  TIM_ClockConfigTypeDef sClockSourceConfig = {0};
-  TIM_MasterConfigTypeDef sMasterConfig = {0};
-
-  /* USER CODE BEGIN TIM15_Init 1 */
-
-  /* USER CODE END TIM15_Init 1 */
-  htim15.Instance = TIM15;
-  htim15.Init.Prescaler = 240-1;
-  htim15.Init.CounterMode = TIM_COUNTERMODE_UP;
-  htim15.Init.Period = 65535;
-  htim15.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
-  htim15.Init.RepetitionCounter = 0;
-  htim15.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
-  if (HAL_TIM_Base_Init(&htim15) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
-  if (HAL_TIM_ConfigClockSource(&htim15, &sClockSourceConfig) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
-  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
-  if (HAL_TIMEx_MasterConfigSynchronization(&htim15, &sMasterConfig) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  /* USER CODE BEGIN TIM15_Init 2 */
-
-  /* USER CODE END TIM15_Init 2 */
-
-}
-
-/**
-  * @brief TIM16 Initialization Function
-  * @param None
-  * @retval None
-  */
-static void MX_TIM16_Init(void)
-{
-
-  /* USER CODE BEGIN TIM16_Init 0 */
-
-  /* USER CODE END TIM16_Init 0 */
-
-  /* USER CODE BEGIN TIM16_Init 1 */
-
-  /* USER CODE END TIM16_Init 1 */
-  htim16.Instance = TIM16;
-  htim16.Init.Prescaler = 240-1;
-  htim16.Init.CounterMode = TIM_COUNTERMODE_UP;
-  htim16.Init.Period = 25000-1;
-  htim16.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
-  htim16.Init.RepetitionCounter = 0;
-  htim16.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
-  if (HAL_TIM_Base_Init(&htim16) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  /* USER CODE BEGIN TIM16_Init 2 */
-
-  /* USER CODE END TIM16_Init 2 */
-
-}
-
-/**
-  * @brief UART4 Initialization Function
-  * @param None
-  * @retval None
-  */
-static void MX_UART4_Init(void)
-{
-
-  /* USER CODE BEGIN UART4_Init 0 */
-
-  /* USER CODE END UART4_Init 0 */
-
-  /* USER CODE BEGIN UART4_Init 1 */
-
-  /* USER CODE END UART4_Init 1 */
-  huart4.Instance = UART4;
-  huart4.Init.BaudRate = 115200;
-  huart4.Init.WordLength = UART_WORDLENGTH_8B;
-  huart4.Init.StopBits = UART_STOPBITS_1;
-  huart4.Init.Parity = UART_PARITY_NONE;
-  huart4.Init.Mode = UART_MODE_TX_RX;
-  huart4.Init.HwFlowCtl = UART_HWCONTROL_NONE;
-  huart4.Init.OverSampling = UART_OVERSAMPLING_16;
-  huart4.Init.OneBitSampling = UART_ONE_BIT_SAMPLE_DISABLE;
-  huart4.Init.ClockPrescaler = UART_PRESCALER_DIV1;
-  huart4.AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_NO_INIT;
-  if (HAL_UART_Init(&huart4) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  if (HAL_UARTEx_SetTxFifoThreshold(&huart4, UART_TXFIFO_THRESHOLD_1_8) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  if (HAL_UARTEx_SetRxFifoThreshold(&huart4, UART_RXFIFO_THRESHOLD_1_8) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  if (HAL_UARTEx_DisableFifoMode(&huart4) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  /* USER CODE BEGIN UART4_Init 2 */
-
-  /* USER CODE END UART4_Init 2 */
-
-}
-
-/**
-  * @brief USART1 Initialization Function
-  * @param None
-  * @retval None
-  */
-static void MX_USART1_Init(void)
-{
-
-  /* USER CODE BEGIN USART1_Init 0 */
-
-  /* USER CODE END USART1_Init 0 */
-
-  /* USER CODE BEGIN USART1_Init 1 */
-
-  /* USER CODE END USART1_Init 1 */
-  husart1.Instance = USART1;
-  husart1.Init.BaudRate = 115200;
-  husart1.Init.WordLength = USART_WORDLENGTH_8B;
-  husart1.Init.StopBits = USART_STOPBITS_1;
-  husart1.Init.Parity = USART_PARITY_NONE;
-  husart1.Init.Mode = USART_MODE_RX;
-  husart1.Init.CLKPolarity = USART_POLARITY_HIGH;
-  husart1.Init.CLKPhase = USART_PHASE_2EDGE;
-  husart1.Init.CLKLastBit = USART_LASTBIT_DISABLE;
-  husart1.Init.ClockPrescaler = USART_PRESCALER_DIV1;
-  husart1.SlaveMode = USART_SLAVEMODE_ENABLE;
-  if (HAL_USART_Init(&husart1) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  if (HAL_USARTEx_SetTxFifoThreshold(&husart1, USART_TXFIFO_THRESHOLD_1_8) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  if (HAL_USARTEx_SetRxFifoThreshold(&husart1, USART_RXFIFO_THRESHOLD_8_8) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  if (HAL_USARTEx_EnableFifoMode(&husart1) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  if (HAL_USARTEx_EnableSlaveMode(&husart1) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  /* USER CODE BEGIN USART1_Init 2 */
-
-  /* USER CODE END USART1_Init 2 */
-
-}
-
-/**
-  * @brief USART2 Initialization Function
-  * @param None
-  * @retval None
-  */
-static void MX_USART2_Init(void)
-{
-
-  /* USER CODE BEGIN USART2_Init 0 */
-
-  /* USER CODE END USART2_Init 0 */
-
-  /* USER CODE BEGIN USART2_Init 1 */
-
-  /* USER CODE END USART2_Init 1 */
-  husart2.Instance = USART2;
-  husart2.Init.BaudRate = 5000000;
-  husart2.Init.WordLength = USART_WORDLENGTH_8B;
-  husart2.Init.StopBits = USART_STOPBITS_1;
-  husart2.Init.Parity = USART_PARITY_NONE;
-  husart2.Init.Mode = USART_MODE_RX;
-  husart2.Init.CLKPolarity = USART_POLARITY_HIGH;
-  husart2.Init.CLKPhase = USART_PHASE_2EDGE;
-  husart2.Init.CLKLastBit = USART_LASTBIT_DISABLE;
-  husart2.Init.ClockPrescaler = USART_PRESCALER_DIV1;
-  husart2.SlaveMode = USART_SLAVEMODE_ENABLE;
-  if (HAL_USART_Init(&husart2) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  if (HAL_USARTEx_SetTxFifoThreshold(&husart2, USART_TXFIFO_THRESHOLD_1_8) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  if (HAL_USARTEx_SetRxFifoThreshold(&husart2, USART_RXFIFO_THRESHOLD_8_8) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  if (HAL_USARTEx_EnableFifoMode(&husart2) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  if (HAL_USARTEx_EnableSlaveMode(&husart2) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  /* USER CODE BEGIN USART2_Init 2 */
-
-  /* USER CODE END USART2_Init 2 */
-
-}
-
-/**
-  * @brief USART3 Initialization Function
-  * @param None
-  * @retval None
-  */
-static void MX_USART3_Init(void)
-{
-
-  /* USER CODE BEGIN USART3_Init 0 */
-
-  /* USER CODE END USART3_Init 0 */
-
-  /* USER CODE BEGIN USART3_Init 1 */
-
-  /* USER CODE END USART3_Init 1 */
-  husart3.Instance = USART3;
-  husart3.Init.BaudRate = 115200;
-  husart3.Init.WordLength = USART_WORDLENGTH_8B;
-  husart3.Init.StopBits = USART_STOPBITS_1;
-  husart3.Init.Parity = USART_PARITY_NONE;
-  husart3.Init.Mode = USART_MODE_RX;
-  husart3.Init.CLKPolarity = USART_POLARITY_HIGH;
-  husart3.Init.CLKPhase = USART_PHASE_2EDGE;
-  husart3.Init.CLKLastBit = USART_LASTBIT_DISABLE;
-  husart3.Init.ClockPrescaler = USART_PRESCALER_DIV1;
-  husart3.SlaveMode = USART_SLAVEMODE_ENABLE;
-  if (HAL_USART_Init(&husart3) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  if (HAL_USARTEx_SetTxFifoThreshold(&husart3, USART_TXFIFO_THRESHOLD_1_8) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  if (HAL_USARTEx_SetRxFifoThreshold(&husart3, USART_RXFIFO_THRESHOLD_8_8) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  if (HAL_USARTEx_EnableFifoMode(&husart3) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  if (HAL_USARTEx_EnableSlaveMode(&husart3) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  /* USER CODE BEGIN USART3_Init 2 */
-
-  /* USER CODE END USART3_Init 2 */
-
-}
-
-/**
-  * @brief USART6 Initialization Function
-  * @param None
-  * @retval None
-  */
-static void MX_USART6_Init(void)
-{
-
-  /* USER CODE BEGIN USART6_Init 0 */
-
-  /* USER CODE END USART6_Init 0 */
-
-  /* USER CODE BEGIN USART6_Init 1 */
-
-  /* USER CODE END USART6_Init 1 */
-  husart6.Instance = USART6;
-  husart6.Init.BaudRate = 4167000;
-  husart6.Init.WordLength = USART_WORDLENGTH_8B;
-  husart6.Init.StopBits = USART_STOPBITS_1;
-  husart6.Init.Parity = USART_PARITY_NONE;
-  husart6.Init.Mode = USART_MODE_RX;
-  husart6.Init.CLKPolarity = USART_POLARITY_HIGH;
-  husart6.Init.CLKPhase = USART_PHASE_2EDGE;
-  husart6.Init.CLKLastBit = USART_LASTBIT_DISABLE;
-  husart6.Init.ClockPrescaler = USART_PRESCALER_DIV1;
-  husart6.SlaveMode = USART_SLAVEMODE_ENABLE;
-  if (HAL_USART_Init(&husart6) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  if (HAL_USARTEx_SetTxFifoThreshold(&husart6, USART_TXFIFO_THRESHOLD_1_8) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  if (HAL_USARTEx_SetRxFifoThreshold(&husart6, USART_RXFIFO_THRESHOLD_8_8) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  if (HAL_USARTEx_EnableFifoMode(&husart6) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  if (HAL_USARTEx_EnableSlaveMode(&husart6) != HAL_OK)
-  {
-    Error_Handler();
-  }
-  /* USER CODE BEGIN USART6_Init 2 */
-
-  /* USER CODE END USART6_Init 2 */
-
-}
-
-/**
-  * Enable DMA controller clock
-  */
-static void MX_BDMA_Init(void)
-{
-
-  /* DMA controller clock enable */
-  __HAL_RCC_BDMA_CLK_ENABLE();
-
-  /* DMA interrupt init */
-  /* BDMA_Channel0_IRQn interrupt configuration */
-  HAL_NVIC_SetPriority(BDMA_Channel0_IRQn, DMA_IRQ_PRIORITY, 0);
-  HAL_NVIC_EnableIRQ(BDMA_Channel0_IRQn);
-
-}
-
-/**
-  * Enable DMA controller clock
-  * Configure DMA for memory to memory transfers
-  *   hdma_memtomem_dma2_stream1
-  */
-static void MX_DMA_Init(void)
-{
-
-  /* DMA controller clock enable */
-  __HAL_RCC_DMA1_CLK_ENABLE();
-  __HAL_RCC_DMA2_CLK_ENABLE();
-
-  /* Configure DMA request hdma_memtomem_dma2_stream1 on DMA2_Stream1 */
-  hdma_memtomem_dma2_stream1.Instance = DMA2_Stream1;
-  hdma_memtomem_dma2_stream1.Init.Request = DMA_REQUEST_MEM2MEM;
-  hdma_memtomem_dma2_stream1.Init.Direction = DMA_MEMORY_TO_MEMORY;
-  hdma_memtomem_dma2_stream1.Init.PeriphInc = DMA_PINC_DISABLE;
-  hdma_memtomem_dma2_stream1.Init.MemInc = DMA_MINC_ENABLE;
-  hdma_memtomem_dma2_stream1.Init.PeriphDataAlignment = DMA_PDATAALIGN_WORD;
-  hdma_memtomem_dma2_stream1.Init.MemDataAlignment = DMA_MDATAALIGN_WORD;
-  hdma_memtomem_dma2_stream1.Init.Mode = DMA_NORMAL;
-  hdma_memtomem_dma2_stream1.Init.Priority = DMA_PRIORITY_MEDIUM;
-  hdma_memtomem_dma2_stream1.Init.FIFOMode = DMA_FIFOMODE_ENABLE;
-  hdma_memtomem_dma2_stream1.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_FULL;
-  hdma_memtomem_dma2_stream1.Init.MemBurst = DMA_MBURST_INC4;
-  hdma_memtomem_dma2_stream1.Init.PeriphBurst = DMA_PBURST_INC4;
-  if (HAL_DMA_Init(&hdma_memtomem_dma2_stream1) != HAL_OK)
-  {
-    Error_Handler( );
-  }
-
-  /* DMA interrupt init */
-  /* DMA1_Stream0_IRQn interrupt configuration */
-  HAL_NVIC_SetPriority(DMA1_Stream0_IRQn, 0, 0);
-  HAL_NVIC_EnableIRQ(DMA1_Stream0_IRQn);
-  /* DMA1_Stream1_IRQn interrupt configuration */
-  HAL_NVIC_SetPriority(DMA1_Stream1_IRQn, 0, 0);
-  HAL_NVIC_EnableIRQ(DMA1_Stream1_IRQn);
-  /* DMA1_Stream2_IRQn interrupt configuration */
-  HAL_NVIC_SetPriority(DMA1_Stream2_IRQn, 0, 0);
-  HAL_NVIC_EnableIRQ(DMA1_Stream2_IRQn);
-  /* DMA1_Stream3_IRQn interrupt configuration */
-  HAL_NVIC_SetPriority(DMA1_Stream3_IRQn, 0, 0);
-  HAL_NVIC_EnableIRQ(DMA1_Stream3_IRQn);
-  /* DMA1_Stream4_IRQn interrupt configuration */
-  HAL_NVIC_SetPriority(DMA1_Stream4_IRQn, 0, 0);
-  HAL_NVIC_EnableIRQ(DMA1_Stream4_IRQn);
-  /* DMA1_Stream5_IRQn interrupt configuration */
-  HAL_NVIC_SetPriority(DMA1_Stream5_IRQn, 0, 0);
-  HAL_NVIC_EnableIRQ(DMA1_Stream5_IRQn);
-  /* DMA1_Stream6_IRQn interrupt configuration */
-  HAL_NVIC_SetPriority(DMA1_Stream6_IRQn, 0, 0);
-  HAL_NVIC_EnableIRQ(DMA1_Stream6_IRQn);
-  /* DMA1_Stream7_IRQn interrupt configuration */
-  HAL_NVIC_SetPriority(DMA1_Stream7_IRQn, 0, 0);
-  HAL_NVIC_EnableIRQ(DMA1_Stream7_IRQn);
-  /* DMA2_Stream0_IRQn interrupt configuration */
-  HAL_NVIC_SetPriority(DMA2_Stream0_IRQn, 0, 0);
-  HAL_NVIC_EnableIRQ(DMA2_Stream0_IRQn);
-
-}
-
-/**
-  * @brief GPIO Initialization Function
-  * @param None
-  * @retval None
-  */
-static void MX_GPIO_Init(void)
-{
-  GPIO_InitTypeDef GPIO_InitStruct = {0};
-  /* USER CODE BEGIN MX_GPIO_Init_1 */
-  /* USER CODE END MX_GPIO_Init_1 */
-
-  /* GPIO Ports Clock Enable */
-  __HAL_RCC_GPIOC_CLK_ENABLE();
-  __HAL_RCC_GPIOE_CLK_ENABLE();
-  __HAL_RCC_GPIOB_CLK_ENABLE();
-  __HAL_RCC_GPIOA_CLK_ENABLE();
-  __HAL_RCC_GPIOD_CLK_ENABLE();
-  __HAL_RCC_GPIOH_CLK_ENABLE();
-
-  /*Configure GPIO pin Output Level */
-  HAL_GPIO_WritePin(GPIOC, ERROR_LED_Pin|MUX_RESET_Pin|USB_MUX_Pin, GPIO_PIN_RESET);
-
-  /*Configure GPIO pin Output Level */
-  HAL_GPIO_WritePin(USB_RESET_GPIO_Port, USB_RESET_Pin, GPIO_PIN_RESET);
-
-  /*Configure GPIO pin Output Level */
-  HAL_GPIO_WritePin(GPIOD, CAM_PWR_1_Pin|CAM_PWR_5_Pin|CAM_PWR_8_Pin|CAM_PWR_4_Pin
-                          |FAN_CTL_Pin|FS_OUT_EN_Pin, GPIO_PIN_SET);
-
-  /*Configure GPIO pin Output Level */
-  HAL_GPIO_WritePin(GPIOE, CAM_PWR_7_Pin|CAM_PWR_2_Pin|FSIN_EN_Pin, GPIO_PIN_SET);
-
-  /*Configure GPIO pin Output Level */
-  HAL_GPIO_WritePin(CAM_PWR_3_GPIO_Port, CAM_PWR_3_Pin, GPIO_PIN_SET);
-
-  /*Configure GPIO pin Output Level */
-  HAL_GPIO_WritePin(CAM_PWR_6_GPIO_Port, CAM_PWR_6_Pin, GPIO_PIN_SET);
-
-  /*Configure GPIO pins : ERROR_LED_Pin MUX_RESET_Pin USB_MUX_Pin CAM_PWR_6_Pin */
-  GPIO_InitStruct.Pin = ERROR_LED_Pin|MUX_RESET_Pin|USB_MUX_Pin|CAM_PWR_6_Pin;
-  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
-  GPIO_InitStruct.Pull = GPIO_NOPULL;
-  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
-  HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
-
-  /*Configure GPIO pins : GPIO0_8_Pin IMU_INT_Pin */
-  GPIO_InitStruct.Pin = GPIO0_8_Pin|IMU_INT_Pin;
-  GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
-  GPIO_InitStruct.Pull = GPIO_NOPULL;
-  HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
-
-  /*Configure GPIO pins : GPIO0_7_Pin GPIO0_5_Pin */
-  GPIO_InitStruct.Pin = GPIO0_7_Pin|GPIO0_5_Pin;
-  GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
-  GPIO_InitStruct.Pull = GPIO_NOPULL;
-  HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
-
-  /*Configure GPIO pins : USB_RESET_Pin CAM_PWR_3_Pin */
-  GPIO_InitStruct.Pin = USB_RESET_Pin|CAM_PWR_3_Pin;
-  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
-  GPIO_InitStruct.Pull = GPIO_NOPULL;
-  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
-  HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
-
-  /*Configure GPIO pins : GPIO0_6_Pin CRESET_8_Pin CRESET_7_Pin PE14
-                           GPIO0_2_Pin GPIO0_3_Pin CRESET_1_Pin CRESET_3_Pin
-                           CRESET_2_Pin CRESET_4_Pin */
-  GPIO_InitStruct.Pin = GPIO0_6_Pin|CRESET_8_Pin|CRESET_7_Pin|GPIO_PIN_14
-                          |GPIO0_2_Pin|GPIO0_3_Pin|CRESET_1_Pin|CRESET_3_Pin
-                          |CRESET_2_Pin|CRESET_4_Pin;
-  GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
-  GPIO_InitStruct.Pull = GPIO_NOPULL;
-  HAL_GPIO_Init(GPIOE, &GPIO_InitStruct);
-
-  /*Configure GPIO pins : CRESET_6_Pin CRESET_5_Pin GPIO0_4_Pin */
-  GPIO_InitStruct.Pin = CRESET_6_Pin|CRESET_5_Pin|GPIO0_4_Pin;
-  GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
-  GPIO_InitStruct.Pull = GPIO_NOPULL;
-  HAL_GPIO_Init(GPIOD, &GPIO_InitStruct);
-
-  /*Configure GPIO pins : CAM_PWR_1_Pin CAM_PWR_5_Pin CAM_PWR_8_Pin CAM_PWR_4_Pin
-                           FAN_CTL_Pin FS_OUT_EN_Pin */
-  GPIO_InitStruct.Pin = CAM_PWR_1_Pin|CAM_PWR_5_Pin|CAM_PWR_8_Pin|CAM_PWR_4_Pin
-                          |FAN_CTL_Pin|FS_OUT_EN_Pin;
-  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
-  GPIO_InitStruct.Pull = GPIO_NOPULL;
-  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
-  HAL_GPIO_Init(GPIOD, &GPIO_InitStruct);
-
-  /*Configure GPIO pins : PA12 PA11 */
-  GPIO_InitStruct.Pin = GPIO_PIN_12|GPIO_PIN_11;
-  GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
-  GPIO_InitStruct.Pull = GPIO_NOPULL;
-  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
-  GPIO_InitStruct.Alternate = GPIO_AF10_OTG1_FS;
-  HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
-
-  /*Configure GPIO pins : CAM_PWR_7_Pin CAM_PWR_2_Pin FSIN_EN_Pin */
-  GPIO_InitStruct.Pin = CAM_PWR_7_Pin|CAM_PWR_2_Pin|FSIN_EN_Pin;
-  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
-  GPIO_InitStruct.Pull = GPIO_NOPULL;
-  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
-  HAL_GPIO_Init(GPIOE, &GPIO_InitStruct);
-
-  /*Configure GPIO pin : PC9 */
-  GPIO_InitStruct.Pin = GPIO_PIN_9;
-  GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
-  GPIO_InitStruct.Pull = GPIO_NOPULL;
-  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
-  GPIO_InitStruct.Alternate = GPIO_AF0_MCO;
-  HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
-
-  /*Configure GPIO pin : GPIO0_1_Pin */
-  GPIO_InitStruct.Pin = GPIO0_1_Pin;
-  GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
-  GPIO_InitStruct.Pull = GPIO_NOPULL;
-  HAL_GPIO_Init(GPIO0_1_GPIO_Port, &GPIO_InitStruct);
-
-  /* USER CODE BEGIN MX_GPIO_Init_2 */
-  /* USER CODE END MX_GPIO_Init_2 */
-}
-
-/* USER CODE BEGIN 4 */
-
-void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
-{
-}
-
-void HAL_UART_TxCpltCallback(UART_HandleTypeDef *huart)
-{
-  if (huart->Instance == UART4)
-  {
-    logging_UART_TxCpltCallback(huart);
-  }
-}
-
-// Error handling callback for USART
-void HAL_USART_ErrorCallback(USART_HandleTypeDef *husart)
-{
-  int8_t cam_id = -1;
-
-  if (husart->Instance == USART1)
-  {
-    cam_id = 4;
-  }
-  else if (husart->Instance == USART2)
-  {
-    cam_id = 0;
-  }
-  else if (husart->Instance == USART3)
-  {
-    cam_id = 2;
-  }
-  else if (husart->Instance == USART6)
-  {
-    cam_id = 3;
-  }
-
-  // Print which USART instance caused the error
-  if (husart->Instance == USART1)
-  {
-    printf("Error in USART1: ");
-  }
-  else if (husart->Instance == USART2)
-  {
-    printf("Error in USART2: ");
-  }
-  else if (husart->Instance == USART3)
-  {
-    printf("Error in USART3: ");
-  }
-  else if (husart->Instance == USART6)
-  {
-    printf("Error in USART6: ");
-  }
-  else
-  {
-    printf("Error in Unknown USART instance: ");
-  }
-
-  // Identify specific errors using error codes
-  if (husart->ErrorCode & HAL_USART_ERROR_PE)
-  {
-    printf("Parity error ");
-  }
-  if (husart->ErrorCode & HAL_USART_ERROR_NE)
-  {
-    printf("Noise error ");
-  }
-  if (husart->ErrorCode & HAL_USART_ERROR_FE)
-  {
-    printf("Framing error ");
-  }
-  if (husart->ErrorCode & HAL_USART_ERROR_ORE)
-  {
-    printf("Overrun error ");
-    __HAL_USART_CLEAR_OREFLAG(husart);
-  }
-  if (husart->ErrorCode & HAL_USART_ERROR_DMA)
-  {
-    printf("DMA transfer error ");
-  }
-  printf("\r\n");
-
-  if ((husart->ErrorCode & HAL_USART_ERROR_ORE) != 0U && cam_id >= 0)
-  {
-    abort_data_reception((uint8_t)cam_id);
-    start_data_reception((uint8_t)cam_id);
-    return;
-  }
-
-  Error_Handler();
-}
-
-// Error handling callback for SPI
-void HAL_SPI_ErrorCallback(SPI_HandleTypeDef *hspi)
-{
-  int8_t cam_id = -1;
-
-  if (hspi->Instance == SPI2)
-  {
-    cam_id = 6;
-  }
-  else if (hspi->Instance == SPI3)
-  {
-    cam_id = 5;
-  }
-  else if (hspi->Instance == SPI4)
-  {
-    cam_id = 7;
-  }
-  else if (hspi->Instance == SPI6)
-  {
-    cam_id = 1;
-  }
-
-  // Print which SPI instance caused the error
-  if (hspi->Instance == SPI2)
-  {
-    printf("Error in SPI2: ");
-  }
-  else if (hspi->Instance == SPI3)
-  {
-    printf("Error in SPI3: ");
-  }
-  else if (hspi->Instance == SPI4)
-  {
-    printf("Error in SPI4: ");
-  }
-  else if (hspi->Instance == SPI6)
-  {
-    printf("Error in SPI6: ");
-  }
-  else
-  {
-    printf("Error in Unknown SPI instance: ");
-  }
-  
-  // Identify specific errors using error codes
-  if (hspi->ErrorCode & HAL_SPI_ERROR_OVR)
-  {
-    printf("Overrun error ");
-    __HAL_SPI_CLEAR_OVRFLAG(hspi);
-  }
-  if (hspi->ErrorCode & HAL_SPI_ERROR_MODF)
-  {
-    printf("Mode fault error ");
-  }
-  if (hspi->ErrorCode & HAL_SPI_ERROR_CRC)
-  {
-    printf("CRC error ");
-  }
-  if (hspi->ErrorCode & HAL_SPI_ERROR_FRE)
-  {
-    printf("Frame error ");
-  }
-  if (hspi->ErrorCode & HAL_SPI_ERROR_DMA)
-  {
-    if (hspi->hdmarx->ErrorCode == HAL_DMA_ERROR_TE)
-      printf("TE - HAL_DMA_ERROR_TE");
-    if (hspi->hdmarx->ErrorCode == HAL_DMA_ERROR_FE)
-      printf("HAL_DMA_ERROR_FE ");
-    if (hspi->hdmarx->ErrorCode == HAL_DMA_ERROR_DME)
-      printf("HAL_DMA_ERROR_DME");
-    if (hspi->hdmarx->ErrorCode == HAL_DMA_ERROR_TIMEOUT)
-      printf("HAL_DMA_ERROR_TIMEOUT");
-    if (hspi->hdmarx->ErrorCode == HAL_DMA_ERROR_PARAM)
-      printf("HAL_DMA_ERROR_PARAM");
-    if (hspi->hdmarx->ErrorCode == HAL_DMA_ERROR_NO_XFER)
-      printf("HAL_DMA_ERROR_NO_XFER");
-    if (hspi->hdmarx->ErrorCode == HAL_DMA_ERROR_NOT_SUPPORTED)
-      printf("HAL_DMA_ERROR_NOT_SUPPORTED");
-    if (hspi->hdmarx->ErrorCode == HAL_DMA_ERROR_SYNC)
-      printf("HAL_DMA_ERROR_SYNC");
-    if (hspi->hdmarx->ErrorCode == HAL_DMA_ERROR_REQGEN)
-      printf("HAL_DMA_ERROR_REQGEN");
-    if (hspi->hdmarx->ErrorCode == HAL_DMA_ERROR_BUSY)
-      printf("HAL_DMA_ERROR_BUSY");
-  }
-  printf("\r\n");
-
-  if ((hspi->ErrorCode & HAL_SPI_ERROR_OVR) != 0U && cam_id >= 0)
-  {
-    abort_data_reception((uint8_t)cam_id);
-    start_data_reception((uint8_t)cam_id);
-    return;
-  }
-
-  Error_Handler(); // Handle any error during re-enabling
-}
-
-
-void set_event_bit_atomic(uint32_t bit) {
-  __disable_irq();
-  event_bits |= bit;
-  __enable_irq();
-}
-
-// Interrupt handler for SPI reception
-void HAL_SPI_RxCpltCallback(SPI_HandleTypeDef *hspi)
-{
-  if (hspi->Instance == SPI2)
-  {
-    set_event_bit_atomic(BIT_6);
-  }
-  else if (hspi->Instance == SPI3)
-  {
-    set_event_bit_atomic(BIT_5);
-  }
-  else if (hspi->Instance == SPI4)
-  {
-    set_event_bit_atomic(BIT_7);
-  }
-  else if (hspi->Instance == SPI6)
-  {
-    set_event_bit_atomic(BIT_1);
-  }
-}
-
-void HAL_USART_RxCpltCallback(USART_HandleTypeDef *husart)
-{
-  if (husart->Instance == USART1)
-  { // Check if the interrupt is for USART2
-    set_event_bit_atomic(BIT_4);
-  }
-  else if (husart->Instance == USART2)
-  { // Check if the interrupt is for USART2
-    set_event_bit_atomic(BIT_0);
-  }
-  else if (husart->Instance == USART3)
-  { // Check if the interrupt is for USART2
-    set_event_bit_atomic(BIT_2);
-  }
-  else if (husart->Instance == USART6)
-  { // Check if the interrupt is for USART2
-    set_event_bit_atomic(BIT_3);
-  }
-
-}
-
-void HAL_TIM_PWM_PulseFinishedCallback(TIM_HandleTypeDef *htim)
-{
-  if (htim->Instance == TIM4) // Call data sender (internal FSIN))
-  {
-    send_data();
-  }
-}
-
-void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
-{     
-    if (GPIO_Pin == GPIO_PIN_13) // Call REAL data sender if interrupt hit and enabled
-    {
-      pulse_count++;
-      send_data();
-    }
-}
-
-void ExitRun0Mode(void) {
-    // Temporary stub for ExitRun0Mode
-}
-
-/**
- * @brief Wait for all USB data queues to finish sending
- * @note This function polls USB endpoint status flags until all transmissions complete
- *       or a timeout occurs. This ensures error messages and diagnostic data are
- *       transmitted before the system halts.
- */
-static void wait_for_usb_queues_to_finish(void)
-{
-  const uint32_t timeout_ms = 1000;  // Maximum wait time: 1 second
-  uint32_t start_time = get_timestamp_ms();
-  uint32_t elapsed = 0;
-  
-  printf("Waiting for USB queues to finish...\r\n");
-  fflush(stdout);
-  
-  // Poll until all endpoints are idle or timeout
-  while (elapsed < timeout_ms) {
-    _Bool all_idle = true;
-    
-    // Check COMMS endpoint (tx_flag: 1 = idle, 0 = busy)
-    if (tx_flag == 0) {
-      all_idle = false;
-    }
-    
-    // Check COMMS bulk endpoint (comms_ep_data: 0 = idle, 1 = transmitting)
-    if (comms_ep_data != 0) {
-      all_idle = false;
-    }
-    
-    // Check HISTO endpoint (histo_ep_data: 0 = idle, 1 = transmitting)
-    if (histo_ep_data != 0) {
-      all_idle = false;
-    }
-    
-    // Check IMU endpoint (imu_ep_data: 0 = idle, 1 = transmitting)
-    if (imu_ep_data != 0) {
-      all_idle = false;
-    }
-    
-    if (all_idle) {
-      printf("All USB queues finished.\r\n");
-      fflush(stdout);
-      return;
-    }
-    
-    // Small delay to avoid busy-waiting
-    delay_ms(1);
-    elapsed = get_timestamp_ms() - start_time;
-  }
-  
-  printf("USB queue wait timeout after %lu ms (some data may not have been sent).\r\n", elapsed);
-  fflush(stdout);
-}
-/* USER CODE END 4 */
-
- /* MPU Configuration */
-
-void MPU_Config(void)
-{
-  MPU_Region_InitTypeDef MPU_InitStruct = {0};
-
-  /* Disables the MPU */
-  HAL_MPU_Disable();
-
-  /** Initializes and configures the Region and the memory to be protected
-  */
-  MPU_InitStruct.Enable = MPU_REGION_ENABLE;
-  MPU_InitStruct.Number = MPU_REGION_NUMBER0;
-  MPU_InitStruct.BaseAddress = 0x0;
-  MPU_InitStruct.Size = MPU_REGION_SIZE_4GB;
-  MPU_InitStruct.SubRegionDisable = 0x87;
-  MPU_InitStruct.TypeExtField = MPU_TEX_LEVEL0;
-  MPU_InitStruct.AccessPermission = MPU_REGION_NO_ACCESS;
-  MPU_InitStruct.DisableExec = MPU_INSTRUCTION_ACCESS_DISABLE;
-  MPU_InitStruct.IsShareable = MPU_ACCESS_SHAREABLE;
-  MPU_InitStruct.IsCacheable = MPU_ACCESS_NOT_CACHEABLE;
-  MPU_InitStruct.IsBufferable = MPU_ACCESS_NOT_BUFFERABLE;
-
-  HAL_MPU_ConfigRegion(&MPU_InitStruct);
-  /* Enables the MPU */
-  HAL_MPU_Enable(MPU_PRIVILEGED_DEFAULT);
-
-}
-
-/**
-  * @brief  Period elapsed callback in non blocking mode
-  * @note   This function is called  when TIM17 interrupt took place, inside
-  * HAL_TIM_IRQHandler(). It makes a direct call to HAL_IncTick() to increment
-  * a global variable "uwTick" used as application time base.
-  * @param  htim : TIM handle
-  * @retval None
-  */
-void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
-{
-  /* USER CODE BEGIN Callback 0 */
-
-  /* USER CODE END Callback 0 */
-  if (htim->Instance == TIM17)
-  {
-    HAL_IncTick();
-  }
-  /* USER CODE BEGIN Callback 1 */
-  if (htim->Instance == TIM16){
-	  HistoFake_GenerateAndSend(&hUsbDeviceHS);
-  }
-
-  if (htim->Instance == TIM14)
-  {
-	  imu_frame_counter++;
-	  // call imu
-	  if(ICM_GetAllRawData(&a,&t, &g, &m) != HAL_OK){
-		  printf("IMU Read Error\r\n");
-	  }else{
-		  memset(usb_buf,0,128);
-		  int len = snprintf(
-		      usb_buf, sizeof(usb_buf),
-		      "{\"F\":%ld,\"G\":[%d,%d,%d],\"M\":[%d,%d,%d],\"A\":[%d,%d,%d],\"T\":%d.%02d}\r\n",
-			  imu_frame_counter,
-		      g.x, g.y, g.z,
-		      m.x, m.y, m.z,
-		      a.x, a.y, a.z,
-			  (int)t, (int)((t - (int)t) * 100.0f)
-		  );
-		  USBD_IMU_SetTxBuffer(&hUsbDeviceHS, (uint8_t *)usb_buf, len);
-	  }
-  }
-
-  if (htim->Instance == TIM15) {
-    HAL_TIM_Base_Stop_IT(htim);
-    if(_enter_dfu) {
-      *((uint32_t *)0x38000000) = 0xDEADBEEF; 
-    }
-
-
-    // De-initialize other specific modules
-    HAL_RNG_DeInit(&hrng);
-    HAL_CRC_DeInit(&hcrc);
-    
-    MX_USB_DEVICE_DeInit();
-    delay_ms(300);
-    // Reset the board
-    NVIC_SystemReset();
-
-  }
-  
-  /* USER CODE END Callback 1 */
-}
-
-/**
-  * @brief  This function is executed in case of error occurrence.
-  * @retval None
-  */
-
-void Error_Handler(void)
-{
-  /* USER CODE BEGIN Error_Handler_Debug */
-  /* User can add his own implementation to report the HAL error return state */
-
-  uint32_t *stack_ptr;
-
-  HAL_GPIO_TogglePin(ERROR_LED_GPIO_Port, ERROR_LED_Pin);
-  printf(">>> HARD FAULT <<<\r\n");
-  fflush(stdout); // Ensure the output is flushed immediately
-
-  // Get the current stack pointer
-  __ASM volatile("MRS %0, MSP" : "=r"(stack_ptr));
-
-  // Print general-purpose registers
-  printf("Stack Pointer (MSP): 0x%08lX\r\n", (unsigned long)stack_ptr);
-  printf("Register dump:\r\n");
-  printf("R0 : 0x%08lX\r\n", stack_ptr[0]);
-  printf("R1 : 0x%08lX\r\n", stack_ptr[1]);
-  printf("R2 : 0x%08lX\r\n", stack_ptr[2]);
-  printf("R3 : 0x%08lX\r\n", stack_ptr[3]);
-  printf("R12: 0x%08lX\r\n", stack_ptr[4]);
-  printf("LR : 0x%08lX (Link Register)\r\n", stack_ptr[5]);
-  printf("PC : 0x%08lX (Program Counter)\r\n", stack_ptr[6]);
-  printf("xPSR: 0x%08lX\r\n", stack_ptr[7]);
-  fflush(stdout);
-
-  // Wait for all USB data queues to finish sending before halting
-  wait_for_usb_queues_to_finish();
-
-  delay_ms(100);
-  __disable_irq();
-  while (1)
-  {
-  }
-  /* USER CODE END Error_Handler_Debug */
-}
-#ifdef USE_FULL_ASSERT
-/**
-  * @brief  Reports the name of the source file and the source line number
-  *         where the assert_param error has occurred.
-  * @param  file: pointer to the source file name
-  * @param  line: assert_param error line source number
-  * @retval None
-  */
-void assert_failed(uint8_t *file, uint32_t line)
-{
-  /* USER CODE BEGIN 6 */
-  /* User can add his own implementation to report the file name and line number,
-     ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
-  /* USER CODE END 6 */
-}
-#endif /* USE_FULL_ASSERT */
+/* USER CODE BEGIN Header */
+/**
+ ******************************************************************************
+ * @file           : main.c
+ * @brief          : Main program body
+ ******************************************************************************
+ * @attention
+ *
+ * Copyright (c) 2024 STMicroelectronics.
+ * All rights reserved.
+ *
+ * This software is licensed under terms that can be found in the LICENSE file
+ * in the root directory of this software component.
+ * If no LICENSE file comes with this software, it is provided AS-IS.
+ *
+ ******************************************************************************
+ */
+/* USER CODE END Header */
+/* Includes ------------------------------------------------------------------*/
+#include "main.h"
+
+/* Private includes ----------------------------------------------------------*/
+/* USER CODE BEGIN Includes */
+#include "0X02C1B.h"
+#include "usb_device.h"
+#include "uart_comms.h"
+#include "usbd_histo.h"
+#include "usbd_imu.h"
+#include "usbd_comms.h"
+#include "logging.h"
+#include "utils.h"
+#include "i2c_master.h"
+#include "histo_fake.h"
+#include "ICM20948.h"
+#include "camera_manager.h"
+
+#include <stdio.h>
+#include <string.h>
+#include <stdbool.h>
+
+#include "stm32h7xx_ll_tim.h"
+
+/* USER CODE END Includes */
+
+/* Private typedef -----------------------------------------------------------*/
+/* USER CODE BEGIN PTD */
+
+/* External references to USB endpoint status variables */
+extern volatile uint8_t tx_flag;  // From uart_comms.c
+extern volatile uint8_t comms_ep_data;  // From usbd_comms.c (__IO is volatile)
+extern volatile uint8_t histo_ep_data;  // From usbd_histo.c (__IO is volatile)
+extern volatile uint8_t imu_ep_data;    // From usbd_imu.c (__IO is volatile)
+
+/* USER CODE END PTD */
+
+/* Private define ------------------------------------------------------------*/
+/* USER CODE BEGIN PD */
+/* USER CODE END PD */
+
+/* Private macro -------------------------------------------------------------*/
+/* USER CODE BEGIN PM */
+
+/* USER CODE END PM */
+
+/* Private variables ---------------------------------------------------------*/
+
+CRC_HandleTypeDef hcrc;
+
+I2C_HandleTypeDef hi2c1;
+
+RNG_HandleTypeDef hrng;
+
+SPI_HandleTypeDef hspi2;
+SPI_HandleTypeDef hspi3;
+SPI_HandleTypeDef hspi4;
+SPI_HandleTypeDef hspi6;
+DMA_HandleTypeDef hdma_spi2_rx;
+DMA_HandleTypeDef hdma_spi3_rx;
+DMA_HandleTypeDef hdma_spi4_rx;
+DMA_HandleTypeDef hdma_spi6_rx;
+
+TIM_HandleTypeDef htim2;
+TIM_HandleTypeDef htim4;
+TIM_HandleTypeDef htim5;
+TIM_HandleTypeDef htim8;
+TIM_HandleTypeDef htim14;
+TIM_HandleTypeDef htim15;
+TIM_HandleTypeDef htim16;
+
+UART_HandleTypeDef huart4;
+USART_HandleTypeDef husart1;
+USART_HandleTypeDef husart2;
+USART_HandleTypeDef husart3;
+USART_HandleTypeDef husart6;
+DMA_HandleTypeDef hdma_uart4_rx;
+DMA_HandleTypeDef hdma_uart4_tx;
+DMA_HandleTypeDef hdma_usart1_rx;
+DMA_HandleTypeDef hdma_usart2_rx;
+DMA_HandleTypeDef hdma_usart3_rx;
+DMA_HandleTypeDef hdma_usart6_rx;
+
+DMA_HandleTypeDef hdma_memtomem_dma2_stream1;
+/* USER CODE BEGIN PV */
+
+uint8_t rxBuffer[COMMAND_MAX_SIZE]  __attribute__((aligned(4)));
+uint8_t txBuffer[COMMAND_MAX_SIZE];
+uint32_t bitstream_len;
+__attribute__((section(".RAM_D1"))) uint8_t bitstream_buffer[MAX_BITSTREAM_SIZE]; // 160KB buffer
+
+volatile uint8_t event_bits = 0x00;         // holds the event bits to be flipped
+volatile uint8_t event_bits_enabled = 0x00; // holds the event bits for the cameras to be enabled
+volatile uint16_t pulse_count = 0;
+extern uint32_t imu_frame_counter;
+volatile bool _enter_dfu = false;
+
+ICM_Axis3D a;
+ICM_Axis3D m;
+ICM_Axis3D g;
+float t;
+char usb_buf[128];
+// Debug flags (sensor debug and fake data are controlled via OW_CMD_DEBUG_FLAGS)
+bool uart_stream = false;
+
+uint16_t fail_count = 0;
+
+extern USBD_HandleTypeDef hUsbDeviceHS;
+
+const char *bit_rep[16] = {
+    [ 0] = "0000", [ 1] = "0001", [ 2] = "0010", [ 3] = "0011",
+    [ 4] = "0100", [ 5] = "0101", [ 6] = "0110", [ 7] = "0111",
+    [ 8] = "1000", [ 9] = "1001", [10] = "1010", [11] = "1011",
+    [12] = "1100", [13] = "1101", [14] = "1110", [15] = "1111",
+};
+/* USER CODE END PV */
+
+/* Private function prototypes -----------------------------------------------*/
+void SystemClock_Config(void);
+void PeriphCommonClock_Config(void);
+static void MPU_Config(void);
+static void MX_GPIO_Init(void);
+static void MX_DMA_Init(void);
+static void MX_BDMA_Init(void);
+static void MX_CRC_Init(void);
+static void MX_I2C1_Init(void);
+static void MX_RNG_Init(void);
+static void MX_SPI2_Init(void);
+static void MX_SPI3_Init(void);
+static void MX_SPI4_Init(void);
+static void MX_SPI6_Init(void);
+static void MX_TIM2_Init(void);
+static void MX_UART4_Init(void);
+static void MX_USART6_Init(void);
+static void MX_TIM8_Init(void);
+static void MX_TIM4_Init(void);
+static void MX_USART1_Init(void);
+static void MX_USART2_Init(void);
+static void MX_USART3_Init(void);
+static void MX_TIM14_Init(void);
+static void MX_TIM16_Init(void);
+static void MX_TIM5_Init(void);
+static void MX_TIM15_Init(void);
+/* USER CODE BEGIN PFP */
+static void poll_main_loop_heartbeat(void);
+/* USER CODE END PFP */
+
+/* Private user code ---------------------------------------------------------*/
+/* USER CODE BEGIN 0 */
+/* Print last reset cause - helps diagnose thermal/brownout (BOR) or watchdog (IWDG) */
+static void print_reset_cause(void)
+{
+	uint8_t first = 1;
+	if (__HAL_RCC_GET_FLAG(RCC_FLAG_BORRST)) {
+		printf("Reset cause: BOR (Brown-Out) - possible thermal/voltage droop\r\n");
+		first = 0;
+	}
+	if (__HAL_RCC_GET_FLAG(RCC_FLAG_PORRST)) {
+		printf("%sReset cause: POR (Power-On)\r\n", first ? "" : "         ");
+		first = 0;
+	}
+	if (__HAL_RCC_GET_FLAG(RCC_FLAG_PINRST)) {
+		printf("%sReset cause: PIN (External NRST)\r\n", first ? "" : "         ");
+		first = 0;
+	}
+	if (__HAL_RCC_GET_FLAG(RCC_FLAG_SFTRST)) {
+		printf("%sReset cause: Software\r\n", first ? "" : "         ");
+		first = 0;
+	}
+#if defined(RCC_FLAG_IWDG1RST)
+	if (__HAL_RCC_GET_FLAG(RCC_FLAG_IWDG1RST)) {
+		printf("%sReset cause: IWDG (Independent Watchdog) - possible lockup/thermal\r\n", first ? "" : "         ");
+		first = 0;
+	}
+#endif
+#if defined(RCC_FLAG_WWDG1RST)
+	if (__HAL_RCC_GET_FLAG(RCC_FLAG_WWDG1RST)) {
+		printf("%sReset cause: WWDG (Window Watchdog)\r\n", first ? "" : "         ");
+		first = 0;
+	}
+#endif
+#if defined(RCC_FLAG_LPWR1RST)
+	if (__HAL_RCC_GET_FLAG(RCC_FLAG_LPWR1RST)) {
+		printf("%sReset cause: LPWR (Low-power)\r\n", first ? "" : "         ");
+		first = 0;
+	}
+#endif
+	if (first) {
+		printf("Reset cause: unknown\r\n");
+	}
+	__HAL_RCC_CLEAR_RESET_FLAGS();
+}
+
+/* Poll MCU junction temperature; print warning if above high threshold */
+#define MCU_TEMP_CHECK_MS  5000u
+static void poll_mcu_temperature(void)
+{
+	static uint32_t last_check_ms = 0;
+	uint32_t now = HAL_GetTick();
+	if ((last_check_ms != 0u) && ((now - last_check_ms) < MCU_TEMP_CHECK_MS)) {
+		return;
+	}
+	last_check_ms = now;
+	uint32_t level = HAL_PWREx_GetTemperatureLevel();
+	if (level == PWR_TEMP_ABOVE_HIGH_THRESHOLD) {
+		printf("[THERMAL] MCU junction temp ABOVE HIGH THRESHOLD - reduce load/cooling!\r\n");
+	}
+}
+
+/* Heartbeat: toggles ERROR_LED (PC14) at ~0.5 Hz (1s on / 1s off) so that
+ * a slow steady blink = main loop alive, LED frozen = main loop hung or
+ * MCU in a fault handler.  If the fault handlers' fast-blink is firing
+ * (2–5 rapid blinks then a pause) that pattern overrides this steady blink,
+ * making the two conditions easy to tell apart visually. */
+#define HEARTBEAT_PERIOD_MS  1000u
+static void poll_main_loop_heartbeat(void)
+{
+  static uint32_t last_toggle_ms = 0;
+  uint32_t now = HAL_GetTick();
+  if ((now - last_toggle_ms) >= HEARTBEAT_PERIOD_MS)
+  {
+    last_toggle_ms = now;
+    HAL_GPIO_TogglePin(ERROR_LED_GPIO_Port, ERROR_LED_Pin);
+  }
+}
+
+static void PrintI2CSpeed(I2C_HandleTypeDef *hi2c)
+{
+  // Assuming the timing is configured in I2C_TIMINGR register
+  uint32_t timing = hi2c->Init.Timing;
+  uint32_t presc = (timing >> 28) & 0xF;
+  uint32_t scll = (timing >> 0) & 0xFF;
+  uint32_t sclh = (timing >> 8) & 0xFF;
+
+  // Get the I2C clock source frequency
+  uint32_t i2c_clk_freq = HAL_RCC_GetPCLK1Freq();
+
+  // Calculate SCL speed (frequency) in Hz
+  uint32_t scl_freq = i2c_clk_freq / ((presc + 1) * (scll + 1 + sclh + 1));
+
+  // Print I2C speed
+  printf("I2C Speed: %ld Hz\r\n", scl_freq); // Print the I2C speed in kHz
+}
+
+/* USER CODE END 0 */
+
+/**
+  * @brief  The application entry point.
+  * @retval int
+  */
+int main(void)
+{
+
+  /* USER CODE BEGIN 1 */
+
+  /* USER CODE END 1 */
+
+  /* MPU Configuration--------------------------------------------------------*/
+  MPU_Config();
+
+  /* MCU Configuration--------------------------------------------------------*/
+
+  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
+  HAL_Init();
+
+  /* USER CODE BEGIN Init */
+
+  /* USER CODE END Init */
+
+  /* Configure the system clock */
+  SystemClock_Config();
+
+  /* Configure the peripherals common clocks */
+  PeriphCommonClock_Config();
+
+  /* USER CODE BEGIN SysInit */
+
+  /* USER CODE END SysInit */
+
+  /* Initialize all configured peripherals */
+  MX_GPIO_Init();
+  MX_DMA_Init();
+  MX_BDMA_Init();
+  MX_CRC_Init();
+  MX_I2C1_Init();
+  MX_RNG_Init();
+  MX_SPI2_Init();
+  MX_SPI3_Init();
+  MX_SPI4_Init();
+  MX_SPI6_Init();
+  MX_TIM2_Init();
+  MX_UART4_Init();
+  MX_USART6_Init();
+  MX_TIM8_Init();
+  MX_TIM4_Init();
+  MX_USART1_Init();
+  MX_USART2_Init();
+  MX_USART3_Init();
+  MX_TIM14_Init();
+  MX_TIM16_Init();
+  MX_TIM5_Init();
+  MX_TIM15_Init();
+  /* USER CODE BEGIN 2 */
+  
+  if (HAL_TIM_Base_Start(&htim5) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  
+  init_dma_logging();
+
+  DWT_Init();
+
+  // enable I2C MUX
+  HAL_GPIO_WritePin(MUX_RESET_GPIO_Port, MUX_RESET_Pin, GPIO_PIN_RESET);
+
+  // enable USB PHY
+  HAL_GPIO_WritePin(USB_RESET_GPIO_Port, USB_RESET_Pin, GPIO_PIN_RESET);
+
+  printf("\033c");
+  fflush(stdout);
+  delay_ms(500);
+  printf("Openwater open-MOTION Aggregator\r\n\r\n");
+  printf("FW: %s (%s)\r\nDate: %s\r\n",
+       FW_VERSION_STRING,
+       FW_SHA_STRING,
+       FW_BUILD_TIME_STRING);
+  printf("CPU Clock Frequency: %lu MHz\r\n",
+         HAL_RCC_GetSysClockFreq() / 1000000);
+  print_reset_cause();
+  HAL_PWREx_EnableMonitoring();  /* Enable junction temp (TEMPH/TEMPL) monitoring */
+  printf("Initializing, please wait ...\r\n");
+
+  // enable HS USB MUX
+  HAL_GPIO_WritePin(USB_MUX_GPIO_Port, USB_MUX_Pin, GPIO_PIN_RESET);
+
+  // enable I2C MUX
+  HAL_GPIO_WritePin(MUX_RESET_GPIO_Port, MUX_RESET_Pin, GPIO_PIN_SET);
+
+  HAL_GPIO_WritePin(FS_OUT_EN_GPIO_Port, FS_OUT_EN_Pin, GPIO_PIN_RESET);  //enable Framesync output
+
+  // test i2c
+  PrintI2CSpeed(&hi2c1);
+  // I2C_scan(&hi2c1, NULL, 0, true);
+  delay_ms(100);
+  X02C1B_FSIN_EXT_disable();
+  GPIO_SetOutput(FSIN_EN_GPIO_Port, FSIN_EN_Pin, GPIO_PIN_RESET); // disable fsin output
+
+  if (ICM_WHOAMI() == ICM20948_EXPECTED_ID)
+  {
+	if(ICM_Init() != HAL_OK){
+		printf("Failed to initialize IMU\r\n\n");
+	}
+	else
+	{
+		printf("IMU detected\r\n");
+	    delay_ms(100);
+	    if(verbose_on) ICM_DumpRegisters();
+	}
+  }
+  else
+  {
+    printf("IMU NOT detected\r\n\n");
+  }
+
+  delay_ms(10);
+
+  HAL_GPIO_WritePin(USB_RESET_GPIO_Port, USB_RESET_Pin, GPIO_PIN_SET);
+
+  HAL_GPIO_WritePin(ERROR_LED_GPIO_Port, ERROR_LED_Pin, GPIO_PIN_SET);
+
+  init_camera_sensors(); // init structures and camera configs
+
+  // Select default camera
+  TCA9548A_SelectChannel(&hi2c1, 0x70, get_active_cam()->i2c_target);
+
+  delay_ms(250);
+  MX_USB_DEVICE_Init();
+  delay_ms(1000);
+  //GPIO_SetHiZ(GPIOA, GPIO_PIN_2);
+
+  comms_host_start();
+
+  /* Report any fault that occurred before the last reset.
+   * g_last_fault lives in .noinit RAM and survives a soft reset.
+   * This helps diagnose silent hang/fault scenarios. */
+  extern volatile FaultRecord_t g_last_fault;
+  if (g_last_fault.magic == 0xDEADBEEFUL)
+  {
+    static const char * const fault_names[] = {
+      "", "", "HardFault", "MemManage", "BusFault", "UsageFault"
+    };
+    uint32_t ft = g_last_fault.fault_type;
+    const char *fname = (ft >= 2u && ft <= 5u) ? fault_names[ft] : "Unknown";
+    printf("*** FAULT BEFORE LAST RESET: %s ***\r\n", fname);
+    printf("  HFSR=0x%08lX  CFSR=0x%08lX\r\n",
+           (unsigned long)g_last_fault.hfsr,
+           (unsigned long)g_last_fault.cfsr);
+    printf("  MMFAR=0x%08lX  BFAR=0x%08lX\r\n",
+           (unsigned long)g_last_fault.mmfar,
+           (unsigned long)g_last_fault.bfar);
+    g_last_fault.magic = 0;  /* Clear so we don't re-report on next boot */
+  }
+
+  // fill_frame_buffers();
+  printf("System Running\r\n");
+  /* USER CODE END 2 */
+
+  /* Infinite loop */
+  /* USER CODE BEGIN WHILE */
+  while (1)
+  {
+    /* USER CODE END WHILE */
+
+    /* USER CODE BEGIN 3 */
+  	comms_host_check_received(); // check comms
+    check_streaming();
+    poll_mcu_temperature();  /* Print warning if MCU junction temp above threshold */
+    poll_main_loop_heartbeat(); /* Toggle ERROR_LED slowly so we can tell if main loop is alive */
+  }
+
+  /* USER CODE END 3 */
+}
+
+/**
+  * @brief System Clock Configuration
+  * @retval None
+  */
+void SystemClock_Config(void)
+{
+  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
+  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
+
+  /** Supply configuration update enable
+  */
+  HAL_PWREx_ConfigSupply(PWR_LDO_SUPPLY);
+
+  /** Configure the main internal regulator output voltage
+  */
+  __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE0);
+
+  while(!__HAL_PWR_GET_FLAG(PWR_FLAG_VOSRDY)) {}
+
+  /** 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.PLL.PLLState = RCC_PLL_ON;
+  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
+  RCC_OscInitStruct.PLL.PLLM = 3;
+  RCC_OscInitStruct.PLL.PLLN = 120;
+  RCC_OscInitStruct.PLL.PLLP = 2;
+  RCC_OscInitStruct.PLL.PLLQ = 20;
+  RCC_OscInitStruct.PLL.PLLR = 2;
+  RCC_OscInitStruct.PLL.PLLRGE = RCC_PLL1VCIRANGE_3;
+  RCC_OscInitStruct.PLL.PLLVCOSEL = RCC_PLL1VCOWIDE;
+  RCC_OscInitStruct.PLL.PLLFRACN = 0;
+  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
+  {
+    Error_Handler();
+  }
+
+  /** Initializes the CPU, AHB and APB buses clocks
+  */
+  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
+                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2
+                              |RCC_CLOCKTYPE_D3PCLK1|RCC_CLOCKTYPE_D1PCLK1;
+  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
+  RCC_ClkInitStruct.SYSCLKDivider = RCC_SYSCLK_DIV1;
+  RCC_ClkInitStruct.AHBCLKDivider = RCC_HCLK_DIV2;
+  RCC_ClkInitStruct.APB3CLKDivider = RCC_APB3_DIV2;
+  RCC_ClkInitStruct.APB1CLKDivider = RCC_APB1_DIV2;
+  RCC_ClkInitStruct.APB2CLKDivider = RCC_APB2_DIV2;
+  RCC_ClkInitStruct.APB4CLKDivider = RCC_APB4_DIV2;
+
+  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_4) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  __HAL_RCC_PLL2CLKOUT_ENABLE(RCC_PLL2_DIVP);
+  HAL_RCC_MCOConfig(RCC_MCO2, RCC_MCO2SOURCE_PLL2PCLK, RCC_MCODIV_5);
+}
+
+/**
+  * @brief Peripherals Common Clock Configuration
+  * @retval None
+  */
+void PeriphCommonClock_Config(void)
+{
+  RCC_PeriphCLKInitTypeDef PeriphClkInitStruct = {0};
+
+  /** Initializes the peripherals clock
+  */
+  PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_SPI3|RCC_PERIPHCLK_SPI2;
+  PeriphClkInitStruct.PLL2.PLL2M = 6;
+  PeriphClkInitStruct.PLL2.PLL2N = 120;
+  PeriphClkInitStruct.PLL2.PLL2P = 4;
+  PeriphClkInitStruct.PLL2.PLL2Q = 2;
+  PeriphClkInitStruct.PLL2.PLL2R = 2;
+  PeriphClkInitStruct.PLL2.PLL2RGE = RCC_PLL2VCIRANGE_2;
+  PeriphClkInitStruct.PLL2.PLL2VCOSEL = RCC_PLL2VCOWIDE;
+  PeriphClkInitStruct.PLL2.PLL2FRACN = 0;
+  PeriphClkInitStruct.Spi123ClockSelection = RCC_SPI123CLKSOURCE_PLL2;
+  if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK)
+  {
+    Error_Handler();
+  }
+}
+
+/**
+  * @brief CRC Initialization Function
+  * @param None
+  * @retval None
+  */
+static void MX_CRC_Init(void)
+{
+
+  /* USER CODE BEGIN CRC_Init 0 */
+
+  /* USER CODE END CRC_Init 0 */
+
+  /* USER CODE BEGIN CRC_Init 1 */
+
+  /* USER CODE END CRC_Init 1 */
+  hcrc.Instance = CRC;
+  hcrc.Init.DefaultPolynomialUse = DEFAULT_POLYNOMIAL_ENABLE;
+  hcrc.Init.DefaultInitValueUse = DEFAULT_INIT_VALUE_ENABLE;
+  hcrc.Init.InputDataInversionMode = CRC_INPUTDATA_INVERSION_NONE;
+  hcrc.Init.OutputDataInversionMode = CRC_OUTPUTDATA_INVERSION_DISABLE;
+  hcrc.InputDataFormat = CRC_INPUTDATA_FORMAT_BYTES;
+  if (HAL_CRC_Init(&hcrc) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  /* USER CODE BEGIN CRC_Init 2 */
+
+  /* USER CODE END CRC_Init 2 */
+
+}
+
+/**
+  * @brief I2C1 Initialization Function
+  * @param None
+  * @retval None
+  */
+static void MX_I2C1_Init(void)
+{
+
+  /* USER CODE BEGIN I2C1_Init 0 */
+
+  /* USER CODE END I2C1_Init 0 */
+
+  /* USER CODE BEGIN I2C1_Init 1 */
+
+  /* USER CODE END I2C1_Init 1 */
+  hi2c1.Instance = I2C1;
+  hi2c1.Init.Timing = 0x00B03FDB;
+  hi2c1.Init.OwnAddress1 = 0;
+  hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
+  hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
+  hi2c1.Init.OwnAddress2 = 0;
+  hi2c1.Init.OwnAddress2Masks = I2C_OA2_NOMASK;
+  hi2c1.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
+  hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
+  if (HAL_I2C_Init(&hi2c1) != HAL_OK)
+  {
+    Error_Handler();
+  }
+
+  /** Configure Analogue filter
+  */
+  if (HAL_I2CEx_ConfigAnalogFilter(&hi2c1, I2C_ANALOGFILTER_ENABLE) != HAL_OK)
+  {
+    Error_Handler();
+  }
+
+  /** Configure Digital filter
+  */
+  if (HAL_I2CEx_ConfigDigitalFilter(&hi2c1, 0) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  /* USER CODE BEGIN I2C1_Init 2 */
+
+  /* USER CODE END I2C1_Init 2 */
+
+}
+
+/**
+  * @brief RNG Initialization Function
+  * @param None
+  * @retval None
+  */
+static void MX_RNG_Init(void)
+{
+
+  /* USER CODE BEGIN RNG_Init 0 */
+
+  /* USER CODE END RNG_Init 0 */
+
+  /* USER CODE BEGIN RNG_Init 1 */
+
+  /* USER CODE END RNG_Init 1 */
+  hrng.Instance = RNG;
+  hrng.Init.ClockErrorDetection = RNG_CED_ENABLE;
+  if (HAL_RNG_Init(&hrng) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  /* USER CODE BEGIN RNG_Init 2 */
+
+  /* USER CODE END RNG_Init 2 */
+
+}
+
+/**
+  * @brief SPI2 Initialization Function
+  * @param None
+  * @retval None
+  */
+static void MX_SPI2_Init(void)
+{
+
+  /* USER CODE BEGIN SPI2_Init 0 */
+
+  /* USER CODE END SPI2_Init 0 */
+
+  /* USER CODE BEGIN SPI2_Init 1 */
+
+  /* USER CODE END SPI2_Init 1 */
+  /* SPI2 parameter configuration*/
+  hspi2.Instance = SPI2;
+  hspi2.Init.Mode = SPI_MODE_SLAVE;
+  hspi2.Init.Direction = SPI_DIRECTION_2LINES;
+  hspi2.Init.DataSize = SPI_DATASIZE_8BIT;
+  hspi2.Init.CLKPolarity = SPI_POLARITY_HIGH;
+  hspi2.Init.CLKPhase = SPI_PHASE_2EDGE;
+  hspi2.Init.NSS = SPI_NSS_SOFT;
+  hspi2.Init.FirstBit = SPI_FIRSTBIT_LSB;
+  hspi2.Init.TIMode = SPI_TIMODE_DISABLE;
+  hspi2.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
+  hspi2.Init.CRCPolynomial = 0x0;
+  hspi2.Init.NSSPMode = SPI_NSS_PULSE_DISABLE;
+  hspi2.Init.NSSPolarity = SPI_NSS_POLARITY_LOW;
+  hspi2.Init.FifoThreshold = SPI_FIFO_THRESHOLD_01DATA;
+  hspi2.Init.TxCRCInitializationPattern = SPI_CRC_INITIALIZATION_ALL_ZERO_PATTERN;
+  hspi2.Init.RxCRCInitializationPattern = SPI_CRC_INITIALIZATION_ALL_ZERO_PATTERN;
+  hspi2.Init.MasterSSIdleness = SPI_MASTER_SS_IDLENESS_00CYCLE;
+  hspi2.Init.MasterInterDataIdleness = SPI_MASTER_INTERDATA_IDLENESS_00CYCLE;
+  hspi2.Init.MasterReceiverAutoSusp = SPI_MASTER_RX_AUTOSUSP_DISABLE;
+  hspi2.Init.MasterKeepIOState = SPI_MASTER_KEEP_IO_STATE_DISABLE;
+  hspi2.Init.IOSwap = SPI_IO_SWAP_DISABLE;
+  if (HAL_SPI_Init(&hspi2) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  /* USER CODE BEGIN SPI2_Init 2 */
+
+  /* USER CODE END SPI2_Init 2 */
+
+}
+
+/**
+  * @brief SPI3 Initialization Function
+  * @param None
+  * @retval None
+  */
+static void MX_SPI3_Init(void)
+{
+
+  /* USER CODE BEGIN SPI3_Init 0 */
+
+  /* USER CODE END SPI3_Init 0 */
+
+  /* USER CODE BEGIN SPI3_Init 1 */
+
+  /* USER CODE END SPI3_Init 1 */
+  /* SPI3 parameter configuration*/
+  hspi3.Instance = SPI3;
+  hspi3.Init.Mode = SPI_MODE_SLAVE;
+  hspi3.Init.Direction = SPI_DIRECTION_2LINES;
+  hspi3.Init.DataSize = SPI_DATASIZE_8BIT;
+  hspi3.Init.CLKPolarity = SPI_POLARITY_HIGH;
+  hspi3.Init.CLKPhase = SPI_PHASE_2EDGE;
+  hspi3.Init.NSS = SPI_NSS_SOFT;
+  hspi3.Init.FirstBit = SPI_FIRSTBIT_LSB;
+  hspi3.Init.TIMode = SPI_TIMODE_DISABLE;
+  hspi3.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
+  hspi3.Init.CRCPolynomial = 0x0;
+  hspi3.Init.NSSPMode = SPI_NSS_PULSE_DISABLE;
+  hspi3.Init.NSSPolarity = SPI_NSS_POLARITY_LOW;
+  hspi3.Init.FifoThreshold = SPI_FIFO_THRESHOLD_01DATA;
+  hspi3.Init.TxCRCInitializationPattern = SPI_CRC_INITIALIZATION_ALL_ZERO_PATTERN;
+  hspi3.Init.RxCRCInitializationPattern = SPI_CRC_INITIALIZATION_ALL_ZERO_PATTERN;
+  hspi3.Init.MasterSSIdleness = SPI_MASTER_SS_IDLENESS_00CYCLE;
+  hspi3.Init.MasterInterDataIdleness = SPI_MASTER_INTERDATA_IDLENESS_00CYCLE;
+  hspi3.Init.MasterReceiverAutoSusp = SPI_MASTER_RX_AUTOSUSP_DISABLE;
+  hspi3.Init.MasterKeepIOState = SPI_MASTER_KEEP_IO_STATE_DISABLE;
+  hspi3.Init.IOSwap = SPI_IO_SWAP_DISABLE;
+  if (HAL_SPI_Init(&hspi3) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  /* USER CODE BEGIN SPI3_Init 2 */
+
+  /* USER CODE END SPI3_Init 2 */
+
+}
+
+/**
+  * @brief SPI4 Initialization Function
+  * @param None
+  * @retval None
+  */
+static void MX_SPI4_Init(void)
+{
+
+  /* USER CODE BEGIN SPI4_Init 0 */
+
+  /* USER CODE END SPI4_Init 0 */
+
+  /* USER CODE BEGIN SPI4_Init 1 */
+
+  /* USER CODE END SPI4_Init 1 */
+  /* SPI4 parameter configuration*/
+  hspi4.Instance = SPI4;
+  hspi4.Init.Mode = SPI_MODE_SLAVE;
+  hspi4.Init.Direction = SPI_DIRECTION_2LINES;
+  hspi4.Init.DataSize = SPI_DATASIZE_8BIT;
+  hspi4.Init.CLKPolarity = SPI_POLARITY_HIGH;
+  hspi4.Init.CLKPhase = SPI_PHASE_2EDGE;
+  hspi4.Init.NSS = SPI_NSS_SOFT;
+  hspi4.Init.FirstBit = SPI_FIRSTBIT_LSB;
+  hspi4.Init.TIMode = SPI_TIMODE_DISABLE;
+  hspi4.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
+  hspi4.Init.CRCPolynomial = 0x0;
+  hspi4.Init.NSSPMode = SPI_NSS_PULSE_DISABLE;
+  hspi4.Init.NSSPolarity = SPI_NSS_POLARITY_LOW;
+  hspi4.Init.FifoThreshold = SPI_FIFO_THRESHOLD_01DATA;
+  hspi4.Init.TxCRCInitializationPattern = SPI_CRC_INITIALIZATION_ALL_ZERO_PATTERN;
+  hspi4.Init.RxCRCInitializationPattern = SPI_CRC_INITIALIZATION_ALL_ZERO_PATTERN;
+  hspi4.Init.MasterSSIdleness = SPI_MASTER_SS_IDLENESS_00CYCLE;
+  hspi4.Init.MasterInterDataIdleness = SPI_MASTER_INTERDATA_IDLENESS_00CYCLE;
+  hspi4.Init.MasterReceiverAutoSusp = SPI_MASTER_RX_AUTOSUSP_DISABLE;
+  hspi4.Init.MasterKeepIOState = SPI_MASTER_KEEP_IO_STATE_DISABLE;
+  hspi4.Init.IOSwap = SPI_IO_SWAP_DISABLE;
+  if (HAL_SPI_Init(&hspi4) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  /* USER CODE BEGIN SPI4_Init 2 */
+
+  /* USER CODE END SPI4_Init 2 */
+
+}
+
+/**
+  * @brief SPI6 Initialization Function
+  * @param None
+  * @retval None
+  */
+static void MX_SPI6_Init(void)
+{
+
+  /* USER CODE BEGIN SPI6_Init 0 */
+
+  /* USER CODE END SPI6_Init 0 */
+
+  /* USER CODE BEGIN SPI6_Init 1 */
+
+  /* USER CODE END SPI6_Init 1 */
+  /* SPI6 parameter configuration*/
+  hspi6.Instance = SPI6;
+  hspi6.Init.Mode = SPI_MODE_SLAVE;
+  hspi6.Init.Direction = SPI_DIRECTION_2LINES;
+  hspi6.Init.DataSize = SPI_DATASIZE_8BIT;
+  hspi6.Init.CLKPolarity = SPI_POLARITY_HIGH;
+  hspi6.Init.CLKPhase = SPI_PHASE_2EDGE;
+  hspi6.Init.NSS = SPI_NSS_SOFT;
+  hspi6.Init.FirstBit = SPI_FIRSTBIT_LSB;
+  hspi6.Init.TIMode = SPI_TIMODE_DISABLE;
+  hspi6.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
+  hspi6.Init.CRCPolynomial = 0x0;
+  hspi6.Init.NSSPMode = SPI_NSS_PULSE_DISABLE;
+  hspi6.Init.NSSPolarity = SPI_NSS_POLARITY_LOW;
+  hspi6.Init.FifoThreshold = SPI_FIFO_THRESHOLD_01DATA;
+  hspi6.Init.TxCRCInitializationPattern = SPI_CRC_INITIALIZATION_ALL_ZERO_PATTERN;
+  hspi6.Init.RxCRCInitializationPattern = SPI_CRC_INITIALIZATION_ALL_ZERO_PATTERN;
+  hspi6.Init.MasterSSIdleness = SPI_MASTER_SS_IDLENESS_00CYCLE;
+  hspi6.Init.MasterInterDataIdleness = SPI_MASTER_INTERDATA_IDLENESS_00CYCLE;
+  hspi6.Init.MasterReceiverAutoSusp = SPI_MASTER_RX_AUTOSUSP_DISABLE;
+  hspi6.Init.MasterKeepIOState = SPI_MASTER_KEEP_IO_STATE_DISABLE;
+  hspi6.Init.IOSwap = SPI_IO_SWAP_DISABLE;
+  if (HAL_SPI_Init(&hspi6) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  /* USER CODE BEGIN SPI6_Init 2 */
+
+  /* USER CODE END SPI6_Init 2 */
+
+}
+
+/**
+  * @brief TIM2 Initialization Function
+  * @param None
+  * @retval None
+  */
+static void MX_TIM2_Init(void)
+{
+
+  /* USER CODE BEGIN TIM2_Init 0 */
+
+  /* USER CODE END TIM2_Init 0 */
+
+  TIM_ClockConfigTypeDef sClockSourceConfig = {0};
+  TIM_MasterConfigTypeDef sMasterConfig = {0};
+  TIM_OC_InitTypeDef sConfigOC = {0};
+
+  /* USER CODE BEGIN TIM2_Init 1 */
+
+  /* USER CODE END TIM2_Init 1 */
+  htim2.Instance = TIM2;
+  htim2.Init.Prescaler = 0;
+  htim2.Init.CounterMode = TIM_COUNTERMODE_UP;
+  htim2.Init.Period = 4294967295;
+  htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
+  htim2.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
+  if (HAL_TIM_Base_Init(&htim2) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
+  if (HAL_TIM_ConfigClockSource(&htim2, &sClockSourceConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  if (HAL_TIM_PWM_Init(&htim2) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
+  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
+  if (HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  sConfigOC.OCMode = TIM_OCMODE_PWM1;
+  sConfigOC.Pulse = 0;
+  sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
+  sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
+  if (HAL_TIM_PWM_ConfigChannel(&htim2, &sConfigOC, TIM_CHANNEL_2) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  /* USER CODE BEGIN TIM2_Init 2 */
+
+  /* USER CODE END TIM2_Init 2 */
+  HAL_TIM_MspPostInit(&htim2);
+
+}
+
+/**
+  * @brief TIM4 Initialization Function
+  * @param None
+  * @retval None
+  */
+static void MX_TIM4_Init(void)
+{
+
+  /* USER CODE BEGIN TIM4_Init 0 */
+
+  /* USER CODE END TIM4_Init 0 */
+
+  TIM_MasterConfigTypeDef sMasterConfig = {0};
+  TIM_OC_InitTypeDef sConfigOC = {0};
+
+  /* USER CODE BEGIN TIM4_Init 1 */
+
+  /* USER CODE END TIM4_Init 1 */
+  htim4.Instance = TIM4;
+  htim4.Init.Prescaler = 1000;
+  htim4.Init.CounterMode = TIM_COUNTERMODE_UP;
+  htim4.Init.Period = 5999;
+  htim4.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
+  htim4.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
+  if (HAL_TIM_PWM_Init(&htim4) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  if (HAL_TIM_OC_Init(&htim4) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
+  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
+  if (HAL_TIMEx_MasterConfigSynchronization(&htim4, &sMasterConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  sConfigOC.OCMode = TIM_OCMODE_PWM1;
+  sConfigOC.Pulse = 3000;
+  sConfigOC.OCPolarity = TIM_OCPOLARITY_LOW;
+  sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
+  if (HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_2) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  sConfigOC.OCMode = TIM_OCMODE_TIMING;
+  sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
+  if (HAL_TIM_OC_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_3) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  /* USER CODE BEGIN TIM4_Init 2 */
+  LL_TIM_EnableIT_CC1(TIM4);
+  /* USER CODE END TIM4_Init 2 */
+  HAL_TIM_MspPostInit(&htim4);
+
+}
+
+/**
+  * @brief TIM5 Initialization Function
+  * @param None
+  * @retval None
+  */
+static void MX_TIM5_Init(void)
+{
+
+  /* USER CODE BEGIN TIM5_Init 0 */
+
+  /* USER CODE END TIM5_Init 0 */
+
+  TIM_ClockConfigTypeDef sClockSourceConfig = {0};
+  TIM_MasterConfigTypeDef sMasterConfig = {0};
+
+  /* USER CODE BEGIN TIM5_Init 1 */
+
+  /* USER CODE END TIM5_Init 1 */
+  htim5.Instance = TIM5;
+  htim5.Init.Prescaler = 2400-1;
+  htim5.Init.CounterMode = TIM_COUNTERMODE_UP;
+  htim5.Init.Period = 0xFFFFFFFF;
+  htim5.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
+  htim5.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
+  if (HAL_TIM_Base_Init(&htim5) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
+  if (HAL_TIM_ConfigClockSource(&htim5, &sClockSourceConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
+  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
+  if (HAL_TIMEx_MasterConfigSynchronization(&htim5, &sMasterConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  /* USER CODE BEGIN TIM5_Init 2 */
+
+  /* USER CODE END TIM5_Init 2 */
+
+}
+
+/**
+  * @brief TIM8 Initialization Function
+  * @param None
+  * @retval None
+  */
+static void MX_TIM8_Init(void)
+{
+
+  /* USER CODE BEGIN TIM8_Init 0 */
+
+  /* USER CODE END TIM8_Init 0 */
+
+  TIM_ClockConfigTypeDef sClockSourceConfig = {0};
+  TIM_MasterConfigTypeDef sMasterConfig = {0};
+
+  /* USER CODE BEGIN TIM8_Init 1 */
+
+  /* USER CODE END TIM8_Init 1 */
+  htim8.Instance = TIM8;
+  htim8.Init.Prescaler = 0;
+  htim8.Init.CounterMode = TIM_COUNTERMODE_UP;
+  htim8.Init.Period = 10-1;
+  htim8.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
+  htim8.Init.RepetitionCounter = 0;
+  htim8.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
+  if (HAL_TIM_Base_Init(&htim8) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
+  if (HAL_TIM_ConfigClockSource(&htim8, &sClockSourceConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
+  sMasterConfig.MasterOutputTrigger2 = TIM_TRGO2_RESET;
+  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
+  if (HAL_TIMEx_MasterConfigSynchronization(&htim8, &sMasterConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  /* USER CODE BEGIN TIM8_Init 2 */
+
+  /* USER CODE END TIM8_Init 2 */
+
+}
+
+/**
+  * @brief TIM14 Initialization Function
+  * @param None
+  * @retval None
+  */
+static void MX_TIM14_Init(void)
+{
+
+  /* USER CODE BEGIN TIM14_Init 0 */
+
+  /* USER CODE END TIM14_Init 0 */
+
+  /* USER CODE BEGIN TIM14_Init 1 */
+
+  /* USER CODE END TIM14_Init 1 */
+  htim14.Instance = TIM14;
+  htim14.Init.Prescaler = 240-1;
+  htim14.Init.CounterMode = TIM_COUNTERMODE_UP;
+  htim14.Init.Period = 5000-1;
+  htim14.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
+  htim14.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
+  if (HAL_TIM_Base_Init(&htim14) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  /* USER CODE BEGIN TIM14_Init 2 */
+
+  /* USER CODE END TIM14_Init 2 */
+
+}
+
+/**
+  * @brief TIM15 Initialization Function
+  * @param None
+  * @retval None
+  */
+static void MX_TIM15_Init(void)
+{
+
+  /* USER CODE BEGIN TIM15_Init 0 */
+
+  /* USER CODE END TIM15_Init 0 */
+
+  TIM_ClockConfigTypeDef sClockSourceConfig = {0};
+  TIM_MasterConfigTypeDef sMasterConfig = {0};
+
+  /* USER CODE BEGIN TIM15_Init 1 */
+
+  /* USER CODE END TIM15_Init 1 */
+  htim15.Instance = TIM15;
+  htim15.Init.Prescaler = 240-1;
+  htim15.Init.CounterMode = TIM_COUNTERMODE_UP;
+  htim15.Init.Period = 65535;
+  htim15.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
+  htim15.Init.RepetitionCounter = 0;
+  htim15.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
+  if (HAL_TIM_Base_Init(&htim15) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
+  if (HAL_TIM_ConfigClockSource(&htim15, &sClockSourceConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
+  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
+  if (HAL_TIMEx_MasterConfigSynchronization(&htim15, &sMasterConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  /* USER CODE BEGIN TIM15_Init 2 */
+
+  /* USER CODE END TIM15_Init 2 */
+
+}
+
+/**
+  * @brief TIM16 Initialization Function
+  * @param None
+  * @retval None
+  */
+static void MX_TIM16_Init(void)
+{
+
+  /* USER CODE BEGIN TIM16_Init 0 */
+
+  /* USER CODE END TIM16_Init 0 */
+
+  /* USER CODE BEGIN TIM16_Init 1 */
+
+  /* USER CODE END TIM16_Init 1 */
+  htim16.Instance = TIM16;
+  htim16.Init.Prescaler = 240-1;
+  htim16.Init.CounterMode = TIM_COUNTERMODE_UP;
+  htim16.Init.Period = 25000-1;
+  htim16.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
+  htim16.Init.RepetitionCounter = 0;
+  htim16.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
+  if (HAL_TIM_Base_Init(&htim16) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  /* USER CODE BEGIN TIM16_Init 2 */
+
+  /* USER CODE END TIM16_Init 2 */
+
+}
+
+/**
+  * @brief UART4 Initialization Function
+  * @param None
+  * @retval None
+  */
+static void MX_UART4_Init(void)
+{
+
+  /* USER CODE BEGIN UART4_Init 0 */
+
+  /* USER CODE END UART4_Init 0 */
+
+  /* USER CODE BEGIN UART4_Init 1 */
+
+  /* USER CODE END UART4_Init 1 */
+  huart4.Instance = UART4;
+  huart4.Init.BaudRate = 115200;
+  huart4.Init.WordLength = UART_WORDLENGTH_8B;
+  huart4.Init.StopBits = UART_STOPBITS_1;
+  huart4.Init.Parity = UART_PARITY_NONE;
+  huart4.Init.Mode = UART_MODE_TX_RX;
+  huart4.Init.HwFlowCtl = UART_HWCONTROL_NONE;
+  huart4.Init.OverSampling = UART_OVERSAMPLING_16;
+  huart4.Init.OneBitSampling = UART_ONE_BIT_SAMPLE_DISABLE;
+  huart4.Init.ClockPrescaler = UART_PRESCALER_DIV1;
+  huart4.AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_NO_INIT;
+  if (HAL_UART_Init(&huart4) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  if (HAL_UARTEx_SetTxFifoThreshold(&huart4, UART_TXFIFO_THRESHOLD_1_8) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  if (HAL_UARTEx_SetRxFifoThreshold(&huart4, UART_RXFIFO_THRESHOLD_1_8) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  if (HAL_UARTEx_DisableFifoMode(&huart4) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  /* USER CODE BEGIN UART4_Init 2 */
+
+  /* USER CODE END UART4_Init 2 */
+
+}
+
+/**
+  * @brief USART1 Initialization Function
+  * @param None
+  * @retval None
+  */
+static void MX_USART1_Init(void)
+{
+
+  /* USER CODE BEGIN USART1_Init 0 */
+
+  /* USER CODE END USART1_Init 0 */
+
+  /* USER CODE BEGIN USART1_Init 1 */
+
+  /* USER CODE END USART1_Init 1 */
+  husart1.Instance = USART1;
+  husart1.Init.BaudRate = 115200;
+  husart1.Init.WordLength = USART_WORDLENGTH_8B;
+  husart1.Init.StopBits = USART_STOPBITS_1;
+  husart1.Init.Parity = USART_PARITY_NONE;
+  husart1.Init.Mode = USART_MODE_RX;
+  husart1.Init.CLKPolarity = USART_POLARITY_HIGH;
+  husart1.Init.CLKPhase = USART_PHASE_2EDGE;
+  husart1.Init.CLKLastBit = USART_LASTBIT_DISABLE;
+  husart1.Init.ClockPrescaler = USART_PRESCALER_DIV1;
+  husart1.SlaveMode = USART_SLAVEMODE_ENABLE;
+  if (HAL_USART_Init(&husart1) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  if (HAL_USARTEx_SetTxFifoThreshold(&husart1, USART_TXFIFO_THRESHOLD_1_8) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  if (HAL_USARTEx_SetRxFifoThreshold(&husart1, USART_RXFIFO_THRESHOLD_8_8) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  if (HAL_USARTEx_EnableFifoMode(&husart1) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  if (HAL_USARTEx_EnableSlaveMode(&husart1) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  /* USER CODE BEGIN USART1_Init 2 */
+
+  /* USER CODE END USART1_Init 2 */
+
+}
+
+/**
+  * @brief USART2 Initialization Function
+  * @param None
+  * @retval None
+  */
+static void MX_USART2_Init(void)
+{
+
+  /* USER CODE BEGIN USART2_Init 0 */
+
+  /* USER CODE END USART2_Init 0 */
+
+  /* USER CODE BEGIN USART2_Init 1 */
+
+  /* USER CODE END USART2_Init 1 */
+  husart2.Instance = USART2;
+  husart2.Init.BaudRate = 5000000;
+  husart2.Init.WordLength = USART_WORDLENGTH_8B;
+  husart2.Init.StopBits = USART_STOPBITS_1;
+  husart2.Init.Parity = USART_PARITY_NONE;
+  husart2.Init.Mode = USART_MODE_RX;
+  husart2.Init.CLKPolarity = USART_POLARITY_HIGH;
+  husart2.Init.CLKPhase = USART_PHASE_2EDGE;
+  husart2.Init.CLKLastBit = USART_LASTBIT_DISABLE;
+  husart2.Init.ClockPrescaler = USART_PRESCALER_DIV1;
+  husart2.SlaveMode = USART_SLAVEMODE_ENABLE;
+  if (HAL_USART_Init(&husart2) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  if (HAL_USARTEx_SetTxFifoThreshold(&husart2, USART_TXFIFO_THRESHOLD_1_8) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  if (HAL_USARTEx_SetRxFifoThreshold(&husart2, USART_RXFIFO_THRESHOLD_8_8) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  if (HAL_USARTEx_EnableFifoMode(&husart2) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  if (HAL_USARTEx_EnableSlaveMode(&husart2) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  /* USER CODE BEGIN USART2_Init 2 */
+
+  /* USER CODE END USART2_Init 2 */
+
+}
+
+/**
+  * @brief USART3 Initialization Function
+  * @param None
+  * @retval None
+  */
+static void MX_USART3_Init(void)
+{
+
+  /* USER CODE BEGIN USART3_Init 0 */
+
+  /* USER CODE END USART3_Init 0 */
+
+  /* USER CODE BEGIN USART3_Init 1 */
+
+  /* USER CODE END USART3_Init 1 */
+  husart3.Instance = USART3;
+  husart3.Init.BaudRate = 115200;
+  husart3.Init.WordLength = USART_WORDLENGTH_8B;
+  husart3.Init.StopBits = USART_STOPBITS_1;
+  husart3.Init.Parity = USART_PARITY_NONE;
+  husart3.Init.Mode = USART_MODE_RX;
+  husart3.Init.CLKPolarity = USART_POLARITY_HIGH;
+  husart3.Init.CLKPhase = USART_PHASE_2EDGE;
+  husart3.Init.CLKLastBit = USART_LASTBIT_DISABLE;
+  husart3.Init.ClockPrescaler = USART_PRESCALER_DIV1;
+  husart3.SlaveMode = USART_SLAVEMODE_ENABLE;
+  if (HAL_USART_Init(&husart3) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  if (HAL_USARTEx_SetTxFifoThreshold(&husart3, USART_TXFIFO_THRESHOLD_1_8) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  if (HAL_USARTEx_SetRxFifoThreshold(&husart3, USART_RXFIFO_THRESHOLD_8_8) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  if (HAL_USARTEx_EnableFifoMode(&husart3) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  if (HAL_USARTEx_EnableSlaveMode(&husart3) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  /* USER CODE BEGIN USART3_Init 2 */
+
+  /* USER CODE END USART3_Init 2 */
+
+}
+
+/**
+  * @brief USART6 Initialization Function
+  * @param None
+  * @retval None
+  */
+static void MX_USART6_Init(void)
+{
+
+  /* USER CODE BEGIN USART6_Init 0 */
+
+  /* USER CODE END USART6_Init 0 */
+
+  /* USER CODE BEGIN USART6_Init 1 */
+
+  /* USER CODE END USART6_Init 1 */
+  husart6.Instance = USART6;
+  husart6.Init.BaudRate = 4167000;
+  husart6.Init.WordLength = USART_WORDLENGTH_8B;
+  husart6.Init.StopBits = USART_STOPBITS_1;
+  husart6.Init.Parity = USART_PARITY_NONE;
+  husart6.Init.Mode = USART_MODE_RX;
+  husart6.Init.CLKPolarity = USART_POLARITY_HIGH;
+  husart6.Init.CLKPhase = USART_PHASE_2EDGE;
+  husart6.Init.CLKLastBit = USART_LASTBIT_DISABLE;
+  husart6.Init.ClockPrescaler = USART_PRESCALER_DIV1;
+  husart6.SlaveMode = USART_SLAVEMODE_ENABLE;
+  if (HAL_USART_Init(&husart6) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  if (HAL_USARTEx_SetTxFifoThreshold(&husart6, USART_TXFIFO_THRESHOLD_1_8) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  if (HAL_USARTEx_SetRxFifoThreshold(&husart6, USART_RXFIFO_THRESHOLD_8_8) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  if (HAL_USARTEx_EnableFifoMode(&husart6) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  if (HAL_USARTEx_EnableSlaveMode(&husart6) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  /* USER CODE BEGIN USART6_Init 2 */
+
+  /* USER CODE END USART6_Init 2 */
+
+}
+
+/**
+  * Enable DMA controller clock
+  */
+static void MX_BDMA_Init(void)
+{
+
+  /* DMA controller clock enable */
+  __HAL_RCC_BDMA_CLK_ENABLE();
+
+  /* DMA interrupt init */
+  /* BDMA_Channel0_IRQn interrupt configuration */
+  HAL_NVIC_SetPriority(BDMA_Channel0_IRQn, DMA_IRQ_PRIORITY, 0);
+  HAL_NVIC_EnableIRQ(BDMA_Channel0_IRQn);
+
+}
+
+/**
+  * Enable DMA controller clock
+  * Configure DMA for memory to memory transfers
+  *   hdma_memtomem_dma2_stream1
+  */
+static void MX_DMA_Init(void)
+{
+
+  /* DMA controller clock enable */
+  __HAL_RCC_DMA1_CLK_ENABLE();
+  __HAL_RCC_DMA2_CLK_ENABLE();
+
+  /* Configure DMA request hdma_memtomem_dma2_stream1 on DMA2_Stream1 */
+  hdma_memtomem_dma2_stream1.Instance = DMA2_Stream1;
+  hdma_memtomem_dma2_stream1.Init.Request = DMA_REQUEST_MEM2MEM;
+  hdma_memtomem_dma2_stream1.Init.Direction = DMA_MEMORY_TO_MEMORY;
+  hdma_memtomem_dma2_stream1.Init.PeriphInc = DMA_PINC_DISABLE;
+  hdma_memtomem_dma2_stream1.Init.MemInc = DMA_MINC_ENABLE;
+  hdma_memtomem_dma2_stream1.Init.PeriphDataAlignment = DMA_PDATAALIGN_WORD;
+  hdma_memtomem_dma2_stream1.Init.MemDataAlignment = DMA_MDATAALIGN_WORD;
+  hdma_memtomem_dma2_stream1.Init.Mode = DMA_NORMAL;
+  hdma_memtomem_dma2_stream1.Init.Priority = DMA_PRIORITY_MEDIUM;
+  hdma_memtomem_dma2_stream1.Init.FIFOMode = DMA_FIFOMODE_ENABLE;
+  hdma_memtomem_dma2_stream1.Init.FIFOThreshold = DMA_FIFO_THRESHOLD_FULL;
+  hdma_memtomem_dma2_stream1.Init.MemBurst = DMA_MBURST_INC4;
+  hdma_memtomem_dma2_stream1.Init.PeriphBurst = DMA_PBURST_INC4;
+  if (HAL_DMA_Init(&hdma_memtomem_dma2_stream1) != HAL_OK)
+  {
+    Error_Handler( );
+  }
+
+  /* DMA interrupt init */
+  /* DMA1_Stream0_IRQn interrupt configuration */
+  HAL_NVIC_SetPriority(DMA1_Stream0_IRQn, 0, 0);
+  HAL_NVIC_EnableIRQ(DMA1_Stream0_IRQn);
+  /* DMA1_Stream1_IRQn interrupt configuration */
+  HAL_NVIC_SetPriority(DMA1_Stream1_IRQn, 0, 0);
+  HAL_NVIC_EnableIRQ(DMA1_Stream1_IRQn);
+  /* DMA1_Stream2_IRQn interrupt configuration */
+  HAL_NVIC_SetPriority(DMA1_Stream2_IRQn, 0, 0);
+  HAL_NVIC_EnableIRQ(DMA1_Stream2_IRQn);
+  /* DMA1_Stream3_IRQn interrupt configuration */
+  HAL_NVIC_SetPriority(DMA1_Stream3_IRQn, 0, 0);
+  HAL_NVIC_EnableIRQ(DMA1_Stream3_IRQn);
+  /* DMA1_Stream4_IRQn interrupt configuration */
+  HAL_NVIC_SetPriority(DMA1_Stream4_IRQn, 0, 0);
+  HAL_NVIC_EnableIRQ(DMA1_Stream4_IRQn);
+  /* DMA1_Stream5_IRQn interrupt configuration */
+  HAL_NVIC_SetPriority(DMA1_Stream5_IRQn, 0, 0);
+  HAL_NVIC_EnableIRQ(DMA1_Stream5_IRQn);
+  /* DMA1_Stream6_IRQn interrupt configuration */
+  HAL_NVIC_SetPriority(DMA1_Stream6_IRQn, 0, 0);
+  HAL_NVIC_EnableIRQ(DMA1_Stream6_IRQn);
+  /* DMA1_Stream7_IRQn interrupt configuration */
+  HAL_NVIC_SetPriority(DMA1_Stream7_IRQn, 0, 0);
+  HAL_NVIC_EnableIRQ(DMA1_Stream7_IRQn);
+  /* DMA2_Stream0_IRQn interrupt configuration */
+  HAL_NVIC_SetPriority(DMA2_Stream0_IRQn, 0, 0);
+  HAL_NVIC_EnableIRQ(DMA2_Stream0_IRQn);
+
+}
+
+/**
+  * @brief GPIO Initialization Function
+  * @param None
+  * @retval None
+  */
+static void MX_GPIO_Init(void)
+{
+  GPIO_InitTypeDef GPIO_InitStruct = {0};
+  /* USER CODE BEGIN MX_GPIO_Init_1 */
+  /* USER CODE END MX_GPIO_Init_1 */
+
+  /* GPIO Ports Clock Enable */
+  __HAL_RCC_GPIOC_CLK_ENABLE();
+  __HAL_RCC_GPIOE_CLK_ENABLE();
+  __HAL_RCC_GPIOB_CLK_ENABLE();
+  __HAL_RCC_GPIOA_CLK_ENABLE();
+  __HAL_RCC_GPIOD_CLK_ENABLE();
+  __HAL_RCC_GPIOH_CLK_ENABLE();
+
+  /*Configure GPIO pin Output Level */
+  HAL_GPIO_WritePin(GPIOC, ERROR_LED_Pin|MUX_RESET_Pin|USB_MUX_Pin, GPIO_PIN_RESET);
+
+  /*Configure GPIO pin Output Level */
+  HAL_GPIO_WritePin(USB_RESET_GPIO_Port, USB_RESET_Pin, GPIO_PIN_RESET);
+
+  /*Configure GPIO pin Output Level */
+  HAL_GPIO_WritePin(GPIOD, CAM_PWR_1_Pin|CAM_PWR_5_Pin|CAM_PWR_8_Pin|CAM_PWR_4_Pin
+                          |FAN_CTL_Pin|FS_OUT_EN_Pin, GPIO_PIN_SET);
+
+  /*Configure GPIO pin Output Level */
+  HAL_GPIO_WritePin(GPIOE, CAM_PWR_7_Pin|CAM_PWR_2_Pin|FSIN_EN_Pin, GPIO_PIN_SET);
+
+  /*Configure GPIO pin Output Level */
+  HAL_GPIO_WritePin(CAM_PWR_3_GPIO_Port, CAM_PWR_3_Pin, GPIO_PIN_SET);
+
+  /*Configure GPIO pin Output Level */
+  HAL_GPIO_WritePin(CAM_PWR_6_GPIO_Port, CAM_PWR_6_Pin, GPIO_PIN_SET);
+
+  /*Configure GPIO pins : ERROR_LED_Pin MUX_RESET_Pin USB_MUX_Pin CAM_PWR_6_Pin */
+  GPIO_InitStruct.Pin = ERROR_LED_Pin|MUX_RESET_Pin|USB_MUX_Pin|CAM_PWR_6_Pin;
+  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
+  GPIO_InitStruct.Pull = GPIO_NOPULL;
+  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+  HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
+
+  /*Configure GPIO pins : GPIO0_8_Pin IMU_INT_Pin */
+  GPIO_InitStruct.Pin = GPIO0_8_Pin|IMU_INT_Pin;
+  GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
+  GPIO_InitStruct.Pull = GPIO_NOPULL;
+  HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
+
+  /*Configure GPIO pins : GPIO0_7_Pin GPIO0_5_Pin */
+  GPIO_InitStruct.Pin = GPIO0_7_Pin|GPIO0_5_Pin;
+  GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
+  GPIO_InitStruct.Pull = GPIO_NOPULL;
+  HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
+
+  /*Configure GPIO pins : USB_RESET_Pin CAM_PWR_3_Pin */
+  GPIO_InitStruct.Pin = USB_RESET_Pin|CAM_PWR_3_Pin;
+  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
+  GPIO_InitStruct.Pull = GPIO_NOPULL;
+  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+  HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
+
+  /*Configure GPIO pins : GPIO0_6_Pin CRESET_8_Pin CRESET_7_Pin PE14
+                           GPIO0_2_Pin GPIO0_3_Pin CRESET_1_Pin CRESET_3_Pin
+                           CRESET_2_Pin CRESET_4_Pin */
+  GPIO_InitStruct.Pin = GPIO0_6_Pin|CRESET_8_Pin|CRESET_7_Pin|GPIO_PIN_14
+                          |GPIO0_2_Pin|GPIO0_3_Pin|CRESET_1_Pin|CRESET_3_Pin
+                          |CRESET_2_Pin|CRESET_4_Pin;
+  GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
+  GPIO_InitStruct.Pull = GPIO_NOPULL;
+  HAL_GPIO_Init(GPIOE, &GPIO_InitStruct);
+
+  /*Configure GPIO pins : CRESET_6_Pin CRESET_5_Pin GPIO0_4_Pin */
+  GPIO_InitStruct.Pin = CRESET_6_Pin|CRESET_5_Pin|GPIO0_4_Pin;
+  GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
+  GPIO_InitStruct.Pull = GPIO_NOPULL;
+  HAL_GPIO_Init(GPIOD, &GPIO_InitStruct);
+
+  /*Configure GPIO pins : CAM_PWR_1_Pin CAM_PWR_5_Pin CAM_PWR_8_Pin CAM_PWR_4_Pin
+                           FAN_CTL_Pin FS_OUT_EN_Pin */
+  GPIO_InitStruct.Pin = CAM_PWR_1_Pin|CAM_PWR_5_Pin|CAM_PWR_8_Pin|CAM_PWR_4_Pin
+                          |FAN_CTL_Pin|FS_OUT_EN_Pin;
+  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
+  GPIO_InitStruct.Pull = GPIO_NOPULL;
+  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+  HAL_GPIO_Init(GPIOD, &GPIO_InitStruct);
+
+  /*Configure GPIO pins : PA12 PA11 */
+  GPIO_InitStruct.Pin = GPIO_PIN_12|GPIO_PIN_11;
+  GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
+  GPIO_InitStruct.Pull = GPIO_NOPULL;
+  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+  GPIO_InitStruct.Alternate = GPIO_AF10_OTG1_FS;
+  HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
+
+  /*Configure GPIO pins : CAM_PWR_7_Pin CAM_PWR_2_Pin FSIN_EN_Pin */
+  GPIO_InitStruct.Pin = CAM_PWR_7_Pin|CAM_PWR_2_Pin|FSIN_EN_Pin;
+  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
+  GPIO_InitStruct.Pull = GPIO_NOPULL;
+  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+  HAL_GPIO_Init(GPIOE, &GPIO_InitStruct);
+
+  /*Configure GPIO pin : PC9 */
+  GPIO_InitStruct.Pin = GPIO_PIN_9;
+  GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
+  GPIO_InitStruct.Pull = GPIO_NOPULL;
+  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
+  GPIO_InitStruct.Alternate = GPIO_AF0_MCO;
+  HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
+
+  /*Configure GPIO pin : GPIO0_1_Pin */
+  GPIO_InitStruct.Pin = GPIO0_1_Pin;
+  GPIO_InitStruct.Mode = GPIO_MODE_ANALOG;
+  GPIO_InitStruct.Pull = GPIO_NOPULL;
+  HAL_GPIO_Init(GPIO0_1_GPIO_Port, &GPIO_InitStruct);
+
+  /* USER CODE BEGIN MX_GPIO_Init_2 */
+  /* USER CODE END MX_GPIO_Init_2 */
+}
+
+/* USER CODE BEGIN 4 */
+
+void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
+{
+}
+
+void HAL_UART_TxCpltCallback(UART_HandleTypeDef *huart)
+{
+  if (huart->Instance == UART4)
+  {
+    logging_UART_TxCpltCallback(huart);
+  }
+}
+
+// Error handling callback for USART
+void HAL_USART_ErrorCallback(USART_HandleTypeDef *husart)
+{
+  int8_t cam_id = -1;
+
+  if (husart->Instance == USART1)
+  {
+    cam_id = 4;
+  }
+  else if (husart->Instance == USART2)
+  {
+    cam_id = 0;
+  }
+  else if (husart->Instance == USART3)
+  {
+    cam_id = 2;
+  }
+  else if (husart->Instance == USART6)
+  {
+    cam_id = 3;
+  }
+
+  // Print which USART instance caused the error
+  if (husart->Instance == USART1)
+  {
+    printf("Error in USART1: ");
+  }
+  else if (husart->Instance == USART2)
+  {
+    printf("Error in USART2: ");
+  }
+  else if (husart->Instance == USART3)
+  {
+    printf("Error in USART3: ");
+  }
+  else if (husart->Instance == USART6)
+  {
+    printf("Error in USART6: ");
+  }
+  else
+  {
+    printf("Error in Unknown USART instance: ");
+  }
+
+  // Identify specific errors using error codes
+  if (husart->ErrorCode & HAL_USART_ERROR_PE)
+  {
+    printf("Parity error ");
+  }
+  if (husart->ErrorCode & HAL_USART_ERROR_NE)
+  {
+    printf("Noise error ");
+  }
+  if (husart->ErrorCode & HAL_USART_ERROR_FE)
+  {
+    printf("Framing error ");
+  }
+  if (husart->ErrorCode & HAL_USART_ERROR_ORE)
+  {
+    printf("Overrun error ");
+    __HAL_USART_CLEAR_OREFLAG(husart);
+  }
+  if (husart->ErrorCode & HAL_USART_ERROR_DMA)
+  {
+    printf("DMA transfer error ");
+  }
+  printf("\r\n");
+
+  /* Attempt graceful recovery for any known camera USART peripheral.
+   * Same fix as HAL_SPI_ErrorCallback: non-overrun errors (framing, noise,
+   * DMA) previously fell through to Error_Handler() and halted the MCU.
+   * Any error on a known camera USART is recoverable via abort+restart. */
+  if (cam_id >= 0)
+  {
+    abort_data_reception((uint8_t)cam_id);
+    start_data_reception((uint8_t)cam_id);
+    return;
+  }
+
+  /* Truly unknown USART peripheral — log and continue rather than halting. */
+  printf("[USART] Unhandled USART error on unknown peripheral, continuing.\r\n");
+}
+
+// Error handling callback for SPI
+void HAL_SPI_ErrorCallback(SPI_HandleTypeDef *hspi)
+{
+  int8_t cam_id = -1;
+
+  if (hspi->Instance == SPI2)
+  {
+    cam_id = 6;
+  }
+  else if (hspi->Instance == SPI3)
+  {
+    cam_id = 5;
+  }
+  else if (hspi->Instance == SPI4)
+  {
+    cam_id = 7;
+  }
+  else if (hspi->Instance == SPI6)
+  {
+    cam_id = 1;
+  }
+
+  // Print which SPI instance caused the error
+  if (hspi->Instance == SPI2)
+  {
+    printf("Error in SPI2: ");
+  }
+  else if (hspi->Instance == SPI3)
+  {
+    printf("Error in SPI3: ");
+  }
+  else if (hspi->Instance == SPI4)
+  {
+    printf("Error in SPI4: ");
+  }
+  else if (hspi->Instance == SPI6)
+  {
+    printf("Error in SPI6: ");
+  }
+  else
+  {
+    printf("Error in Unknown SPI instance: ");
+  }
+  
+  // Identify specific errors using error codes
+  if (hspi->ErrorCode & HAL_SPI_ERROR_OVR)
+  {
+    printf("Overrun error ");
+    __HAL_SPI_CLEAR_OVRFLAG(hspi);
+  }
+  if (hspi->ErrorCode & HAL_SPI_ERROR_MODF)
+  {
+    printf("Mode fault error ");
+  }
+  if (hspi->ErrorCode & HAL_SPI_ERROR_CRC)
+  {
+    printf("CRC error ");
+  }
+  if (hspi->ErrorCode & HAL_SPI_ERROR_FRE)
+  {
+    printf("Frame error ");
+  }
+  if (hspi->ErrorCode & HAL_SPI_ERROR_DMA)
+  {
+    if (hspi->hdmarx->ErrorCode == HAL_DMA_ERROR_TE)
+      printf("TE - HAL_DMA_ERROR_TE");
+    if (hspi->hdmarx->ErrorCode == HAL_DMA_ERROR_FE)
+      printf("HAL_DMA_ERROR_FE ");
+    if (hspi->hdmarx->ErrorCode == HAL_DMA_ERROR_DME)
+      printf("HAL_DMA_ERROR_DME");
+    if (hspi->hdmarx->ErrorCode == HAL_DMA_ERROR_TIMEOUT)
+      printf("HAL_DMA_ERROR_TIMEOUT");
+    if (hspi->hdmarx->ErrorCode == HAL_DMA_ERROR_PARAM)
+      printf("HAL_DMA_ERROR_PARAM");
+    if (hspi->hdmarx->ErrorCode == HAL_DMA_ERROR_NO_XFER)
+      printf("HAL_DMA_ERROR_NO_XFER");
+    if (hspi->hdmarx->ErrorCode == HAL_DMA_ERROR_NOT_SUPPORTED)
+      printf("HAL_DMA_ERROR_NOT_SUPPORTED");
+    if (hspi->hdmarx->ErrorCode == HAL_DMA_ERROR_SYNC)
+      printf("HAL_DMA_ERROR_SYNC");
+    if (hspi->hdmarx->ErrorCode == HAL_DMA_ERROR_REQGEN)
+      printf("HAL_DMA_ERROR_REQGEN");
+    if (hspi->hdmarx->ErrorCode == HAL_DMA_ERROR_BUSY)
+      printf("HAL_DMA_ERROR_BUSY");
+  }
+  printf("\r\n");
+
+  /* Attempt graceful recovery for any known camera SPI peripheral.
+   * Previously only overrun errors were recovered here; all other error types
+   * fell through to Error_Handler() which calls __disable_irq() + while(1),
+   * killing USB and all other peripherals.  The observed failure mode was:
+   *   Camera 6 overheats → SPI3 DMA/frame error (not OVR) →
+   *   Error_Handler() entered from interrupt context →
+   *   wait_for_usb_queues_to_finish() blocks (USB ISR can't run) →
+   *   __disable_irq() → MCU completely dark.
+   * Now any error on a known camera SPI triggers abort+restart instead. */
+  if (cam_id >= 0)
+  {
+    abort_data_reception((uint8_t)cam_id);
+    start_data_reception((uint8_t)cam_id);
+    return;
+  }
+
+  /* Truly unknown SPI peripheral — log and continue rather than halting. */
+  printf("[SPI] Unhandled SPI error on unknown peripheral, continuing.\r\n");
+}
+
+
+void set_event_bit_atomic(uint32_t bit) {
+  __disable_irq();
+  event_bits |= bit;
+  __enable_irq();
+}
+
+// Interrupt handler for SPI reception
+void HAL_SPI_RxCpltCallback(SPI_HandleTypeDef *hspi)
+{
+  if (hspi->Instance == SPI2)
+  {
+    set_event_bit_atomic(BIT_6);
+  }
+  else if (hspi->Instance == SPI3)
+  {
+    set_event_bit_atomic(BIT_5);
+  }
+  else if (hspi->Instance == SPI4)
+  {
+    set_event_bit_atomic(BIT_7);
+  }
+  else if (hspi->Instance == SPI6)
+  {
+    set_event_bit_atomic(BIT_1);
+  }
+}
+
+void HAL_USART_RxCpltCallback(USART_HandleTypeDef *husart)
+{
+  if (husart->Instance == USART1)
+  { // Check if the interrupt is for USART2
+    set_event_bit_atomic(BIT_4);
+  }
+  else if (husart->Instance == USART2)
+  { // Check if the interrupt is for USART2
+    set_event_bit_atomic(BIT_0);
+  }
+  else if (husart->Instance == USART3)
+  { // Check if the interrupt is for USART2
+    set_event_bit_atomic(BIT_2);
+  }
+  else if (husart->Instance == USART6)
+  { // Check if the interrupt is for USART2
+    set_event_bit_atomic(BIT_3);
+  }
+
+}
+
+void HAL_TIM_PWM_PulseFinishedCallback(TIM_HandleTypeDef *htim)
+{
+  if (htim->Instance == TIM4) // Call data sender (internal FSIN))
+  {
+    send_data();
+  }
+}
+
+void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
+{     
+    if (GPIO_Pin == GPIO_PIN_13) // Call REAL data sender if interrupt hit and enabled
+    {
+      pulse_count++;
+      send_data();
+    }
+}
+
+void ExitRun0Mode(void) {
+    // Temporary stub for ExitRun0Mode
+}
+
+/**
+ * @brief Wait for all USB data queues to finish sending
+ * @note This function polls USB endpoint status flags until all transmissions complete
+ *       or a timeout occurs. This ensures error messages and diagnostic data are
+ *       transmitted before the system halts.
+ */
+static void wait_for_usb_queues_to_finish(void)
+{
+  const uint32_t timeout_ms = 1000;  // Maximum wait time: 1 second
+  uint32_t start_time = get_timestamp_ms();
+  uint32_t elapsed = 0;
+  
+  printf("Waiting for USB queues to finish...\r\n");
+  fflush(stdout);
+  
+  // Poll until all endpoints are idle or timeout
+  while (elapsed < timeout_ms) {
+    _Bool all_idle = true;
+    
+    // Check COMMS endpoint (tx_flag: 1 = idle, 0 = busy)
+    if (tx_flag == 0) {
+      all_idle = false;
+    }
+    
+    // Check COMMS bulk endpoint (comms_ep_data: 0 = idle, 1 = transmitting)
+    if (comms_ep_data != 0) {
+      all_idle = false;
+    }
+    
+    // Check HISTO endpoint (histo_ep_data: 0 = idle, 1 = transmitting)
+    if (histo_ep_data != 0) {
+      all_idle = false;
+    }
+    
+    // Check IMU endpoint (imu_ep_data: 0 = idle, 1 = transmitting)
+    if (imu_ep_data != 0) {
+      all_idle = false;
+    }
+    
+    if (all_idle) {
+      printf("All USB queues finished.\r\n");
+      fflush(stdout);
+      return;
+    }
+    
+    // Small delay to avoid busy-waiting
+    delay_ms(1);
+    elapsed = get_timestamp_ms() - start_time;
+  }
+  
+  printf("USB queue wait timeout after %lu ms (some data may not have been sent).\r\n", elapsed);
+  fflush(stdout);
+}
+/* USER CODE END 4 */
+
+ /* MPU Configuration */
+
+void MPU_Config(void)
+{
+  MPU_Region_InitTypeDef MPU_InitStruct = {0};
+
+  /* Disables the MPU */
+  HAL_MPU_Disable();
+
+  /** Initializes and configures the Region and the memory to be protected
+  */
+  MPU_InitStruct.Enable = MPU_REGION_ENABLE;
+  MPU_InitStruct.Number = MPU_REGION_NUMBER0;
+  MPU_InitStruct.BaseAddress = 0x0;
+  MPU_InitStruct.Size = MPU_REGION_SIZE_4GB;
+  MPU_InitStruct.SubRegionDisable = 0x87;
+  MPU_InitStruct.TypeExtField = MPU_TEX_LEVEL0;
+  MPU_InitStruct.AccessPermission = MPU_REGION_NO_ACCESS;
+  MPU_InitStruct.DisableExec = MPU_INSTRUCTION_ACCESS_DISABLE;
+  MPU_InitStruct.IsShareable = MPU_ACCESS_SHAREABLE;
+  MPU_InitStruct.IsCacheable = MPU_ACCESS_NOT_CACHEABLE;
+  MPU_InitStruct.IsBufferable = MPU_ACCESS_NOT_BUFFERABLE;
+
+  HAL_MPU_ConfigRegion(&MPU_InitStruct);
+  /* Enables the MPU */
+  HAL_MPU_Enable(MPU_PRIVILEGED_DEFAULT);
+
+}
+
+/**
+  * @brief  Period elapsed callback in non blocking mode
+  * @note   This function is called  when TIM17 interrupt took place, inside
+  * HAL_TIM_IRQHandler(). It makes a direct call to HAL_IncTick() to increment
+  * a global variable "uwTick" used as application time base.
+  * @param  htim : TIM handle
+  * @retval None
+  */
+void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
+{
+  /* USER CODE BEGIN Callback 0 */
+
+  /* USER CODE END Callback 0 */
+  if (htim->Instance == TIM17)
+  {
+    HAL_IncTick();
+  }
+  /* USER CODE BEGIN Callback 1 */
+  if (htim->Instance == TIM16){
+	  HistoFake_GenerateAndSend(&hUsbDeviceHS);
+  }
+
+  if (htim->Instance == TIM14)
+  {
+	  imu_frame_counter++;
+	  // call imu
+	  if(ICM_GetAllRawData(&a,&t, &g, &m) != HAL_OK){
+		  printf("IMU Read Error\r\n");
+	  }else{
+		  memset(usb_buf,0,128);
+		  int len = snprintf(
+		      usb_buf, sizeof(usb_buf),
+		      "{\"F\":%ld,\"G\":[%d,%d,%d],\"M\":[%d,%d,%d],\"A\":[%d,%d,%d],\"T\":%d.%02d}\r\n",
+			  imu_frame_counter,
+		      g.x, g.y, g.z,
+		      m.x, m.y, m.z,
+		      a.x, a.y, a.z,
+			  (int)t, (int)((t - (int)t) * 100.0f)
+		  );
+		  USBD_IMU_SetTxBuffer(&hUsbDeviceHS, (uint8_t *)usb_buf, len);
+	  }
+  }
+
+  if (htim->Instance == TIM15) {
+    HAL_TIM_Base_Stop_IT(htim);
+    if(_enter_dfu) {
+      *((uint32_t *)0x38000000) = 0xDEADBEEF; 
+    }
+
+
+    // De-initialize other specific modules
+    HAL_RNG_DeInit(&hrng);
+    HAL_CRC_DeInit(&hcrc);
+    
+    MX_USB_DEVICE_DeInit();
+    delay_ms(300);
+    // Reset the board
+    NVIC_SystemReset();
+
+  }
+  
+  /* USER CODE END Callback 1 */
+}
+
+/**
+  * @brief  This function is executed in case of error occurrence.
+  * @retval None
+  */
+
+void Error_Handler(void)
+{
+  /* USER CODE BEGIN Error_Handler_Debug */
+  /* User can add his own implementation to report the HAL error return state */
+
+  uint32_t *stack_ptr;
+
+  HAL_GPIO_TogglePin(ERROR_LED_GPIO_Port, ERROR_LED_Pin);
+  printf(">>> HARD FAULT <<<\r\n");
+  fflush(stdout); // Ensure the output is flushed immediately
+
+  // Get the current stack pointer
+  __ASM volatile("MRS %0, MSP" : "=r"(stack_ptr));
+
+  // Print general-purpose registers
+  printf("Stack Pointer (MSP): 0x%08lX\r\n", (unsigned long)stack_ptr);
+  printf("Register dump:\r\n");
+  printf("R0 : 0x%08lX\r\n", stack_ptr[0]);
+  printf("R1 : 0x%08lX\r\n", stack_ptr[1]);
+  printf("R2 : 0x%08lX\r\n", stack_ptr[2]);
+  printf("R3 : 0x%08lX\r\n", stack_ptr[3]);
+  printf("R12: 0x%08lX\r\n", stack_ptr[4]);
+  printf("LR : 0x%08lX (Link Register)\r\n", stack_ptr[5]);
+  printf("PC : 0x%08lX (Program Counter)\r\n", stack_ptr[6]);
+  printf("xPSR: 0x%08lX\r\n", stack_ptr[7]);
+  fflush(stdout);
+
+  // Wait for all USB data queues to finish sending before halting
+  wait_for_usb_queues_to_finish();
+
+  delay_ms(100);
+  __disable_irq();
+  while (1)
+  {
+  }
+  /* USER CODE END Error_Handler_Debug */
+}
+#ifdef USE_FULL_ASSERT
+/**
+  * @brief  Reports the name of the source file and the source line number
+  *         where the assert_param error has occurred.
+  * @param  file: pointer to the source file name
+  * @param  line: assert_param error line source number
+  * @retval None
+  */
+void assert_failed(uint8_t *file, uint32_t line)
+{
+  /* USER CODE BEGIN 6 */
+  /* User can add his own implementation to report the file name and line number,
+     ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
+  /* USER CODE END 6 */
+}
+#endif /* USE_FULL_ASSERT */
diff --git a/Core/Src/stm32h7xx_it.c b/Core/Src/stm32h7xx_it.c
index 51d0b2d..461f592 100644
--- a/Core/Src/stm32h7xx_it.c
+++ b/Core/Src/stm32h7xx_it.c
@@ -105,13 +105,80 @@ void NMI_Handler(void)
   /* USER CODE END NonMaskableInt_IRQn 1 */
 }
 
+/* ---------------------------------------------------------------------------
+ * Fault blink helper.
+ *
+ * Blinks ERROR_LED (PC14) N times fast (5 Hz) then pauses 1 s, repeating
+ * forever.  N encodes which fault fired:
+ *   2 blinks = HardFault
+ *   3 blinks = MemManage
+ *   4 blinks = BusFault
+ *   5 blinks = UsageFault
+ *
+ * Called with interrupts already disabled (we are inside a fault handler).
+ * Uses direct register writes so it works even if the HAL is corrupt.
+ * Also writes the fault address and status registers to a noinit RAM variable
+ * so they survive a subsequent watchdog reset for post-mortem inspection.
+ * ---------------------------------------------------------------------------
+ */
+typedef struct {
+  uint32_t magic;      /* 0xDEADBEEF if populated */
+  uint32_t fault_type; /* 2=Hard 3=Mem 4=Bus 5=Usage */
+  uint32_t hfsr;       /* HardFault Status Register */
+  uint32_t cfsr;       /* Configurable Fault Status Register */
+  uint32_t mmfar;      /* MemManage Fault Address Register */
+  uint32_t bfar;       /* BusFault Address Register */
+  uint32_t lr_at_fault;/* LR saved on fault entry */
+  uint32_t pc_at_fault;/* PC saved on fault entry */
+} FaultRecord_t;
+
+__attribute__((section(".noinit")))
+volatile FaultRecord_t g_last_fault;
+
+static void __attribute__((noreturn)) fault_blink(uint32_t fault_type)
+{
+  /* Capture fault status registers */
+  g_last_fault.magic      = 0xDEADBEEFUL;
+  g_last_fault.fault_type = fault_type;
+  g_last_fault.hfsr       = SCB->HFSR;
+  g_last_fault.cfsr       = SCB->CFSR;
+  g_last_fault.mmfar      = SCB->MMFAR;
+  g_last_fault.bfar       = SCB->BFAR;
+  /* Caller's LR/PC are on the stack but we can't safely unwind here;
+   * a debugger can inspect the stacked frame directly. */
+
+  /* Disable all interrupts — we own the CPU now */
+  __disable_irq();
+
+  /* Configure PC14 (ERROR_LED) as push-pull output via direct register write.
+   * GPIOC clock should already be enabled from normal init. */
+  GPIOC->MODER  = (GPIOC->MODER  & ~(3U << (14 * 2))) | (1U << (14 * 2));
+  GPIOC->OTYPER &= ~(1U << 14);
+  GPIOC->OSPEEDR = (GPIOC->OSPEEDR & ~(3U << (14 * 2)));  /* low speed */
+  GPIOC->PUPDR  &= ~(3U << (14 * 2));
+
+  while (1)
+  {
+    /* Blink N times at ~5 Hz */
+    for (uint32_t i = 0; i < fault_type; i++)
+    {
+      GPIOC->BSRR = (1U << 14);           /* LED ON  */
+      for (volatile uint32_t d = 0; d < 960000U; d++) {}  /* ~100 ms @ 480 MHz */
+      GPIOC->BSRR = (1U << (14 + 16));    /* LED OFF */
+      for (volatile uint32_t d = 0; d < 960000U; d++) {}  /* ~100 ms */
+    }
+    /* Long pause between blink groups */
+    for (volatile uint32_t d = 0; d < 9600000U; d++) {}   /* ~1 s */
+  }
+}
+
 /**
   * @brief This function handles Hard fault interrupt.
   */
 void HardFault_Handler(void)
 {
   /* USER CODE BEGIN HardFault_IRQn 0 */
-
+  fault_blink(2);  /* 2 blinks = HardFault */
   /* USER CODE END HardFault_IRQn 0 */
   while (1)
   {
@@ -126,7 +193,7 @@ void HardFault_Handler(void)
 void MemManage_Handler(void)
 {
   /* USER CODE BEGIN MemoryManagement_IRQn 0 */
-
+  fault_blink(3);  /* 3 blinks = MemManage */
   /* USER CODE END MemoryManagement_IRQn 0 */
   while (1)
   {
@@ -141,7 +208,7 @@ void MemManage_Handler(void)
 void BusFault_Handler(void)
 {
   /* USER CODE BEGIN BusFault_IRQn 0 */
-
+  fault_blink(4);  /* 4 blinks = BusFault */
   /* USER CODE END BusFault_IRQn 0 */
   while (1)
   {
@@ -156,7 +223,7 @@ void BusFault_Handler(void)
 void UsageFault_Handler(void)
 {
   /* USER CODE BEGIN UsageFault_IRQn 0 */
-
+  fault_blink(5);  /* 5 blinks = UsageFault */
   /* USER CODE END UsageFault_IRQn 0 */
   while (1)
   {
diff --git a/STM32H743VIHX_FLASH.ld b/STM32H743VIHX_FLASH.ld
index 05911f3..57cac1a 100644
--- a/STM32H743VIHX_FLASH.ld
+++ b/STM32H743VIHX_FLASH.ld
@@ -166,6 +166,15 @@ SECTIONS
     . = ALIGN(8);
   } >RAM_D1
 
+/* Fault record: intentionally NOT zeroed at reset so it survives a watchdog
+ * or NVIC soft reset.  Checked at boot to report the last CPU fault. */
+.noinit (NOLOAD) :
+{
+  . = ALIGN(4);
+  *(.noinit)
+  . = ALIGN(4);
+} >RAM_D1
+
 /* Place USB buffers in RAM_D2 */
 .ram_d2 (NOLOAD) :
 {

From c7f4273dace7854644557ac4d10dc7dd9b202f50 Mon Sep 17 00:00:00 2001
From: boringethan <ethan@openwater.cc>
Date: Mon, 23 Mar 2026 12:08:37 -0700
Subject: [PATCH 05/11] undo brown out detection stuff

---
 Core/Src/main.c         | 67 ++++--------------------------------
 Core/Src/stm32h7xx_it.c | 75 +++--------------------------------------
 STM32H743VIHX_FLASH.ld  |  9 -----
 3 files changed, 10 insertions(+), 141 deletions(-)

diff --git a/Core/Src/main.c b/Core/Src/main.c
index 0cdf17f..ceb5a7c 100644
--- a/Core/Src/main.c
+++ b/Core/Src/main.c
@@ -160,7 +160,7 @@ static void MX_TIM16_Init(void);
 static void MX_TIM5_Init(void);
 static void MX_TIM15_Init(void);
 /* USER CODE BEGIN PFP */
-static void poll_main_loop_heartbeat(void);
+
 /* USER CODE END PFP */
 
 /* Private user code ---------------------------------------------------------*/
@@ -225,23 +225,6 @@ static void poll_mcu_temperature(void)
 	}
 }
 
-/* Heartbeat: toggles ERROR_LED (PC14) at ~0.5 Hz (1s on / 1s off) so that
- * a slow steady blink = main loop alive, LED frozen = main loop hung or
- * MCU in a fault handler.  If the fault handlers' fast-blink is firing
- * (2–5 rapid blinks then a pause) that pattern overrides this steady blink,
- * making the two conditions easy to tell apart visually. */
-#define HEARTBEAT_PERIOD_MS  1000u
-static void poll_main_loop_heartbeat(void)
-{
-  static uint32_t last_toggle_ms = 0;
-  uint32_t now = HAL_GetTick();
-  if ((now - last_toggle_ms) >= HEARTBEAT_PERIOD_MS)
-  {
-    last_toggle_ms = now;
-    HAL_GPIO_TogglePin(ERROR_LED_GPIO_Port, ERROR_LED_Pin);
-  }
-}
-
 static void PrintI2CSpeed(I2C_HandleTypeDef *hi2c)
 {
   // Assuming the timing is configured in I2C_TIMINGR register
@@ -398,28 +381,6 @@ int main(void)
   //GPIO_SetHiZ(GPIOA, GPIO_PIN_2);
 
   comms_host_start();
-
-  /* Report any fault that occurred before the last reset.
-   * g_last_fault lives in .noinit RAM and survives a soft reset.
-   * This helps diagnose silent hang/fault scenarios. */
-  extern volatile FaultRecord_t g_last_fault;
-  if (g_last_fault.magic == 0xDEADBEEFUL)
-  {
-    static const char * const fault_names[] = {
-      "", "", "HardFault", "MemManage", "BusFault", "UsageFault"
-    };
-    uint32_t ft = g_last_fault.fault_type;
-    const char *fname = (ft >= 2u && ft <= 5u) ? fault_names[ft] : "Unknown";
-    printf("*** FAULT BEFORE LAST RESET: %s ***\r\n", fname);
-    printf("  HFSR=0x%08lX  CFSR=0x%08lX\r\n",
-           (unsigned long)g_last_fault.hfsr,
-           (unsigned long)g_last_fault.cfsr);
-    printf("  MMFAR=0x%08lX  BFAR=0x%08lX\r\n",
-           (unsigned long)g_last_fault.mmfar,
-           (unsigned long)g_last_fault.bfar);
-    g_last_fault.magic = 0;  /* Clear so we don't re-report on next boot */
-  }
-
   // fill_frame_buffers();
   printf("System Running\r\n");
   /* USER CODE END 2 */
@@ -431,10 +392,9 @@ int main(void)
     /* USER CODE END WHILE */
 
     /* USER CODE BEGIN 3 */
-  	comms_host_check_received(); // check comms
+  	comms_host_check_received(); // check comms  
     check_streaming();
     poll_mcu_temperature();  /* Print warning if MCU junction temp above threshold */
-    poll_main_loop_heartbeat(); /* Toggle ERROR_LED slowly so we can tell if main loop is alive */
   }
 
   /* USER CODE END 3 */
@@ -1677,19 +1637,14 @@ void HAL_USART_ErrorCallback(USART_HandleTypeDef *husart)
   }
   printf("\r\n");
 
-  /* Attempt graceful recovery for any known camera USART peripheral.
-   * Same fix as HAL_SPI_ErrorCallback: non-overrun errors (framing, noise,
-   * DMA) previously fell through to Error_Handler() and halted the MCU.
-   * Any error on a known camera USART is recoverable via abort+restart. */
-  if (cam_id >= 0)
+  if ((husart->ErrorCode & HAL_USART_ERROR_ORE) != 0U && cam_id >= 0)
   {
     abort_data_reception((uint8_t)cam_id);
     start_data_reception((uint8_t)cam_id);
     return;
   }
 
-  /* Truly unknown USART peripheral — log and continue rather than halting. */
-  printf("[USART] Unhandled USART error on unknown peripheral, continuing.\r\n");
+  Error_Handler();
 }
 
 // Error handling callback for SPI
@@ -1779,24 +1734,14 @@ void HAL_SPI_ErrorCallback(SPI_HandleTypeDef *hspi)
   }
   printf("\r\n");
 
-  /* Attempt graceful recovery for any known camera SPI peripheral.
-   * Previously only overrun errors were recovered here; all other error types
-   * fell through to Error_Handler() which calls __disable_irq() + while(1),
-   * killing USB and all other peripherals.  The observed failure mode was:
-   *   Camera 6 overheats → SPI3 DMA/frame error (not OVR) →
-   *   Error_Handler() entered from interrupt context →
-   *   wait_for_usb_queues_to_finish() blocks (USB ISR can't run) →
-   *   __disable_irq() → MCU completely dark.
-   * Now any error on a known camera SPI triggers abort+restart instead. */
-  if (cam_id >= 0)
+  if ((hspi->ErrorCode & HAL_SPI_ERROR_OVR) != 0U && cam_id >= 0)
   {
     abort_data_reception((uint8_t)cam_id);
     start_data_reception((uint8_t)cam_id);
     return;
   }
 
-  /* Truly unknown SPI peripheral — log and continue rather than halting. */
-  printf("[SPI] Unhandled SPI error on unknown peripheral, continuing.\r\n");
+  Error_Handler(); // Handle any error during re-enabling
 }
 
 
diff --git a/Core/Src/stm32h7xx_it.c b/Core/Src/stm32h7xx_it.c
index 461f592..51d0b2d 100644
--- a/Core/Src/stm32h7xx_it.c
+++ b/Core/Src/stm32h7xx_it.c
@@ -105,80 +105,13 @@ void NMI_Handler(void)
   /* USER CODE END NonMaskableInt_IRQn 1 */
 }
 
-/* ---------------------------------------------------------------------------
- * Fault blink helper.
- *
- * Blinks ERROR_LED (PC14) N times fast (5 Hz) then pauses 1 s, repeating
- * forever.  N encodes which fault fired:
- *   2 blinks = HardFault
- *   3 blinks = MemManage
- *   4 blinks = BusFault
- *   5 blinks = UsageFault
- *
- * Called with interrupts already disabled (we are inside a fault handler).
- * Uses direct register writes so it works even if the HAL is corrupt.
- * Also writes the fault address and status registers to a noinit RAM variable
- * so they survive a subsequent watchdog reset for post-mortem inspection.
- * ---------------------------------------------------------------------------
- */
-typedef struct {
-  uint32_t magic;      /* 0xDEADBEEF if populated */
-  uint32_t fault_type; /* 2=Hard 3=Mem 4=Bus 5=Usage */
-  uint32_t hfsr;       /* HardFault Status Register */
-  uint32_t cfsr;       /* Configurable Fault Status Register */
-  uint32_t mmfar;      /* MemManage Fault Address Register */
-  uint32_t bfar;       /* BusFault Address Register */
-  uint32_t lr_at_fault;/* LR saved on fault entry */
-  uint32_t pc_at_fault;/* PC saved on fault entry */
-} FaultRecord_t;
-
-__attribute__((section(".noinit")))
-volatile FaultRecord_t g_last_fault;
-
-static void __attribute__((noreturn)) fault_blink(uint32_t fault_type)
-{
-  /* Capture fault status registers */
-  g_last_fault.magic      = 0xDEADBEEFUL;
-  g_last_fault.fault_type = fault_type;
-  g_last_fault.hfsr       = SCB->HFSR;
-  g_last_fault.cfsr       = SCB->CFSR;
-  g_last_fault.mmfar      = SCB->MMFAR;
-  g_last_fault.bfar       = SCB->BFAR;
-  /* Caller's LR/PC are on the stack but we can't safely unwind here;
-   * a debugger can inspect the stacked frame directly. */
-
-  /* Disable all interrupts — we own the CPU now */
-  __disable_irq();
-
-  /* Configure PC14 (ERROR_LED) as push-pull output via direct register write.
-   * GPIOC clock should already be enabled from normal init. */
-  GPIOC->MODER  = (GPIOC->MODER  & ~(3U << (14 * 2))) | (1U << (14 * 2));
-  GPIOC->OTYPER &= ~(1U << 14);
-  GPIOC->OSPEEDR = (GPIOC->OSPEEDR & ~(3U << (14 * 2)));  /* low speed */
-  GPIOC->PUPDR  &= ~(3U << (14 * 2));
-
-  while (1)
-  {
-    /* Blink N times at ~5 Hz */
-    for (uint32_t i = 0; i < fault_type; i++)
-    {
-      GPIOC->BSRR = (1U << 14);           /* LED ON  */
-      for (volatile uint32_t d = 0; d < 960000U; d++) {}  /* ~100 ms @ 480 MHz */
-      GPIOC->BSRR = (1U << (14 + 16));    /* LED OFF */
-      for (volatile uint32_t d = 0; d < 960000U; d++) {}  /* ~100 ms */
-    }
-    /* Long pause between blink groups */
-    for (volatile uint32_t d = 0; d < 9600000U; d++) {}   /* ~1 s */
-  }
-}
-
 /**
   * @brief This function handles Hard fault interrupt.
   */
 void HardFault_Handler(void)
 {
   /* USER CODE BEGIN HardFault_IRQn 0 */
-  fault_blink(2);  /* 2 blinks = HardFault */
+
   /* USER CODE END HardFault_IRQn 0 */
   while (1)
   {
@@ -193,7 +126,7 @@ void HardFault_Handler(void)
 void MemManage_Handler(void)
 {
   /* USER CODE BEGIN MemoryManagement_IRQn 0 */
-  fault_blink(3);  /* 3 blinks = MemManage */
+
   /* USER CODE END MemoryManagement_IRQn 0 */
   while (1)
   {
@@ -208,7 +141,7 @@ void MemManage_Handler(void)
 void BusFault_Handler(void)
 {
   /* USER CODE BEGIN BusFault_IRQn 0 */
-  fault_blink(4);  /* 4 blinks = BusFault */
+
   /* USER CODE END BusFault_IRQn 0 */
   while (1)
   {
@@ -223,7 +156,7 @@ void BusFault_Handler(void)
 void UsageFault_Handler(void)
 {
   /* USER CODE BEGIN UsageFault_IRQn 0 */
-  fault_blink(5);  /* 5 blinks = UsageFault */
+
   /* USER CODE END UsageFault_IRQn 0 */
   while (1)
   {
diff --git a/STM32H743VIHX_FLASH.ld b/STM32H743VIHX_FLASH.ld
index 57cac1a..05911f3 100644
--- a/STM32H743VIHX_FLASH.ld
+++ b/STM32H743VIHX_FLASH.ld
@@ -166,15 +166,6 @@ SECTIONS
     . = ALIGN(8);
   } >RAM_D1
 
-/* Fault record: intentionally NOT zeroed at reset so it survives a watchdog
- * or NVIC soft reset.  Checked at boot to report the last CPU fault. */
-.noinit (NOLOAD) :
-{
-  . = ALIGN(4);
-  *(.noinit)
-  . = ALIGN(4);
-} >RAM_D1
-
 /* Place USB buffers in RAM_D2 */
 .ram_d2 (NOLOAD) :
 {

From 4b164015edc257e7007255c8ca2f8c7c23ef841a Mon Sep 17 00:00:00 2001
From: boringethan <ethan@openwater.cc>
Date: Mon, 23 Mar 2026 12:14:05 -0700
Subject: [PATCH 06/11] add crc16 checksum

---
 Core/Inc/camera_manager.h |  5 +++++
 Core/Src/camera_manager.c | 17 +++++++++++++----
 2 files changed, 18 insertions(+), 4 deletions(-)

diff --git a/Core/Inc/camera_manager.h b/Core/Inc/camera_manager.h
index 1ef6426..90e6cfc 100644
--- a/Core/Inc/camera_manager.h
+++ b/Core/Inc/camera_manager.h
@@ -52,6 +52,11 @@ typedef struct {
 #define TYPE_HISTO_CMP 0x01
 #define HISTO_HEADER_SIZE 6
 #define HISTO_TRAILER_SIZE 3
+/* TYPE_HISTO_CMP packets carry an extra 2-byte CRC-16 of the *uncompressed*
+ * payload immediately before the normal packet footer.  This lets the SDK
+ * verify that decompression produced the correct output independently of the
+ * transport-level CRC that only covers the compressed bytes. */
+#define HISTO_CMP_UNCMP_CRC_SIZE 2
 
 
 void init_camera_sensors(void);
diff --git a/Core/Src/camera_manager.c b/Core/Src/camera_manager.c
index 7680345..379ffe2 100644
--- a/Core/Src/camera_manager.c
+++ b/Core/Src/camera_manager.c
@@ -1440,7 +1440,7 @@ _Bool send_histogram_data_cmp(void) {
 	}
 
 	/* --- Compress payload into packet_buffer (after header) --- */
-	int dst_max = HISTO_JSON_BUFFER_SIZE - HISTO_HEADER_SIZE - HISTO_TRAILER_SIZE;
+	int dst_max = HISTO_JSON_BUFFER_SIZE - HISTO_HEADER_SIZE - HISTO_CMP_UNCMP_CRC_SIZE - HISTO_TRAILER_SIZE;
 
 	uint32_t cyc_start = DWT->CYCCNT;
 	int cmp_len = rle_compress(uncmp_payload, p_off,
@@ -1464,7 +1464,7 @@ _Bool send_histogram_data_cmp(void) {
 	 * flag worth watching; above ~20 ms risks SPI overrun on the next frame. */
 #define CMP_BUDGET_WARNING_US  10000UL  /* 10 ms */
 	if (elapsed_us > CMP_BUDGET_WARNING_US) {
-		printf("[CMP] WARN: compression took %lu us (>20 ms frame budget!), %d cams, "
+		printf("[CMP] WARN: compression took %lu us (>10 ms warning threshold), %d cams, "
 		       "%d->%d bytes (ratio %d%%)\r\n",
 		       (unsigned long)elapsed_us, count, p_off, cmp_len,
 		       (p_off > 0) ? (cmp_len * 100 / p_off) : 0);
@@ -1476,7 +1476,7 @@ _Bool send_histogram_data_cmp(void) {
 	cmp_frame_count++;
 
 	/* --- Header --- */
-	uint32_t total_size = HISTO_HEADER_SIZE + (uint32_t)cmp_len + HISTO_TRAILER_SIZE;
+	uint32_t total_size = HISTO_HEADER_SIZE + (uint32_t)cmp_len + HISTO_CMP_UNCMP_CRC_SIZE + HISTO_TRAILER_SIZE;
 	int offset = 0;
 	packet_buffer[offset++] = HISTO_SOF;
 	packet_buffer[offset++] = TYPE_HISTO_CMP;
@@ -1488,7 +1488,16 @@ _Bool send_histogram_data_cmp(void) {
 	/* Skip over the compressed data we already wrote */
 	offset = HISTO_HEADER_SIZE + cmp_len;
 
-	/* --- Footer --- */
+	/* --- Uncompressed payload CRC (2 bytes, written before the packet footer) ---
+	 * Computed over the uncompressed payload using the same algorithm and
+	 * off-by-one convention as the packet CRC (covers bytes 0..p_off-2).
+	 * The decompressor checks this after expanding the payload to confirm
+	 * that the decompressor produced the correct output. */
+	uint16_t uncmp_crc = util_crc16(uncmp_payload, (uint32_t)p_off - 1u);
+	packet_buffer[offset++] = uncmp_crc & 0xFF;
+	packet_buffer[offset++] = (uncmp_crc >> 8) & 0xFF;
+
+	/* --- Packet footer (CRC covers header + compressed data + uncmp_crc) --- */
 	uint16_t crc = util_crc16(packet_buffer, offset - 1);
 	packet_buffer[offset++] = crc & 0xFF;
 	packet_buffer[offset++] = (crc >> 8) & 0xFF;

From 0dd31f2b4a1e559078d46aab4d27fd0439cd809c Mon Sep 17 00:00:00 2001
From: boringethan <ethan@openwater.cc>
Date: Mon, 23 Mar 2026 12:34:29 -0700
Subject: [PATCH 07/11] Remove call to Error_Handler upon SPI or USART failure

---
 Core/Src/main.c | 23 +++++++++++++++++++----
 1 file changed, 19 insertions(+), 4 deletions(-)

diff --git a/Core/Src/main.c b/Core/Src/main.c
index ceb5a7c..64424d4 100644
--- a/Core/Src/main.c
+++ b/Core/Src/main.c
@@ -1637,14 +1637,19 @@ void HAL_USART_ErrorCallback(USART_HandleTypeDef *husart)
   }
   printf("\r\n");
 
-  if ((husart->ErrorCode & HAL_USART_ERROR_ORE) != 0U && cam_id >= 0)
+  /* Attempt graceful recovery for any known camera USART peripheral.
+   * Same fix as HAL_SPI_ErrorCallback: non-overrun errors (framing, noise,
+   * DMA) previously fell through to Error_Handler() and halted the MCU.
+   * Any error on a known camera USART is recoverable via abort+restart. */
+  if (cam_id >= 0)
   {
     abort_data_reception((uint8_t)cam_id);
     start_data_reception((uint8_t)cam_id);
     return;
   }
 
-  Error_Handler();
+  /* Truly unknown USART peripheral — log and continue rather than halting. */
+  printf("[USART] Unhandled USART error on unknown peripheral, continuing.\r\n");
 }
 
 // Error handling callback for SPI
@@ -1734,14 +1739,24 @@ void HAL_SPI_ErrorCallback(SPI_HandleTypeDef *hspi)
   }
   printf("\r\n");
 
-  if ((hspi->ErrorCode & HAL_SPI_ERROR_OVR) != 0U && cam_id >= 0)
+  /* Attempt graceful recovery for any known camera SPI peripheral.
+   * Previously only overrun errors were recovered here; all other error types
+   * fell through to Error_Handler() which calls __disable_irq() + while(1),
+   * killing USB and all other peripherals.  The observed failure mode was:
+   *   Camera 6 overheats → SPI3 DMA/frame error (not OVR) →
+   *   Error_Handler() entered from interrupt context →
+   *   wait_for_usb_queues_to_finish() blocks (USB ISR can't run) →
+   *   __disable_irq() → MCU completely dark.
+   * Now any error on a known camera SPI triggers abort+restart instead. */
+  if (cam_id >= 0)
   {
     abort_data_reception((uint8_t)cam_id);
     start_data_reception((uint8_t)cam_id);
     return;
   }
 
-  Error_Handler(); // Handle any error during re-enabling
+  /* Truly unknown SPI peripheral — log and continue rather than halting. */
+  printf("[SPI] Unhandled SPI error on unknown peripheral, continuing.\r\n");
 }
 
 

From f5c9b1a493fbd583f30d72a7d613c4ac8f785320 Mon Sep 17 00:00:00 2001
From: boringethan <ethan@openwater.cc>
Date: Mon, 23 Mar 2026 14:11:23 -0700
Subject: [PATCH 08/11] Delete camera_failure_detected array and lump that
 state into the proper camera array. Mark isPresent as false if a camera dies
 and properly prevent further comms

---
 Core/Src/camera_manager.c | 37 +++++++++++++++++++++++++++++--------
 1 file changed, 29 insertions(+), 8 deletions(-)

diff --git a/Core/Src/camera_manager.c b/Core/Src/camera_manager.c
index 379ffe2..7e219c1 100644
--- a/Core/Src/camera_manager.c
+++ b/Core/Src/camera_manager.c
@@ -68,7 +68,6 @@ bool streaming_first_frame = false;
 // Camera failure detection
 #define CAMERA_FAILURE_THRESHOLD_CYCLES 3  // Number of consecutive cycles before reporting failure
 static uint8_t camera_failure_counters[CAMERA_COUNT] = {0};  // Track consecutive cycles without event bits
-static bool camera_failure_detected[CAMERA_COUNT] = {false};  // Track if failure has been detected
 
  __ALIGN_BEGIN __attribute__((section(".sram4"))) volatile uint8_t spi6_buffer[SPI_PACKET_LENGTH] __ALIGN_END;
 
@@ -1114,20 +1113,42 @@ static void check_camera_failures(void) {
 			if (has_event_bit) {
 				// Camera set its event bit, reset failure counter
 				camera_failure_counters[cam_id] = 0;
-				if(camera_failure_detected[cam_id]){
-					camera_failure_detected[cam_id] = false;
-					printf("Camera %d has recovered from failure\r\n", cam_id + 1);
-				}
 			} else {
 				// Camera didn't set its event bit, increment failure counter
 				camera_failure_counters[cam_id]++;
-				
+
 				// Check if threshold reached
 				if (camera_failure_counters[cam_id] >= CAMERA_FAILURE_THRESHOLD_CYCLES) {
-					// Only print once per failure (when threshold is exactly reached)
+					// Only act once per failure (when threshold is exactly reached)
 					if (camera_failure_counters[cam_id] == CAMERA_FAILURE_THRESHOLD_CYCLES) {
-						camera_failure_detected[cam_id] = true;
+						cam_array[cam_id].isPresent = false;
 						printf("Camera %d has stopped posting data\r\n", cam_id + 1);
+
+						/* ── Cleanly isolate the failed camera ────────────────────────
+						 * 1. Remove from event_bits_enabled so subsequent frames never
+						 *    wait for it (and the temperature poller skips it).
+						 * 2. Tell the TCA9548A I2C mux to disconnect that channel.
+						 *    This prevents poll_camera_temperatures() from ever
+						 *    selecting it and getting the I2C bus stuck if the sensor
+						 *    is holding SDA low (the root cause of COMM going dark). */
+
+						/* 1. Clear the camera's event-enable bit (atomic) */
+						__disable_irq();
+						event_bits_enabled &= ~(uint8_t)(1u << cam_id);
+						__enable_irq();
+
+						/* 2. Deselect the camera's I2C mux channel.
+						 *    Uses the bounded TCA9548A_I2C_TIMEOUT_MS timeout (50 ms)
+						 *    so this is a one-time bounded stall, not a hang. */
+						CameraDevice *pFailed = get_camera_byID(cam_id);
+						if (pFailed != NULL) {
+							HAL_StatusTypeDef mux_ret =
+								TCA9548A_DisableChannel(&hi2c1, 0x70, pFailed->i2c_target);
+							if (mux_ret != HAL_OK) {
+								printf("Camera %d: failed to disable TCA mux channel %u (ret=%d)\r\n",
+								       cam_id + 1, (unsigned)pFailed->i2c_target, (int)mux_ret);
+							}
+						}
 					}
 				}
 			}

From cc8c66fddc237f2dc52f2fc2e22f614dac6fcf6b Mon Sep 17 00:00:00 2001
From: boringethan <ethan@openwater.cc>
Date: Mon, 23 Mar 2026 14:17:56 -0700
Subject: [PATCH 09/11] stop the i2c transmissions from being on max delay

---
 Core/Src/i2c_master.c | 21 +++++++++++++++++----
 1 file changed, 17 insertions(+), 4 deletions(-)

diff --git a/Core/Src/i2c_master.c b/Core/Src/i2c_master.c
index 2f1970f..1f60ebe 100644
--- a/Core/Src/i2c_master.c
+++ b/Core/Src/i2c_master.c
@@ -28,6 +28,19 @@ static void TCA9548A_ResetHardware(void)
     I2C_current_target = 0x00;
 }
 
+/* Maximum time to wait for one I2C mux write.  HAL_MAX_DELAY must NEVER be
+ * used here because TCA9548A_WriteControl is called from interrupt context
+ * (via poll_camera_temperatures → send_data → FSIN IRQ).  A stuck I2C bus
+ * (e.g. a failed camera sensor holding SDA low after its channel is selected)
+ * would otherwise block the CPU forever, making COMM unresponsive. */
+#define TCA9548A_I2C_TIMEOUT_MS  50U
+
+/* Bounded timeout for all general slave read/write operations.  Using
+ * HAL_MAX_DELAY on any I2C call risks a permanent hang if the slave holds
+ * SDA low or the peripheral locks up.  100 ms is generous for normal I2C
+ * traffic but still guarantees the system stays responsive. */
+#define I2C_SLAVE_TIMEOUT_MS     100U
+
 static HAL_StatusTypeDef TCA9548A_WriteControl(I2C_HandleTypeDef *hi2c, uint8_t address, uint8_t data)
 {
     if (I2C_current_target == data) {
@@ -36,7 +49,7 @@ static HAL_StatusTypeDef TCA9548A_WriteControl(I2C_HandleTypeDef *hi2c, uint8_t
 
     HAL_StatusTypeDef ret = HAL_ERROR;
     for (uint8_t attempt = 0; attempt < 3; attempt++) {
-        ret = HAL_I2C_Master_Transmit(hi2c, address << 1, &data, 1, HAL_MAX_DELAY);
+        ret = HAL_I2C_Master_Transmit(hi2c, address << 1, &data, 1, TCA9548A_I2C_TIMEOUT_MS);
         if (ret == HAL_OK) {
             I2C_current_target = data;
             return HAL_OK;
@@ -97,7 +110,7 @@ uint8_t send_buffer_to_slave(I2C_HandleTypeDef * pI2c, uint8_t slave_addr, uint8
     }
 
 	// printf("===> Sending Packet %d Bytes\r\n", buf_len);
-    if(HAL_I2C_Master_Transmit(pI2c, (uint16_t)(slave_addr << 1), pBuffer, buf_len, HAL_MAX_DELAY)!= HAL_OK)
+    if(HAL_I2C_Master_Transmit(pI2c, (uint16_t)(slave_addr << 1), pBuffer, buf_len, I2C_SLAVE_TIMEOUT_MS)!= HAL_OK)
 	{
         return 1; // I2C is not in a ready state
 	}
@@ -127,7 +140,7 @@ uint8_t read_status_register_of_slave(I2C_HandleTypeDef * pI2c, uint8_t slave_ad
 
 #else
 
-    if(HAL_I2C_Mem_Read(pI2c, (uint16_t)(slave_addr << 1), 0x00, I2C_MEMADD_SIZE_8BIT, pBuffer, rx_len, HAL_MAX_DELAY)!= HAL_OK)
+    if(HAL_I2C_Mem_Read(pI2c, (uint16_t)(slave_addr << 1), 0x00, I2C_MEMADD_SIZE_8BIT, pBuffer, rx_len, I2C_SLAVE_TIMEOUT_MS)!= HAL_OK)
     {
         return 1; // I2C is not in a ready state
     }
@@ -147,7 +160,7 @@ uint8_t read_data_register_of_slave(I2C_HandleTypeDef * pI2c, uint8_t slave_addr
 
 	// printf("===> Receive Data Packet %d Bytes\r\n", rx_len);
 
-    if(HAL_I2C_Mem_Read(pI2c, (uint16_t)(slave_addr << 1), 0x01, I2C_MEMADD_SIZE_8BIT, pBuffer, rx_len, HAL_MAX_DELAY)!= HAL_OK)
+    if(HAL_I2C_Mem_Read(pI2c, (uint16_t)(slave_addr << 1), 0x01, I2C_MEMADD_SIZE_8BIT, pBuffer, rx_len, I2C_SLAVE_TIMEOUT_MS)!= HAL_OK)
     {
         return 1; // I2C is not in a ready state
     }

From 47aa92ce34b7a517562d56322d9db2945acdddf5 Mon Sep 17 00:00:00 2001
From: boringethan <ethan@openwater.cc>
Date: Mon, 23 Mar 2026 14:43:21 -0700
Subject: [PATCH 10/11] revert some timeout changes

---
 Core/Src/i2c_master.c | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/Core/Src/i2c_master.c b/Core/Src/i2c_master.c
index 1f60ebe..83711dc 100644
--- a/Core/Src/i2c_master.c
+++ b/Core/Src/i2c_master.c
@@ -110,7 +110,7 @@ uint8_t send_buffer_to_slave(I2C_HandleTypeDef * pI2c, uint8_t slave_addr, uint8
     }
 
 	// printf("===> Sending Packet %d Bytes\r\n", buf_len);
-    if(HAL_I2C_Master_Transmit(pI2c, (uint16_t)(slave_addr << 1), pBuffer, buf_len, I2C_SLAVE_TIMEOUT_MS)!= HAL_OK)
+    if(HAL_I2C_Master_Transmit(pI2c, (uint16_t)(slave_addr << 1), pBuffer, buf_len, HAL_MAX_DELAY)!= HAL_OK)
 	{
         return 1; // I2C is not in a ready state
 	}
@@ -140,7 +140,7 @@ uint8_t read_status_register_of_slave(I2C_HandleTypeDef * pI2c, uint8_t slave_ad
 
 #else
 
-    if(HAL_I2C_Mem_Read(pI2c, (uint16_t)(slave_addr << 1), 0x00, I2C_MEMADD_SIZE_8BIT, pBuffer, rx_len, I2C_SLAVE_TIMEOUT_MS)!= HAL_OK)
+    if(HAL_I2C_Mem_Read(pI2c, (uint16_t)(slave_addr << 1), 0x00, I2C_MEMADD_SIZE_8BIT, pBuffer, rx_len, HAL_MAX_DELAY)!= HAL_OK)
     {
         return 1; // I2C is not in a ready state
     }
@@ -160,7 +160,7 @@ uint8_t read_data_register_of_slave(I2C_HandleTypeDef * pI2c, uint8_t slave_addr
 
 	// printf("===> Receive Data Packet %d Bytes\r\n", rx_len);
 
-    if(HAL_I2C_Mem_Read(pI2c, (uint16_t)(slave_addr << 1), 0x01, I2C_MEMADD_SIZE_8BIT, pBuffer, rx_len, I2C_SLAVE_TIMEOUT_MS)!= HAL_OK)
+    if(HAL_I2C_Mem_Read(pI2c, (uint16_t)(slave_addr << 1), 0x01, I2C_MEMADD_SIZE_8BIT, pBuffer, rx_len, HAL_MAX_DELAY)!= HAL_OK)
     {
         return 1; // I2C is not in a ready state
     }

From c221a0f4fc0b9fc5ba79d27a0fdb5165b5b04b62 Mon Sep 17 00:00:00 2001
From: boringethan <ethan@openwater.cc>
Date: Mon, 23 Mar 2026 15:09:51 -0700
Subject: [PATCH 11/11] fix bug in disable_camera_stream that throws error
 incorrectly if a camera dies during a scan

---
 Core/Src/camera_manager.c | 11 ++++++++++-
 1 file changed, 10 insertions(+), 1 deletion(-)

diff --git a/Core/Src/camera_manager.c b/Core/Src/camera_manager.c
index 7e219c1..56cada7 100644
--- a/Core/Src/camera_manager.c
+++ b/Core/Src/camera_manager.c
@@ -1803,10 +1803,19 @@ _Bool enable_camera_stream(uint8_t cam_id){
 
 _Bool disable_camera_stream(uint8_t cam_id){
 	// printf("C%d: disable...", cam_id+1);
-	if (!camera_request_is_valid(cam_id)) {
+	if (cam_id >= CAMERA_COUNT) {
+		printf("Camera %d index out of range\r\n", cam_id + 1);
 		return false;
 	}
 
+	/* A camera that is not present is already stopped — treat disable as a
+	 * no-op success.  Returning false here would cause OW_CAMERA_STREAM to
+	 * report OW_ERROR when the host tries to disable all cameras at the end
+	 * of a scan, even though the failed camera is already fully quiesced. */
+	if (!cam_array[cam_id].isPresent) {
+		return true;
+	}
+
 	bool enabled = (event_bits_enabled & (1 << cam_id)) != 0;
 	if(!enabled){
 		printf("already done\r\n");