Next

Core/Inc/camera_manager.h

@@ -49,8 +49,14 @@ 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
+/* 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);
@@ -77,6 +83,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);

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

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;
@@ -50,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;
@@ -59,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;
 
@@ -1105,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);
+							}
+						}
 					}
 				}
 			}
@@ -1152,8 +1182,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();
 	}
@@ -1187,9 +1220,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;
 		}
 	}
@@ -1293,6 +1347,199 @@ _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.
+ *
+ * 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) {
+		/* 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("[CMP] No cameras have data to send (ready_bits=0x%02X, enabled=0x%02X)\r\n",
+			       ready_bits, event_bits_enabled);
+		}
+		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 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,
+	                           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) {
+		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;
+	}
+
+	/* Warn if compression time is eating into the frame budget.
+	 * 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 (>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);
+	}
+
+	/* 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_CMP_UNCMP_CRC_SIZE + 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;
+
+	/* --- 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;
+	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;
+		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++;
+
+	return status;
+}
+
 _Bool send_fake_data(void) {
 
 	fill_frame_buffers();
@@ -1556,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");