Merge pull request #119 from NawfalMotii79/fix/mcu-fault-ack-emergency-clear

fix(mcu): FAULT_ACK USB command clears system_emergency_state (closes #83)

9_Firmware/9_1_Microcontroller/9_1_1_C_Cpp_Libraries/USBHandler.cpp

@@ -13,6 +13,7 @@ void USBHandler::reset() {
     start_flag_received = false;
     buffer_index = 0;
     current_settings.resetToDefaults();
+    fault_ack_received = false;
 }
 
 void USBHandler::processUSBData(const uint8_t* data, uint32_t length) {
@@ -23,6 +24,18 @@ void USBHandler::processUSBData(const uint8_t* data, uint32_t length) {
     
     DIAG("USB", "processUSBData: %lu bytes, state=%d", (unsigned long)length, (int)current_state);
     
+    // FAULT_ACK: host sends exactly 4 bytes [0x40, 0x00, 0x00, 0x00].
+    // Requires exact 4-byte packet length: settings packets are always
+    // >= 82 bytes, so a lone 4-byte payload is unambiguous. Scanning
+    // inside larger packets would false-trigger on the IEEE 754
+    // encoding of 2.0 (0x4000000000000000) embedded in settings doubles.
+    static const uint8_t FAULT_ACK_SEQ[4] = {0x40, 0x00, 0x00, 0x00};
+    if (length == 4 && memcmp(data, FAULT_ACK_SEQ, 4) == 0) {
+        fault_ack_received = true;
+        DIAG("USB", "FAULT_ACK received");
+        return;
+    }
+    
     switch (current_state) {
         case USBState::WAITING_FOR_START:
             processStartFlag(data, length);

9_Firmware/9_1_Microcontroller/9_1_1_C_Cpp_Libraries/USBHandler.h

@@ -29,6 +29,11 @@ public:
     // Reset USB handler
     void reset();
 
+    // Fault-acknowledgement: host sends FAULT_ACK (0x40) to clear
+    // system_emergency_state and exit the safe-mode blink loop.
+    bool isFaultAckReceived() const { return fault_ack_received; }
+    void clearFaultAck()            { fault_ack_received = false; }
+
 private:
     RadarSettings current_settings;
     USBState current_state;
@@ -38,6 +43,7 @@ private:
     static constexpr uint32_t MAX_BUFFER_SIZE = 256;
     uint8_t usb_buffer[MAX_BUFFER_SIZE];
     uint32_t buffer_index;
+    bool fault_ack_received;
     
     void processStartFlag(const uint8_t* data, uint32_t length);
     void processSettingsData(const uint8_t* data, uint32_t length);

9_Firmware/9_1_Microcontroller/9_1_3_C_Cpp_Code/main.cpp

@@ -2054,6 +2054,10 @@ int main(void)
 	            HAL_GPIO_TogglePin(LED_3_GPIO_Port, LED_3_Pin);
 	            HAL_GPIO_TogglePin(LED_4_GPIO_Port, LED_4_Pin);
 	            HAL_Delay(250);
+	            if (usbHandler.isFaultAckReceived()) {
+	                system_emergency_state = false;
+	                usbHandler.clearFaultAck();
+	            }
 	        }
 	        DIAG("SYS", "Exited safe mode blink loop -- system_emergency_state cleared");
 	    }

9_Firmware/9_1_Microcontroller/tests/Makefile

@@ -70,7 +70,8 @@ TESTS_STANDALONE := test_bug12_pa_cal_loop_inverted \
                     test_gap3_idq_periodic_reread \
                     test_gap3_emergency_state_ordering \
                     test_gap3_overtemp_emergency_stop \
-                    test_gap3_health_watchdog_cold_start
+                    test_gap3_health_watchdog_cold_start \
+                    test_gap3_fault_ack_clears_emergency
 
 # Tests that need platform_noos_stm32.o + mocks
 TESTS_WITH_PLATFORM := test_bug11_platform_spi_transmit_only
@@ -178,6 +179,9 @@ test_gap3_overtemp_emergency_stop: test_gap3_overtemp_emergency_stop.c
 test_gap3_health_watchdog_cold_start: test_gap3_health_watchdog_cold_start.c
 	$(CC) $(CFLAGS) $< -o $@
 
+test_gap3_fault_ack_clears_emergency: test_gap3_fault_ack_clears_emergency.c
+	$(CC) $(CFLAGS) $< -o $@
+
 # Tests that need platform_noos_stm32.o + mocks
 $(TESTS_WITH_PLATFORM): %: %.c $(MOCK_OBJS) $(PLATFORM_OBJ)
 	$(CC) $(CFLAGS) $(INCLUDES) $< $(MOCK_OBJS) $(PLATFORM_OBJ) -o $@

9_Firmware/9_1_Microcontroller/tests/test_gap3_fault_ack_clears_emergency.c

@@ -0,0 +1,121 @@
+/*******************************************************************************
+ * test_gap3_fault_ack_clears_emergency.c
+ *
+ * Verifies the FAULT_ACK clear path for system_emergency_state:
+ *   - USBHandler detects exactly [0x40, 0x00, 0x00, 0x00] in a 4-byte packet
+ *   - Detection is false-positive-free: larger packets (settings data) carrying
+ *     the same bytes as a subsequence must NOT trigger the ack
+ *   - Main-loop blink logic clears system_emergency_state on receipt
+ *
+ * Logic extracted from USBHandler.cpp + main.cpp to mirror the actual code
+ * paths without requiring HAL headers.
+ ******************************************************************************/
+#include <assert.h>
+#include <stdio.h>
+#include <stdbool.h>
+#include <string.h>
+#include <stdint.h>
+
+/* ── Simulated USBHandler state ─────────────────────────────────────────── */
+static bool fault_ack_received = false;
+static volatile bool system_emergency_state = false;
+
+static const uint8_t FAULT_ACK_SEQ[4] = {0x40, 0x00, 0x00, 0x00};
+
+/* Mirrors USBHandler::processUSBData() detection logic */
+static void sim_processUSBData(const uint8_t *data, uint32_t length)
+{
+    if (data == NULL || length == 0) return;
+    if (length == 4 && memcmp(data, FAULT_ACK_SEQ, 4) == 0) {
+        fault_ack_received = true;
+        return;
+    }
+    /* (normal state machine omitted — not under test here) */
+}
+
+/* Mirrors one iteration of the blink loop in main.cpp */
+static void sim_blink_iteration(void)
+{
+    /* HAL_GPIO_TogglePin + HAL_Delay omitted */
+    if (fault_ack_received) {
+        system_emergency_state = false;
+        fault_ack_received = false;
+    }
+}
+
+int main(void)
+{
+    printf("=== Gap-3 FAULT_ACK clears system_emergency_state ===\n");
+
+    /* Test 1: exact 4-byte FAULT_ACK packet sets the flag */
+    printf("  Test 1: exact FAULT_ACK packet detected... ");
+    fault_ack_received = false;
+    const uint8_t ack_pkt[4] = {0x40, 0x00, 0x00, 0x00};
+    sim_processUSBData(ack_pkt, 4);
+    assert(fault_ack_received == true);
+    printf("PASS\n");
+
+    /* Test 2: flag cleared and system_emergency_state exits blink loop */
+    printf("  Test 2: blink loop exits on FAULT_ACK... ");
+    system_emergency_state = true;
+    fault_ack_received = true;
+    sim_blink_iteration();
+    assert(system_emergency_state == false);
+    assert(fault_ack_received == false);
+    printf("PASS\n");
+
+    /* Test 3: blink loop does NOT exit without ack */
+    printf("  Test 3: blink loop holds without ack... ");
+    system_emergency_state = true;
+    fault_ack_received = false;
+    sim_blink_iteration();
+    assert(system_emergency_state == true);
+    printf("PASS\n");
+
+    /* Test 4: settings-sized packet carrying [0x40,0x00,0x00,0x00] as first
+     * 4 bytes does NOT trigger ack (IEEE 754 double 2.0 = 0x4000000000000000) */
+    printf("  Test 4: settings packet with 2.0 double does not false-trigger... ");
+    fault_ack_received = false;
+    uint8_t settings_pkt[82];
+    memset(settings_pkt, 0, sizeof(settings_pkt));
+    /* First 4 bytes look like FAULT_ACK but packet length is 82 */
+    settings_pkt[0] = 0x40; settings_pkt[1] = 0x00;
+    settings_pkt[2] = 0x00; settings_pkt[3] = 0x00;
+    sim_processUSBData(settings_pkt, sizeof(settings_pkt));
+    assert(fault_ack_received == false);
+    printf("PASS\n");
+
+    /* Test 5: 3-byte packet (truncated) does not trigger */
+    printf("  Test 5: truncated 3-byte packet ignored... ");
+    fault_ack_received = false;
+    const uint8_t short_pkt[3] = {0x40, 0x00, 0x00};
+    sim_processUSBData(short_pkt, 3);
+    assert(fault_ack_received == false);
+    printf("PASS\n");
+
+    /* Test 6: wrong opcode byte in 4-byte packet does not trigger */
+    printf("  Test 6: wrong opcode (0x28 AGC_ENABLE) not detected as FAULT_ACK... ");
+    fault_ack_received = false;
+    const uint8_t agc_pkt[4] = {0x28, 0x00, 0x00, 0x01};
+    sim_processUSBData(agc_pkt, 4);
+    assert(fault_ack_received == false);
+    printf("PASS\n");
+
+    /* Test 7: multiple blink iterations — loop stays active until ack */
+    printf("  Test 7: loop stays active across multiple iterations until ack... ");
+    system_emergency_state = true;
+    fault_ack_received = false;
+    sim_blink_iteration();
+    assert(system_emergency_state == true);
+    sim_blink_iteration();
+    assert(system_emergency_state == true);
+    /* Now ack arrives */
+    sim_processUSBData(ack_pkt, 4);
+    assert(fault_ack_received == true);
+    sim_blink_iteration();
+    assert(system_emergency_state == false);
+    printf("PASS\n");
+
+    printf("\n=== Gap-3 FAULT_ACK: ALL 7 TESTS PASSED ===\n\n");
+    return 0;
+}

9_Firmware/9_3_GUI/radar_protocol.py

@@ -103,6 +103,15 @@ class Opcode(IntEnum):
     STATUS_REQUEST      = 0xFF
 
 
+# MCU-only commands — NOT dispatched to the FPGA opcode switch.
+# These values have no corresponding case in radar_system_top.v.
+# Listed here so the GUI can build and send them via build_command().
+# contract_parser.py filters MCU_ONLY_OPCODES out of the Python/Verilog
+# bidirectional check.
+FAULT_ACK = 0x40  # Exact 4-byte CDC packet; clears system_emergency_state
+MCU_ONLY_OPCODES: frozenset[int] = frozenset({0x40})
+
+
 # ============================================================================
 # Data Structures
 # ============================================================================

9_Firmware/9_3_GUI/test_v7.py

@@ -586,6 +586,135 @@ class TestSoftwareFPGA(unittest.TestCase):
         self.assertEqual(fpga.agc_holdoff, 0x0F)
 
 
+# ============================================================================
+# Test: live vs replay physical-unit parity — regression guard for unit drift
+#
+# Uses AST parse of workers.py (not inspect.getsource / import) so the test
+# runs in headless CI without PyQt6 — v7.workers imports PyQt6 unconditionally
+# at workers.py:24, and other worker tests here already use skipUnless(
+# _pyqt6_available()). Contract enforcement must not be gated on GUI deps.
+#
+# Asserts on AST nodes (Call / Attribute / BinOp), not source substrings, so
+# false-pass on comments or docstring wording is impossible.
+# ============================================================================
+
+
+class TestLiveReplayPhysicalUnitsParity(unittest.TestCase):
+    """Contract: live path (RadarDataWorker._run_host_dsp) and replay path
+    (ReplayWorker._emit_frame) both derive bin-to-physical conversion from
+    WaveformConfig — same source of truth, identical (range_m, velocity_ms)
+    for identical detections.
+
+    Regression context: before the fix, live path used
+    RadarSettings.velocity_resolution (default 1.0 in models.py:113) while
+    replay used WaveformConfig.velocity_resolution_mps (~5.343). Live GUI
+    therefore under-reported velocity by factor ~5.34x vs replay for
+    identical frames. See test_v7.py:449 for the WaveformConfig pin.
+    """
+
+    @staticmethod
+    def _parse_method(class_name: str, method_name: str):
+        """Return AST FunctionDef for class_name.method_name from workers.py,
+        without importing v7.workers (PyQt6-independent)."""
+        import ast
+        from pathlib import Path
+        path = Path(__file__).parent / "v7" / "workers.py"
+        tree = ast.parse(path.read_text(encoding="utf-8"))
+        for node in tree.body:
+            if isinstance(node, ast.ClassDef) and node.name == class_name:
+                for item in node.body:
+                    if isinstance(item, ast.FunctionDef) and item.name == method_name:
+                        return item
+        raise RuntimeError(f"{class_name}.{method_name} not found in workers.py")
+
+    @staticmethod
+    def _has_attribute_chain(tree, chain):
+        """True if AST tree contains a dotted attribute access matching chain.
+
+        Chain ('self', '_settings', 'range_resolution') matches
+        ``self._settings.range_resolution`` exactly.
+        """
+        import ast
+        for n in ast.walk(tree):
+            if isinstance(n, ast.Attribute):
+                parts = [n.attr]
+                cur = n.value
+                while isinstance(cur, ast.Attribute):
+                    parts.append(cur.attr)
+                    cur = cur.value
+                if isinstance(cur, ast.Name):
+                    parts.append(cur.id)
+                    parts.reverse()
+                    if tuple(parts) == tuple(chain):
+                        return True
+        return False
+
+    @staticmethod
+    def _has_call_to(tree, func_name):
+        """True if AST tree contains a call to a bare name (func_name())."""
+        import ast
+        for n in ast.walk(tree):
+            if (isinstance(n, ast.Call) and isinstance(n.func, ast.Name)
+                    and n.func.id == func_name):
+                return True
+        return False
+
+    @staticmethod
+    def _has_dbin_minus(tree, literal):
+        """True if AST tree contains ``dbin - <literal>`` binary op."""
+        import ast
+        for n in ast.walk(tree):
+            if (isinstance(n, ast.BinOp) and isinstance(n.op, ast.Sub)
+                    and isinstance(n.left, ast.Name) and n.left.id == "dbin"
+                    and isinstance(n.right, ast.Constant)
+                    and n.right.value == literal):
+                return True
+        return False
+
+    def test_live_path_uses_waveform_config(self):
+        """RadarDataWorker.__init__ must instantiate WaveformConfig() into
+        self._waveform; _run_host_dsp must read self._waveform.range_resolution_m
+        / velocity_resolution_mps — not self._settings equivalents."""
+        init = self._parse_method("RadarDataWorker", "__init__")
+        self.assertTrue(self._has_call_to(init, "WaveformConfig"),
+            "RadarDataWorker.__init__ must instantiate WaveformConfig() into self._waveform.")
+        method = self._parse_method("RadarDataWorker", "_run_host_dsp")
+        self.assertTrue(
+            self._has_attribute_chain(method, ("self", "_waveform", "range_resolution_m")),
+            "Live path must read self._waveform.range_resolution_m.")
+        self.assertTrue(
+            self._has_attribute_chain(method, ("self", "_waveform", "velocity_resolution_mps")),
+            "Live path must read self._waveform.velocity_resolution_mps. "
+            "RadarSettings.velocity_resolution default 1.0 caused ~5.34x "
+            "underreport vs replay (test_v7.py:449 pins ~5.343).")
+        self.assertFalse(self._has_attribute_chain(
+            method, ("self", "_settings", "range_resolution")),
+            "Live path still reads stale RadarSettings.range_resolution.")
+        self.assertFalse(self._has_attribute_chain(
+            method, ("self", "_settings", "velocity_resolution")),
+            "Live path still reads stale RadarSettings.velocity_resolution.")
+
+    def test_live_path_doppler_center_not_hardcoded(self):
+        """_run_host_dsp must derive doppler_center from frame shape, not
+        use hardcoded ``dbin - 16`` — mirrors processing.py:520."""
+        method = self._parse_method("RadarDataWorker", "_run_host_dsp")
+        self.assertFalse(self._has_dbin_minus(method, 16),
+            "Hardcoded doppler_center=16 breaks if frame shape changes. "
+            "Use frame.detections.shape[1] // 2 like processing.py:520.")
+
+    def test_replay_path_still_uses_waveform_config(self):
+        """Parity half: replay path (ReplayWorker._emit_frame) must keep
+        reading self._waveform.range_resolution_m / velocity_resolution_mps —
+        guards against someone breaking the replay side of the invariant."""
+        method = self._parse_method("ReplayWorker", "_emit_frame")
+        self.assertTrue(self._has_attribute_chain(
+            method, ("self", "_waveform", "range_resolution_m")),
+            "Replay path lost WaveformConfig range source of truth.")
+        self.assertTrue(self._has_attribute_chain(
+            method, ("self", "_waveform", "velocity_resolution_mps")),
+            "Replay path lost WaveformConfig velocity source of truth.")
+
+
 class TestSoftwareFPGASignalChain(unittest.TestCase):
     """SoftwareFPGA.process_chirps with real co-sim data."""
 

9_Firmware/9_3_GUI/v7/workers.py

@@ -23,7 +23,7 @@ import numpy as np
 
 from PyQt6.QtCore import QThread, QObject, QTimer, pyqtSignal
 
-from .models import RadarTarget, GPSData, RadarSettings
+from .models import RadarTarget, GPSData, RadarSettings, WaveformConfig
 from .hardware import (
     RadarAcquisition,
     RadarFrame,
@@ -84,6 +84,7 @@ class RadarDataWorker(QThread):
         self._recorder = recorder
         self._gps = gps_data_ref
         self._settings = settings or RadarSettings()
+        self._waveform = WaveformConfig()
         self._running = False
 
         # Frame queue for production RadarAcquisition → this thread
@@ -97,6 +98,9 @@ class RadarDataWorker(QThread):
         self._byte_count = 0
         self._error_count = 0
 
+    def set_waveform(self, wf: "WaveformConfig") -> None:
+        self._waveform = wf
+
     def stop(self):
         self._running = False
         if self._acquisition:
@@ -169,8 +173,8 @@ class RadarDataWorker(QThread):
         The FPGA already does: FFT, MTI, CFAR, DC notch.
         Host-side DSP adds: clustering, tracking, geo-coordinate mapping.
 
-        Bin-to-physical conversion uses RadarSettings.range_resolution
-        and velocity_resolution (should be calibrated to actual waveform).
+        Bin-to-physical conversion uses self._waveform (WaveformConfig) to keep
+        live and replay units aligned. Override via set_waveform() if needed.
         """
         targets: list[RadarTarget] = []
 
@@ -180,8 +184,11 @@ class RadarDataWorker(QThread):
 
         # Extract detections from FPGA CFAR flags
         det_indices = np.argwhere(frame.detections > 0)
-        r_res = self._settings.range_resolution
-        v_res = self._settings.velocity_resolution
+        r_res = self._waveform.range_resolution_m
+        v_res = self._waveform.velocity_resolution_mps
+        n_doppler = (frame.detections.shape[1] if frame.detections.ndim == 2
+                     else self._waveform.n_doppler_bins)
+        doppler_center = n_doppler // 2
 
         for idx in det_indices:
             rbin, dbin = idx
@@ -190,8 +197,9 @@ class RadarDataWorker(QThread):
 
             # Convert bin indices to physical units
             range_m = float(rbin) * r_res
-            # Doppler: centre bin (16) = 0 m/s; positive bins = approaching
-            velocity_ms = float(dbin - 16) * v_res
+            # Doppler: centre bin = 0 m/s; positive bins = approaching.
+            # Derived from frame shape — mirrors processing.py:520.
+            velocity_ms = float(dbin - doppler_center) * v_res
 
             # Apply pitch correction if GPS data available
             raw_elev = 0.0  # FPGA doesn't send elevation per-detection

9_Firmware/tests/cross_layer/contract_parser.py

@@ -108,12 +108,23 @@ class ConcatWidth:
 
 def parse_python_opcodes(filepath: Path | None = None) -> dict[int, OpcodeEntry]:
     """Parse the Opcode enum from radar_protocol.py.
-    Returns {opcode_value: OpcodeEntry}.
+    Returns {opcode_value: OpcodeEntry}, excluding MCU_ONLY_OPCODES.
+    MCU-only opcodes have no FPGA case statement and must not appear in
+    the bidirectional Python/Verilog contract check.
     """
     if filepath is None:
         filepath = GUI_DIR / "radar_protocol.py"
     text = filepath.read_text()
 
+    # Extract MCU_ONLY_OPCODES set so we can exclude those values below.
+    mcu_only: set[int] = set()
+    m_set = re.search(r'MCU_ONLY_OPCODES[^=]*=\s*frozenset\(\{([^}]*)\}\)', text)
+    if m_set:
+        for tok in m_set.group(1).split(','):
+            tok = tok.strip()
+            if tok.startswith(('0x', '0X')):
+                mcu_only.add(int(tok, 16))
+
     # Find the Opcode class body
     match = re.search(r'class Opcode\b.*?(?=\nclass |\Z)', text, re.DOTALL)
     if not match:
@@ -123,7 +134,8 @@ def parse_python_opcodes(filepath: Path | None = None) -> dict[int, OpcodeEntry]
     for m in re.finditer(r'(\w+)\s*=\s*(0x[0-9a-fA-F]+)', match.group()):
         name = m.group(1)
         value = int(m.group(2), 16)
-        opcodes[value] = OpcodeEntry(name=name, value=value)
+        if value not in mcu_only:
+            opcodes[value] = OpcodeEntry(name=name, value=value)
     return opcodes