From fda8aab7a26d7a108f1f3b07e77af03f3ce60e5b Mon Sep 17 00:00:00 2001
From: Jason <83615043+JJassonn69@users.noreply.github.com>
Date: Thu, 19 Mar 2026 08:57:58 +0200
Subject: [PATCH] Add DIAG instrumentation to beamformer, PA, USB, and
 remaining main.cpp subsystems

Completes the observe-before-fix instrumentation pass across all critical
firmware subsystems:

- ADAR1000_Manager.cpp: 99 DIAG calls covering power-up/down, TX/RX mode
  switching, ADTR1107 init sequence, SPI transfers, ADC reads (with 100ms
  timeout guard on unbounded busy-wait), and scratchpad verification.
- DA5578.c: 21 DIAG calls on init, reset, channel writes, clear pin
  activation, and I2C error paths.
- ADS7830.c: DIAG on init (with test-read verification) and I2C
  transmit/receive error logging in single-ended and differential reads.
- USBHandler.cpp: DIAG on state transitions, start flag detection,
  settings data accumulation, and SET/END marker parsing.
- main.cpp remaining sections: CDC_Receive_FS callback, systemPowerUp/Down
  sequences, executeChirpSequence (entry-only, timing-critical path),
  runRadarPulseSequence (beam position + stepper logging), checkSystemHealth
  (per-subsystem error logging with GPIO reads), attemptErrorRecovery,
  Emergency_Stop, handleSystemError, PA IDQ calibration loops (DAC/ADC init,
  per-channel initial readings, calibration iterations with final values),
  TMP37 ADC3 init, error handler init, and GUI status send.

No behavioral changes. All logging is compile-time removable via DIAG_DISABLE.
---
 .../ADAR1000_Manager.cpp                      | 358 ++++++++++++------
 .../9_1_1_C_Cpp_Libraries/ADS7830.c           |  14 +-
 .../9_1_1_C_Cpp_Libraries/DA5578.c            |  30 ++
 .../9_1_1_C_Cpp_Libraries/USBHandler.cpp      |  25 +-
 .../9_1_3_C_Cpp_Code/main.cpp                 | 145 ++++++-
 5 files changed, 436 insertions(+), 136 deletions(-)

diff --git a/9_Firmware/9_1_Microcontroller/9_1_1_C_Cpp_Libraries/ADAR1000_Manager.cpp b/9_Firmware/9_1_Microcontroller/9_1_1_C_Cpp_Libraries/ADAR1000_Manager.cpp
index 7c21f0e..316cb75 100644
--- a/9_Firmware/9_1_Microcontroller/9_1_1_C_Cpp_Libraries/ADAR1000_Manager.cpp
+++ b/9_Firmware/9_1_Microcontroller/9_1_1_C_Cpp_Libraries/ADAR1000_Manager.cpp
@@ -2,6 +2,7 @@
 #include "main.h"
 #include "stm32f7xx_hal.h"
 #include "ADAR1000_Manager.h"
+#include "diag_log.h"
 #include <cmath>
 #include <cstring>
 
@@ -44,51 +45,72 @@ ADAR1000Manager::~ADAR1000Manager() {
 
 // System Management
 bool ADAR1000Manager::powerUpSystem() {
+    DIAG_SECTION("BF POWER-UP SEQUENCE");
+    uint32_t t0 = HAL_GetTick();
     const uint8_t msg[] = "Starting System Power-Up Sequence...\r\n";
     HAL_UART_Transmit(&huart3, msg, sizeof(msg) - 1, 1000);
 
     // Power-up sequence steps...
+    DIAG("BF", "Enabling VDD_SW (3.3V)");
     HAL_GPIO_WritePin(EN_P_3V3_VDD_SW_GPIO_Port, EN_P_3V3_VDD_SW_Pin, GPIO_PIN_SET);
     HAL_Delay(2);
 
+    DIAG("BF", "Enabling VSS_SW (3.3V)");
     HAL_GPIO_WritePin(EN_P_3V3_SW_GPIO_Port, EN_P_3V3_SW_Pin, GPIO_PIN_SET);
     HAL_Delay(2);
 
     // Initialize devices
+    DIAG("BF", "Calling initializeAllDevices()");
     if (!initializeAllDevices()) {
+        DIAG_ERR("BF", "initializeAllDevices() FAILED");
         const uint8_t err[] = "ERROR: ADAR1000 initialization failed!\r\n";
         HAL_UART_Transmit(&huart3, err, sizeof(err) - 1, 1000);
         return false;
     }
+    DIAG("BF", "initializeAllDevices() OK");
 
     // Start in RX mode
+    DIAG("BF", "Setting initial mode to RX");
     switchToRXMode();
 
+    DIAG_ELAPSED("BF", "powerUpSystem() total", t0);
     const uint8_t success[] = "System Power-Up Sequence Completed Successfully.\r\n";
     HAL_UART_Transmit(&huart3, success, sizeof(success) - 1, 1000);
     return true;
 }
 
 bool ADAR1000Manager::powerDownSystem() {
+    DIAG_SECTION("BF POWER-DOWN SEQUENCE");
+    DIAG("BF", "Switching to RX mode before power-down");
     switchToRXMode();
     HAL_Delay(10);
 
+    DIAG("BF", "Disabling PA supplies");
     disablePASupplies();
+    DIAG("BF", "Disabling LNA supplies");
     disableLNASupplies();
+    DIAG("BF", "Disabling VSS_SW rail");
     HAL_GPIO_WritePin(EN_P_3V3_SW_GPIO_Port, EN_P_3V3_SW_Pin, GPIO_PIN_RESET);
+    DIAG("BF", "Disabling VDD_SW rail");
     HAL_GPIO_WritePin(EN_P_3V3_VDD_SW_GPIO_Port, EN_P_3V3_VDD_SW_Pin, GPIO_PIN_RESET);
 
+    DIAG("BF", "powerDownSystem() complete");
     return true;
 }
 
 // Mode Switching
 void ADAR1000Manager::switchToTXMode() {
+    DIAG_SECTION("BF SWITCH TO TX MODE");
+    DIAG("BF", "Step 1: LNA bias OFF");
     setLNABias(false);
     delayUs(10);
+    DIAG("BF", "Step 2: Enable PA supplies");
     enablePASupplies();
     delayUs(100);
+    DIAG("BF", "Step 3: PA bias ON");
     setPABias(true);
     delayUs(50);
+    DIAG("BF", "Step 4: ADTR1107 -> TX");
     setADTR1107Control(true);
 
     for (uint8_t dev = 0; dev < devices_.size(); ++dev) {
@@ -96,18 +118,26 @@ void ADAR1000Manager::switchToTXMode() {
         adarWrite(dev, REG_TX_ENABLES, 0x0F, BROADCAST_OFF);
         adarSetTxBias(dev, BROADCAST_OFF);
         devices_[dev]->current_mode = BeamDirection::TX;
+        DIAG("BF", "  dev[%u] TX enables=0x0F, TX bias set", dev);
     }
     current_mode_ = BeamDirection::TX;
+    DIAG("BF", "switchToTXMode() complete");
 }
 
 void ADAR1000Manager::switchToRXMode() {
+    DIAG_SECTION("BF SWITCH TO RX MODE");
+    DIAG("BF", "Step 1: PA bias OFF");
     setPABias(false);
     delayUs(50);
+    DIAG("BF", "Step 2: Disable PA supplies");
     disablePASupplies();
     delayUs(10);
+    DIAG("BF", "Step 3: ADTR1107 -> RX");
     setADTR1107Control(false);
+    DIAG("BF", "Step 4: Enable LNA supplies");
     enableLNASupplies();
     delayUs(50);
+    DIAG("BF", "Step 5: LNA bias ON");
     setLNABias(true);
     delayUs(50);
 
@@ -115,11 +145,14 @@ void ADAR1000Manager::switchToRXMode() {
         adarWrite(dev, REG_TX_ENABLES, 0x00, BROADCAST_OFF);
         adarWrite(dev, REG_RX_ENABLES, 0x0F, BROADCAST_OFF);
         devices_[dev]->current_mode = BeamDirection::RX;
+        DIAG("BF", "  dev[%u] RX enables=0x0F", dev);
     }
     current_mode_ = BeamDirection::RX;
+    DIAG("BF", "switchToRXMode() complete");
 }
 
 void ADAR1000Manager::fastTXMode() {
+    DIAG("BF", "fastTXMode(): ADTR1107 -> TX (no bias sequencing)");
     setADTR1107Control(true);
     for (uint8_t dev = 0; dev < devices_.size(); ++dev) {
         adarWrite(dev, REG_RX_ENABLES, 0x00, BROADCAST_OFF);
@@ -130,6 +163,7 @@ void ADAR1000Manager::fastTXMode() {
 }
 
 void ADAR1000Manager::fastRXMode() {
+    DIAG("BF", "fastRXMode(): ADTR1107 -> RX (no bias sequencing)");
     setADTR1107Control(false);
     for (uint8_t dev = 0; dev < devices_.size(); ++dev) {
         adarWrite(dev, REG_TX_ENABLES, 0x00, BROADCAST_OFF);
@@ -140,19 +174,25 @@ void ADAR1000Manager::fastRXMode() {
 }
 
 void ADAR1000Manager::pulseTXMode() {
+    DIAG("BF", "pulseTXMode(): TR switch only");
     setADTR1107Control(true);
     last_switch_time_us_ = HAL_GetTick() * 1000;
 }
 
 void ADAR1000Manager::pulseRXMode() {
+    DIAG("BF", "pulseRXMode(): TR switch only");
     setADTR1107Control(false);
     last_switch_time_us_ = HAL_GetTick() * 1000;
 }
 
 // Beam Steering
 bool ADAR1000Manager::setBeamAngle(float angle_degrees, BeamDirection direction) {
+    DIAG("BF", "setBeamAngle(%.1f deg, %s)", (double)angle_degrees,
+         direction == BeamDirection::TX ? "TX" : "RX");
     uint8_t phase_settings[4];
     calculatePhaseSettings(angle_degrees, phase_settings);
+    DIAG("BF", "  phase[0..3] = %u, %u, %u, %u",
+         phase_settings[0], phase_settings[1], phase_settings[2], phase_settings[3]);
 
     if (direction == BeamDirection::TX) {
         setAllDevicesTXMode();
@@ -237,21 +277,33 @@ void ADAR1000Manager::clearBeamSequence(BeamDirection direction) {
 // Monitoring and Diagnostics
 float ADAR1000Manager::readTemperature(uint8_t deviceIndex) {
     if (deviceIndex >= devices_.size() || !devices_[deviceIndex]->initialized) {
+        DIAG_WARN("BF", "readTemperature(dev[%u]) skipped: not initialized", deviceIndex);
         return -273.15f;
     }
 
     uint8_t temp_raw = adarAdcRead(deviceIndex, BROADCAST_OFF);
-    return (temp_raw * 0.5f) - 50.0f;
+    float temp_c = (temp_raw * 0.5f) - 50.0f;
+    DIAG("BF", "readTemperature(dev[%u]): raw=0x%02X => %.1f C", deviceIndex, temp_raw, (double)temp_c);
+    return temp_c;
 }
 
 bool ADAR1000Manager::verifyDeviceCommunication(uint8_t deviceIndex) {
-    if (deviceIndex >= devices_.size()) return false;
+    if (deviceIndex >= devices_.size()) {
+        DIAG_ERR("BF", "verifyDeviceComm(dev[%u]): index out of range", deviceIndex);
+        return false;
+    }
 
     uint8_t test_value = 0xA5;
     adarWrite(deviceIndex, REG_SCRATCHPAD, test_value, BROADCAST_OFF);
     HAL_Delay(1);
     uint8_t readback = adarRead(deviceIndex, REG_SCRATCHPAD);
-    return (readback == test_value);
+    bool pass = (readback == test_value);
+    if (pass) {
+        DIAG("BF", "verifyDeviceComm(dev[%u]): scratchpad 0xA5 -> 0x%02X OK", deviceIndex, readback);
+    } else {
+        DIAG_ERR("BF", "verifyDeviceComm(dev[%u]): scratchpad 0xA5 -> 0x%02X MISMATCH", deviceIndex, readback);
+    }
+    return pass;
 }
 
 uint8_t ADAR1000Manager::readRegister(uint8_t deviceIndex, uint32_t address) {
@@ -268,15 +320,18 @@ void ADAR1000Manager::setSwitchSettlingTime(uint32_t us) {
 }
 
 void ADAR1000Manager::setFastSwitchMode(bool enable) {
+    DIAG("BF", "setFastSwitchMode(%s)", enable ? "ON" : "OFF");
     fast_switch_mode_ = enable;
     if (enable) {
         switch_settling_time_us_ = 10;
+        DIAG("BF", "  settling time = 10 us, enabling PA+LNA supplies and bias simultaneously");
         enablePASupplies();
         enableLNASupplies();
         setPABias(true);
         setLNABias(true);
     } else {
         switch_settling_time_us_ = 50;
+        DIAG("BF", "  settling time = 50 us");
     }
 }
 
@@ -291,15 +346,19 @@ void ADAR1000Manager::setBeamDwellTime(uint32_t ms) {
 // ============================================================================
 
 bool ADAR1000Manager::initializeAllDevices() {
-
+    DIAG_SECTION("BF INIT ALL DEVICES");
 
     // Initialize each ADAR1000
     for (uint8_t i = 0; i < devices_.size(); ++i) {
+        DIAG("BF", "Initializing ADAR1000 dev[%u]...", i);
         if (!initializeSingleDevice(i)) {
+            DIAG_ERR("BF", "initializeSingleDevice(%u) FAILED -- aborting init", i);
             return false;
         }
+        DIAG("BF", "  dev[%u] init OK", i);
     }
 
+    DIAG("BF", "All 4 ADAR1000 devices initialized, setting TX mode");
     setAllDevicesTXMode();
     return true;
 }
@@ -307,89 +366,113 @@ bool ADAR1000Manager::initializeAllDevices() {
 bool ADAR1000Manager::initializeSingleDevice(uint8_t deviceIndex) {
     if (deviceIndex >= devices_.size()) return false;
 
+    DIAG("BF", "  dev[%u] soft reset", deviceIndex);
     adarSoftReset(deviceIndex);
     HAL_Delay(10);
 
+    DIAG("BF", "  dev[%u] write ConfigA (SDO_ACTIVE)", deviceIndex);
     adarWriteConfigA(deviceIndex, INTERFACE_CONFIG_A_SDO_ACTIVE, BROADCAST_OFF);
+    DIAG("BF", "  dev[%u] set RAM bypass (bias+beam)", deviceIndex);
     adarSetRamBypass(deviceIndex, BROADCAST_OFF);
 
     // Initialize ADC
+    DIAG("BF", "  dev[%u] enable ADC (2MHz clk)", deviceIndex);
     adarWrite(deviceIndex, REG_ADC_CONTROL, ADAR1000_ADC_2MHZ_CLK | ADAR1000_ADC_EN, BROADCAST_OFF);
 
+    // Verify communication with scratchpad test
+    DIAG("BF", "  dev[%u] verifying SPI communication...", deviceIndex);
+    bool comms_ok = verifyDeviceCommunication(deviceIndex);
+    if (!comms_ok) {
+        DIAG_WARN("BF", "  dev[%u] scratchpad verify FAILED but marking initialized anyway", deviceIndex);
+    }
+
     devices_[deviceIndex]->initialized = true;
     return true;
 }
 
 bool ADAR1000Manager::initializeADTR1107Sequence() {
+    DIAG_SECTION("ADTR1107 POWER SEQUENCE (9-step)");
+    uint32_t t0 = HAL_GetTick();
 
 	//Powering up ADTR1107 TX mode
     const uint8_t msg[] = "Starting ADTR1107 Power Sequence...\r\n";
     HAL_UART_Transmit(&huart3, msg, sizeof(msg) - 1, 1000);
 
-    // Step 1: Connect all GND pins to ground (assumed in hardware)
-
-    // Step 2: Set VDD_SW to 3.3V
-    HAL_GPIO_WritePin(EN_P_3V3_VDD_SW_GPIO_Port, EN_P_3V3_VDD_SW_Pin, GPIO_PIN_SET);
-    HAL_Delay(1);
-
-    // Step 3: Set VSS_SW to -3.3V
-    HAL_GPIO_WritePin(EN_P_3V3_SW_GPIO_Port, EN_P_3V3_SW_Pin, GPIO_PIN_SET);
-    HAL_Delay(1);
+    // Step 1: Connect all GND pins to ground (assumed in hardware)
+    DIAG("BF", "Step 1: GND pins (hardware -- assumed connected)");
+
+    // Step 2: Set VDD_SW to 3.3V
+    DIAG("BF", "Step 2: VDD_SW -> 3.3V");
+    HAL_GPIO_WritePin(EN_P_3V3_VDD_SW_GPIO_Port, EN_P_3V3_VDD_SW_Pin, GPIO_PIN_SET);
+    HAL_Delay(1);
+
+    // Step 3: Set VSS_SW to -3.3V
+    DIAG("BF", "Step 3: VSS_SW -> -3.3V");
+    HAL_GPIO_WritePin(EN_P_3V3_SW_GPIO_Port, EN_P_3V3_SW_Pin, GPIO_PIN_SET);
+    HAL_Delay(1);
 
     // Step 4: Set CTRL_SW to RX mode initially via GPIO
+    DIAG("BF", "Step 4: CTRL_SW -> RX (initial safe mode)");
     setADTR1107Control(false); // RX mode
     HAL_Delay(1);
 
     // Step 5: Set VGG_LNA to 0
-    uint8_t lna_bias_voltage = kLnaBiasOff;
-    for (uint8_t dev = 0; dev < devices_.size(); ++dev) {
-        adarWrite(dev, REG_LNA_BIAS_ON, lna_bias_voltage, BROADCAST_OFF);
-        adarWrite(dev, REG_LNA_BIAS_OFF, kLnaBiasOff, BROADCAST_OFF);
-    }
-
-    // Step 6: Set VDD_LNA to 0V for TX mode
-    HAL_GPIO_WritePin(EN_P_3V3_ADTR_GPIO_Port, EN_P_3V3_ADTR_Pin, GPIO_PIN_RESET);
-    HAL_Delay(2);
+    DIAG("BF", "Step 5: VGG_LNA bias -> OFF (0x%02X)", kLnaBiasOff);
+    uint8_t lna_bias_voltage = kLnaBiasOff;
+    for (uint8_t dev = 0; dev < devices_.size(); ++dev) {
+        adarWrite(dev, REG_LNA_BIAS_ON, lna_bias_voltage, BROADCAST_OFF);
+        adarWrite(dev, REG_LNA_BIAS_OFF, kLnaBiasOff, BROADCAST_OFF);
+    }
+
+    // Step 6: Set VDD_LNA to 0V for TX mode
+    DIAG("BF", "Step 6: VDD_LNA -> 0V (disable ADTR LNA supply)");
+    HAL_GPIO_WritePin(EN_P_3V3_ADTR_GPIO_Port, EN_P_3V3_ADTR_Pin, GPIO_PIN_RESET);
+    HAL_Delay(2);
 
     // Step 7: Set VGG_PA to safe negative voltage (PA off for TX mode)
     /*A 0x00 value in the
     on or off bias registers, correspond to a 0 V output. A 0xFF in the
     on or off bias registers correspond to a −4.8 V output.*/
-    uint8_t safe_pa_bias = kPaBiasTxSafe; // Safe negative voltage (-1.75V) to keep PA off
-    for (uint8_t dev = 0; dev < devices_.size(); ++dev) {
-        adarWrite(dev, REG_PA_CH1_BIAS_ON, safe_pa_bias, BROADCAST_OFF);
-        adarWrite(dev, REG_PA_CH2_BIAS_ON, safe_pa_bias, BROADCAST_OFF);
+    DIAG("BF", "Step 7: VGG_PA -> safe bias 0x%02X (~ -1.75V, PA off)", kPaBiasTxSafe);
+    uint8_t safe_pa_bias = kPaBiasTxSafe; // Safe negative voltage (-1.75V) to keep PA off
+    for (uint8_t dev = 0; dev < devices_.size(); ++dev) {
+        adarWrite(dev, REG_PA_CH1_BIAS_ON, safe_pa_bias, BROADCAST_OFF);
+        adarWrite(dev, REG_PA_CH2_BIAS_ON, safe_pa_bias, BROADCAST_OFF);
         adarWrite(dev, REG_PA_CH3_BIAS_ON, safe_pa_bias, BROADCAST_OFF);
         adarWrite(dev, REG_PA_CH4_BIAS_ON, safe_pa_bias, BROADCAST_OFF);
     }
-    HAL_Delay(10);
-
-    // Step 8: Set VDD_PA to 0V (PA powered up for TX mode)
-    enablePASupplies();
-    HAL_Delay(50);
+    HAL_Delay(10);
+
+    // Step 8: Set VDD_PA to 0V (PA powered up for TX mode)
+    DIAG("BF", "Step 8: Enable PA supplies (VDD_PA)");
+    enablePASupplies();
+    HAL_Delay(50);
 
     // Step 9: Adjust VGG_PA voltage between −1.75 V and −0.25 V to achieve the desired IDQ_PA=220mA
     //Set VGG_PA to safe negative voltage (PA off for TX mode)
     /*A 0x00 value in the
     on or off bias registers, correspond to a 0 V output. A 0xFF in the
     on or off bias registers correspond to a −4.8 V output.*/
-    uint8_t Idq_pa_bias = kPaBiasIdqCalibration; // Safe negative voltage (-0.2447V) to keep PA off
-    for (uint8_t dev = 0; dev < devices_.size(); ++dev) {
-        adarWrite(dev, REG_PA_CH1_BIAS_ON, Idq_pa_bias, BROADCAST_OFF);
-        adarWrite(dev, REG_PA_CH2_BIAS_ON, Idq_pa_bias, BROADCAST_OFF);
+    DIAG("BF", "Step 9: VGG_PA -> Idq cal bias 0x%02X (~ -0.24V, target 220mA)", kPaBiasIdqCalibration);
+    uint8_t Idq_pa_bias = kPaBiasIdqCalibration; // Safe negative voltage (-0.2447V) to keep PA off
+    for (uint8_t dev = 0; dev < devices_.size(); ++dev) {
+        adarWrite(dev, REG_PA_CH1_BIAS_ON, Idq_pa_bias, BROADCAST_OFF);
+        adarWrite(dev, REG_PA_CH2_BIAS_ON, Idq_pa_bias, BROADCAST_OFF);
         adarWrite(dev, REG_PA_CH3_BIAS_ON, Idq_pa_bias, BROADCAST_OFF);
         adarWrite(dev, REG_PA_CH4_BIAS_ON, Idq_pa_bias, BROADCAST_OFF);
     }
     HAL_Delay(10);
 
+    DIAG_ELAPSED("BF", "ADTR1107 power sequence", t0);
 
     const uint8_t success[] = "ADTR1107 power sequence completed.\r\n";
     HAL_UART_Transmit(&huart3, success, sizeof(success) - 1, 1000);
 
     return true;
-}
+}
 
 bool ADAR1000Manager::setAllDevicesTXMode() {
+    DIAG("BF", "setAllDevicesTXMode(): ADTR1107 -> TX, then configure ADAR1000s");
     // Set ADTR1107 to TX mode first
     setADTR1107Mode(BeamDirection::TX);
 
@@ -403,12 +486,14 @@ bool ADAR1000Manager::setAllDevicesTXMode() {
         adarSetTxBias(dev, BROADCAST_OFF);
 
         devices_[dev]->current_mode = BeamDirection::TX;
+        DIAG("BF", "  dev[%u] TX mode set (enables=0x0F, bias applied)", dev);
     }
     current_mode_ = BeamDirection::TX;
     return true;
 }
 
 bool ADAR1000Manager::setAllDevicesRXMode() {
+    DIAG("BF", "setAllDevicesRXMode(): ADTR1107 -> RX, then configure ADAR1000s");
     // Set ADTR1107 to RX mode first
     setADTR1107Mode(BeamDirection::RX);
 
@@ -421,83 +506,100 @@ bool ADAR1000Manager::setAllDevicesRXMode() {
         adarWrite(dev, REG_RX_ENABLES, 0x0F, BROADCAST_OFF); // Enable all 4 channels
 
         devices_[dev]->current_mode = BeamDirection::RX;
+        DIAG("BF", "  dev[%u] RX mode set (enables=0x0F)", dev);
     }
     current_mode_ = BeamDirection::RX;
     return true;
 }
 
 void ADAR1000Manager::setADTR1107Mode(BeamDirection direction) {
-    if (direction == BeamDirection::TX) {
-        setADTR1107Control(true); // TX mode
-
-        // Step 1: Disable LNA power first
-        disableLNASupplies();
-        HAL_Delay(5);
-
-        // Step 2: Set LNA bias to safe off value
-        for (uint8_t dev = 0; dev < devices_.size(); ++dev) {
-            adarWrite(dev, REG_LNA_BIAS_ON, kLnaBiasOff, BROADCAST_OFF); // Turn off LNA bias
-        }
-        HAL_Delay(5);
-
-        // Step 3: Enable PA power
-        enablePASupplies();
-        HAL_Delay(10);
-
-        // Step 4: Set PA bias to operational value
-        uint8_t operational_pa_bias = kPaBiasOperational; // Maximum bias for full power
-        for (uint8_t dev = 0; dev < devices_.size(); ++dev) {
-            adarWrite(dev, REG_PA_CH1_BIAS_ON, operational_pa_bias, BROADCAST_OFF);
-            adarWrite(dev, REG_PA_CH2_BIAS_ON, operational_pa_bias, BROADCAST_OFF);
+    if (direction == BeamDirection::TX) {
+        DIAG_SECTION("ADTR1107 -> TX MODE");
+        setADTR1107Control(true); // TX mode
+
+        // Step 1: Disable LNA power first
+        DIAG("BF", "  Disable LNA supplies");
+        disableLNASupplies();
+        HAL_Delay(5);
+
+        // Step 2: Set LNA bias to safe off value
+        DIAG("BF", "  LNA bias -> OFF (0x%02X)", kLnaBiasOff);
+        for (uint8_t dev = 0; dev < devices_.size(); ++dev) {
+            adarWrite(dev, REG_LNA_BIAS_ON, kLnaBiasOff, BROADCAST_OFF); // Turn off LNA bias
+        }
+        HAL_Delay(5);
+
+        // Step 3: Enable PA power
+        DIAG("BF", "  Enable PA supplies");
+        enablePASupplies();
+        HAL_Delay(10);
+
+        // Step 4: Set PA bias to operational value
+        DIAG("BF", "  PA bias -> operational (0x%02X)", kPaBiasOperational);
+        uint8_t operational_pa_bias = kPaBiasOperational; // Maximum bias for full power
+        for (uint8_t dev = 0; dev < devices_.size(); ++dev) {
+            adarWrite(dev, REG_PA_CH1_BIAS_ON, operational_pa_bias, BROADCAST_OFF);
+            adarWrite(dev, REG_PA_CH2_BIAS_ON, operational_pa_bias, BROADCAST_OFF);
             adarWrite(dev, REG_PA_CH3_BIAS_ON, operational_pa_bias, BROADCAST_OFF);
             adarWrite(dev, REG_PA_CH4_BIAS_ON, operational_pa_bias, BROADCAST_OFF);
         }
         HAL_Delay(5);
 
         // Step 5: Set TR switch to TX mode
+        DIAG("BF", "  TR switch -> TX (TR_SOURCE=1, BIAS_EN)");
         for (uint8_t dev = 0; dev < devices_.size(); ++dev) {
             adarSetBit(dev, REG_SW_CONTROL, 2, BROADCAST_OFF); // TR_SOURCE = 1 (TX)
             adarSetBit(dev, REG_MISC_ENABLES, 5, BROADCAST_OFF); // BIAS_EN
         }
+        DIAG("BF", "  ADTR1107 TX mode complete");
 
     } else {
         // RECEIVE MODE: Enable LNA, Disable PA
-        setADTR1107Control(false); // RX mode
-
-        // Step 1: Disable PA power first
-        disablePASupplies();
-        HAL_Delay(5);
-
-        // Step 2: Set PA bias to safe negative voltage
-        uint8_t safe_pa_bias = kPaBiasRxSafe;
-        for (uint8_t dev = 0; dev < devices_.size(); ++dev) {
-            adarWrite(dev, REG_PA_CH1_BIAS_ON, safe_pa_bias, BROADCAST_OFF);
-            adarWrite(dev, REG_PA_CH2_BIAS_ON, safe_pa_bias, BROADCAST_OFF);
+        DIAG_SECTION("ADTR1107 -> RX MODE");
+        setADTR1107Control(false); // RX mode
+
+        // Step 1: Disable PA power first
+        DIAG("BF", "  Disable PA supplies");
+        disablePASupplies();
+        HAL_Delay(5);
+
+        // Step 2: Set PA bias to safe negative voltage
+        DIAG("BF", "  PA bias -> safe (0x%02X)", kPaBiasRxSafe);
+        uint8_t safe_pa_bias = kPaBiasRxSafe;
+        for (uint8_t dev = 0; dev < devices_.size(); ++dev) {
+            adarWrite(dev, REG_PA_CH1_BIAS_ON, safe_pa_bias, BROADCAST_OFF);
+            adarWrite(dev, REG_PA_CH2_BIAS_ON, safe_pa_bias, BROADCAST_OFF);
             adarWrite(dev, REG_PA_CH3_BIAS_ON, safe_pa_bias, BROADCAST_OFF);
             adarWrite(dev, REG_PA_CH4_BIAS_ON, safe_pa_bias, BROADCAST_OFF);
         }
-        HAL_Delay(5);
-
-        // Step 3: Enable LNA power
-        enableLNASupplies();
-        HAL_Delay(10);
-
-        // Step 4: Set LNA bias to operational value
-        uint8_t operational_lna_bias = kLnaBiasOperational;
-        for (uint8_t dev = 0; dev < devices_.size(); ++dev) {
-            adarWrite(dev, REG_LNA_BIAS_ON, operational_lna_bias, BROADCAST_OFF);
-        }
+        HAL_Delay(5);
+
+        // Step 3: Enable LNA power
+        DIAG("BF", "  Enable LNA supplies");
+        enableLNASupplies();
+        HAL_Delay(10);
+
+        // Step 4: Set LNA bias to operational value
+        DIAG("BF", "  LNA bias -> operational (0x%02X)", kLnaBiasOperational);
+        uint8_t operational_lna_bias = kLnaBiasOperational;
+        for (uint8_t dev = 0; dev < devices_.size(); ++dev) {
+            adarWrite(dev, REG_LNA_BIAS_ON, operational_lna_bias, BROADCAST_OFF);
+        }
         HAL_Delay(5);
 
         // Step 5: Set TR switch to RX mode
+        DIAG("BF", "  TR switch -> RX (TR_SOURCE=0, LNA_BIAS_OUT_EN)");
         for (uint8_t dev = 0; dev < devices_.size(); ++dev) {
             adarResetBit(dev, REG_SW_CONTROL, 2, BROADCAST_OFF); // TR_SOURCE = 0 (RX)
             adarSetBit(dev, REG_MISC_ENABLES, 4, BROADCAST_OFF); // LNA_BIAS_OUT_EN
         }
+        DIAG("BF", "  ADTR1107 RX mode complete");
     }
-}
+}
 
 void ADAR1000Manager::setADTR1107Control(bool tx_mode) {
+    DIAG("BF", "setADTR1107Control(%s): setting TR switch on all %u devices, settling %lu us",
+         tx_mode ? "TX" : "RX", (unsigned)devices_.size(), (unsigned long)switch_settling_time_us_);
     for (uint8_t dev = 0; dev < devices_.size(); ++dev) {
         setTRSwitchPosition(dev, tx_mode);
     }
@@ -529,45 +631,51 @@ bool ADAR1000Manager::setCustomBeamPattern16(const uint8_t phase_pattern[16], Be
     return true;
 }
 
-void ADAR1000Manager::enablePASupplies() {
-    HAL_GPIO_WritePin(EN_P_5V0_PA1_GPIO_Port, EN_P_5V0_PA1_Pin, GPIO_PIN_SET);
-    HAL_GPIO_WritePin(EN_P_5V0_PA2_GPIO_Port, EN_P_5V0_PA2_Pin, GPIO_PIN_SET);
-    HAL_GPIO_WritePin(EN_P_5V0_PA3_GPIO_Port, EN_P_5V0_PA3_Pin, GPIO_PIN_SET);
-}
-
-void ADAR1000Manager::disablePASupplies() {
-    HAL_GPIO_WritePin(EN_P_5V0_PA1_GPIO_Port, EN_P_5V0_PA1_Pin, GPIO_PIN_RESET);
-    HAL_GPIO_WritePin(EN_P_5V0_PA2_GPIO_Port, EN_P_5V0_PA2_Pin, GPIO_PIN_RESET);
-    HAL_GPIO_WritePin(EN_P_5V0_PA3_GPIO_Port, EN_P_5V0_PA3_Pin, GPIO_PIN_RESET);
-}
-
-void ADAR1000Manager::enableLNASupplies() {
-    HAL_GPIO_WritePin(EN_P_3V3_ADTR_GPIO_Port, EN_P_3V3_ADTR_Pin, GPIO_PIN_SET);
-}
-
-void ADAR1000Manager::disableLNASupplies() {
-    HAL_GPIO_WritePin(EN_P_3V3_ADTR_GPIO_Port, EN_P_3V3_ADTR_Pin, GPIO_PIN_RESET);
-}
-
-void ADAR1000Manager::setPABias(bool enable) {
-    uint8_t pa_bias = enable ? kPaBiasOperational : kPaBiasRxSafe; // Operational vs safe bias
-
-    for (uint8_t dev = 0; dev < devices_.size(); ++dev) {
-        adarWrite(dev, REG_PA_CH1_BIAS_ON, pa_bias, BROADCAST_OFF);
+void ADAR1000Manager::enablePASupplies() {
+    DIAG("BF", "enablePASupplies(): PA1+PA2+PA3 -> ON");
+    HAL_GPIO_WritePin(EN_P_5V0_PA1_GPIO_Port, EN_P_5V0_PA1_Pin, GPIO_PIN_SET);
+    HAL_GPIO_WritePin(EN_P_5V0_PA2_GPIO_Port, EN_P_5V0_PA2_Pin, GPIO_PIN_SET);
+    HAL_GPIO_WritePin(EN_P_5V0_PA3_GPIO_Port, EN_P_5V0_PA3_Pin, GPIO_PIN_SET);
+}
+
+void ADAR1000Manager::disablePASupplies() {
+    DIAG("BF", "disablePASupplies(): PA1+PA2+PA3 -> OFF");
+    HAL_GPIO_WritePin(EN_P_5V0_PA1_GPIO_Port, EN_P_5V0_PA1_Pin, GPIO_PIN_RESET);
+    HAL_GPIO_WritePin(EN_P_5V0_PA2_GPIO_Port, EN_P_5V0_PA2_Pin, GPIO_PIN_RESET);
+    HAL_GPIO_WritePin(EN_P_5V0_PA3_GPIO_Port, EN_P_5V0_PA3_Pin, GPIO_PIN_RESET);
+}
+
+void ADAR1000Manager::enableLNASupplies() {
+    DIAG("BF", "enableLNASupplies(): ADTR 3.3V -> ON");
+    HAL_GPIO_WritePin(EN_P_3V3_ADTR_GPIO_Port, EN_P_3V3_ADTR_Pin, GPIO_PIN_SET);
+}
+
+void ADAR1000Manager::disableLNASupplies() {
+    DIAG("BF", "disableLNASupplies(): ADTR 3.3V -> OFF");
+    HAL_GPIO_WritePin(EN_P_3V3_ADTR_GPIO_Port, EN_P_3V3_ADTR_Pin, GPIO_PIN_RESET);
+}
+
+void ADAR1000Manager::setPABias(bool enable) {
+    uint8_t pa_bias = enable ? kPaBiasOperational : kPaBiasRxSafe; // Operational vs safe bias
+    DIAG("BF", "setPABias(%s): bias=0x%02X", enable ? "ON" : "OFF", pa_bias);
+
+    for (uint8_t dev = 0; dev < devices_.size(); ++dev) {
+        adarWrite(dev, REG_PA_CH1_BIAS_ON, pa_bias, BROADCAST_OFF);
         adarWrite(dev, REG_PA_CH2_BIAS_ON, pa_bias, BROADCAST_OFF);
         adarWrite(dev, REG_PA_CH3_BIAS_ON, pa_bias, BROADCAST_OFF);
         adarWrite(dev, REG_PA_CH4_BIAS_ON, pa_bias, BROADCAST_OFF);
     }
-}
-
-void ADAR1000Manager::setLNABias(bool enable) {
-    uint8_t lna_bias = enable ? kLnaBiasOperational : kLnaBiasOff; // Operational vs off
-
-    for (uint8_t dev = 0; dev < devices_.size(); ++dev) {
-        adarWrite(dev, REG_LNA_BIAS_ON, lna_bias, BROADCAST_OFF);
-    }
 }
 
+void ADAR1000Manager::setLNABias(bool enable) {
+    uint8_t lna_bias = enable ? kLnaBiasOperational : kLnaBiasOff; // Operational vs off
+    DIAG("BF", "setLNABias(%s): bias=0x%02X", enable ? "ON" : "OFF", lna_bias);
+
+    for (uint8_t dev = 0; dev < devices_.size(); ++dev) {
+        adarWrite(dev, REG_LNA_BIAS_ON, lna_bias, BROADCAST_OFF);
+    }
+}
+
 void ADAR1000Manager::delayUs(uint32_t microseconds) {
     // Simple implementation - for F7 @ 216MHz, each loop ~7 cycles ≈ 0.032us
     volatile uint32_t cycles = microseconds * 10; // Adjust this multiplier for your clock
@@ -594,11 +702,15 @@ void ADAR1000Manager::calculatePhaseSettings(float angle_degrees, uint8_t phase_
 }
 
 bool ADAR1000Manager::performSystemCalibration() {
+    DIAG_SECTION("BF SYSTEM CALIBRATION");
     for (uint8_t i = 0; i < devices_.size(); ++i) {
+        DIAG("BF", "Calibration: verifying dev[%u] communication...", i);
         if (!verifyDeviceCommunication(i)) {
+            DIAG_ERR("BF", "Calibration FAILED at dev[%u]", i);
             return false;
         }
     }
+    DIAG("BF", "performSystemCalibration() OK -- all devices verified");
     return true;
 }
 
@@ -615,6 +727,10 @@ uint32_t ADAR1000Manager::spiTransfer(uint8_t* txData, uint8_t* rxData, uint32_t
         status = HAL_SPI_Transmit(&hspi1, txData, size, 1000);
     }
 
+    if (status != HAL_OK) {
+        DIAG_ERR("BF", "SPI1 transfer FAILED: HAL status=%d, size=%lu", (int)status, (unsigned long)size);
+    }
+
     return (status == HAL_OK) ? size : 0;
 }
 
@@ -678,6 +794,7 @@ void ADAR1000Manager::adarResetBit(uint8_t deviceIndex, uint32_t mem_addr, uint8
 }
 
 void ADAR1000Manager::adarSoftReset(uint8_t deviceIndex) {
+    DIAG("BF", "adarSoftReset(dev[%u]): addr=0x%02X", deviceIndex, devices_[deviceIndex]->dev_addr);
     uint8_t instruction[3];
     instruction[0] = ((devices_[deviceIndex]->dev_addr & 0x03) << 5);
     instruction[1] = 0x00;
@@ -742,10 +859,19 @@ void ADAR1000Manager::adarSetTxBias(uint8_t deviceIndex, uint8_t broadcast) {
 uint8_t ADAR1000Manager::adarAdcRead(uint8_t deviceIndex, uint8_t broadcast) {
     adarWrite(deviceIndex, REG_ADC_CONTROL, ADAR1000_ADC_ST_CONV, broadcast);
 
-    // Wait for conversion
+    // Wait for conversion -- WARNING: no timeout, can hang if ADC never completes
+    uint32_t t0 = HAL_GetTick();
+    uint32_t polls = 0;
     while (!(adarRead(deviceIndex, REG_ADC_CONTROL) & 0x01)) {
-        // Busy wait
+        polls++;
+        if (HAL_GetTick() - t0 > 100) {
+            DIAG_ERR("BF", "adarAdcRead(dev[%u]): ADC conversion TIMEOUT after %lu ms, %lu polls",
+                     deviceIndex, (unsigned long)(HAL_GetTick() - t0), (unsigned long)polls);
+            return 0;
+        }
     }
+    DIAG("BF", "adarAdcRead(dev[%u]): conversion done in %lu ms (%lu polls)",
+         deviceIndex, (unsigned long)(HAL_GetTick() - t0), (unsigned long)polls);
 
     return adarRead(deviceIndex, REG_ADC_OUT);
 }
diff --git a/9_Firmware/9_1_Microcontroller/9_1_1_C_Cpp_Libraries/ADS7830.c b/9_Firmware/9_1_Microcontroller/9_1_1_C_Cpp_Libraries/ADS7830.c
index 9b1891e..372bb02 100644
--- a/9_Firmware/9_1_Microcontroller/9_1_1_C_Cpp_Libraries/ADS7830.c
+++ b/9_Firmware/9_1_Microcontroller/9_1_1_C_Cpp_Libraries/ADS7830.c
@@ -1,4 +1,5 @@
 #include "ADS7830.h"
+#include "diag_log.h"
 #include <string.h>
 
 /**
@@ -13,7 +14,11 @@
 bool ADS7830_Init(ADS7830_HandleTypeDef *hadc, I2C_HandleTypeDef *hi2c, uint8_t i2c_addr, 
                   ADS7830_SDMode_t sdmode, ADS7830_PDMode_t pdmode) {
     
+    DIAG("PA", "ADS7830_Init: addr=0x%02X (shifted=0x%02X), sdmode=%u, pdmode=%u",
+         i2c_addr, i2c_addr << 1, (unsigned)sdmode, (unsigned)pdmode);
+    
     if (hadc == NULL || hi2c == NULL) {
+        DIAG_ERR("PA", "ADS7830_Init: NULL handle(s)");
         return false;
     }
     
@@ -25,7 +30,10 @@ bool ADS7830_Init(ADS7830_HandleTypeDef *hadc, I2C_HandleTypeDef *hi2c, uint8_t
     hadc->last_conversion_result = 0;
     
     /* Test communication by reading from a channel */
-    return (ADS7830_Measure_SingleEnded(hadc, 0) != 0xFF); // 0xFF indicates communication error
+    uint8_t test_read = ADS7830_Measure_SingleEnded(hadc, 0);
+    bool ok = (test_read != 0xFF);
+    DIAG("PA", "ADS7830_Init: test read ch0 = 0x%02X => %s", test_read, ok ? "OK" : "FAILED (0xFF)");
+    return ok;
 }
 
 /**
@@ -110,6 +118,7 @@ uint8_t ADS7830_Measure_SingleEnded(ADS7830_HandleTypeDef *hadc, uint8_t channel
     // Write config register to the ADC
     HAL_StatusTypeDef status = HAL_I2C_Master_Transmit(hadc->hi2c, hadc->i2c_addr, &config, 1, HAL_MAX_DELAY);
     if (status != HAL_OK) {
+        DIAG_ERR("PA", "ADS7830 I2C transmit FAILED: addr=0x%02X ch=%u HAL=%d", hadc->i2c_addr, channel, (int)status);
         return 0xFF;
     }
 
@@ -120,6 +129,7 @@ uint8_t ADS7830_Measure_SingleEnded(ADS7830_HandleTypeDef *hadc, uint8_t channel
     uint8_t result = 0;
     status = HAL_I2C_Master_Receive(hadc->hi2c, hadc->i2c_addr, &result, 1, HAL_MAX_DELAY);
     if (status != HAL_OK) {
+        DIAG_ERR("PA", "ADS7830 I2C receive FAILED: addr=0x%02X ch=%u HAL=%d", hadc->i2c_addr, channel, (int)status);
         return 0xFF;
     }
     
@@ -179,6 +189,7 @@ int8_t ADS7830_Measure_Differential(ADS7830_HandleTypeDef *hadc, uint8_t channel
     // Write config register to the ADC
     HAL_StatusTypeDef status = HAL_I2C_Master_Transmit(hadc->hi2c, hadc->i2c_addr, &config, 1, HAL_MAX_DELAY);
     if (status != HAL_OK) {
+        DIAG_ERR("PA", "ADS7830 diff I2C transmit FAILED: addr=0x%02X ch=%u HAL=%d", hadc->i2c_addr, channel, (int)status);
         return (int8_t)0x80;
     }
 
@@ -189,6 +200,7 @@ int8_t ADS7830_Measure_Differential(ADS7830_HandleTypeDef *hadc, uint8_t channel
     uint8_t raw_adc = 0;
     status = HAL_I2C_Master_Receive(hadc->hi2c, hadc->i2c_addr, &raw_adc, 1, HAL_MAX_DELAY);
     if (status != HAL_OK) {
+        DIAG_ERR("PA", "ADS7830 diff I2C receive FAILED: addr=0x%02X ch=%u HAL=%d", hadc->i2c_addr, channel, (int)status);
         return (int8_t)0x80;
     }
     
diff --git a/9_Firmware/9_1_Microcontroller/9_1_1_C_Cpp_Libraries/DA5578.c b/9_Firmware/9_1_Microcontroller/9_1_1_C_Cpp_Libraries/DA5578.c
index e6696bb..8a5f6bc 100644
--- a/9_Firmware/9_1_Microcontroller/9_1_1_C_Cpp_Libraries/DA5578.c
+++ b/9_Firmware/9_1_Microcontroller/9_1_1_C_Cpp_Libraries/DA5578.c
@@ -1,4 +1,5 @@
 #include "DAC5578.h"
+#include "diag_log.h"
 #include <string.h>
 
 /**
@@ -17,7 +18,10 @@ bool DAC5578_Init(DAC5578_HandleTypeDef *hdac, I2C_HandleTypeDef *hi2c, uint8_t
                   uint8_t resolution, GPIO_TypeDef *ldac_port, uint16_t ldac_pin,
                   GPIO_TypeDef *clr_port, uint16_t clr_pin) {
     
+    DIAG("PA", "DAC5578_Init: addr=0x%02X (shifted=0x%02X), res=%u", i2c_addr, i2c_addr << 1, resolution);
+    
     if (hdac == NULL || hi2c == NULL) {
+        DIAG_ERR("PA", "DAC5578_Init: NULL handle(s)");
         return false;
     }
     
@@ -34,24 +38,32 @@ bool DAC5578_Init(DAC5578_HandleTypeDef *hdac, I2C_HandleTypeDef *hi2c, uint8_t
     
     /* Set LDAC high (inactive) and CLR high (normal operation) */
     if (ldac_port != NULL) {
+        DIAG("PA", "  LDAC pin -> HIGH (inactive)");
         HAL_GPIO_WritePin(ldac_port, ldac_pin, GPIO_PIN_SET);
     }
     
     if (clr_port != NULL) {
+        DIAG("PA", "  CLR pin -> HIGH (normal operation)");
         HAL_GPIO_WritePin(clr_port, clr_pin, GPIO_PIN_SET);
     }
 
     /* Reset the DAC and enable internal reference by default */
+    DIAG("PA", "  Resetting DAC5578...");
     bool success = DAC5578_Reset(hdac);
     if (success) {
+        DIAG("PA", "  Enabling internal reference...");
         success = DAC5578_SetInternalReference(hdac, true);
+    } else {
+        DIAG_ERR("PA", "  DAC5578_Reset FAILED");
     }
 
     /* Set the clear code in the device */
     if (success) {
+        DIAG("PA", "  Setting clear code to ZERO...");
         success = DAC5578_SetClearCode(hdac, hdac->clear_code);
     }
 
+    DIAG("PA", "DAC5578_Init: %s", success ? "OK" : "FAILED");
     return success;
 }
 
@@ -61,12 +73,16 @@ bool DAC5578_Init(DAC5578_HandleTypeDef *hdac, I2C_HandleTypeDef *hi2c, uint8_t
   * @retval bool: true if successful, false otherwise
   */
 bool DAC5578_Reset(DAC5578_HandleTypeDef *hdac) {
+    DIAG("PA", "DAC5578_Reset: addr=0x%02X", hdac->i2c_addr);
     uint8_t buffer[3];
     buffer[0] = DAC5578_CMD_RESET;
     buffer[1] = 0x00;
     buffer[2] = 0x00;
 
     HAL_StatusTypeDef status = HAL_I2C_Master_Transmit(hdac->hi2c, hdac->i2c_addr, buffer, 3, HAL_MAX_DELAY);
+    if (status != HAL_OK) {
+        DIAG_ERR("PA", "DAC5578_Reset: I2C transmit FAILED, HAL status=%d", (int)status);
+    }
     return (status == HAL_OK);
 }
 
@@ -123,12 +139,14 @@ bool DAC5578_UpdateChannel(DAC5578_HandleTypeDef *hdac, uint8_t channel) {
   */
 bool DAC5578_WriteAndUpdateChannelValue(DAC5578_HandleTypeDef *hdac, uint8_t channel, uint16_t value) {
     if (channel > 7) {
+        DIAG_ERR("PA", "DAC5578_WriteAndUpdate: channel %u out of range", channel);
         return false;
     }
 
     /* DAC5578 is 8-bit, so mask value */
     value &= 0xFF;
 
+    DIAG("PA", "DAC5578_WriteAndUpdate: addr=0x%02X ch=%u val=0x%02X (%u)", hdac->i2c_addr, channel, value, value);
     return DAC5578_CommandWrite(hdac, DAC5578_CMD_WRITE_UPDATE | (channel & 0x7), value);
 }
 
@@ -310,8 +328,11 @@ DAC5578_ClearCode_t DAC5578_GetClearCode(DAC5578_HandleTypeDef *hdac) {
   * @retval None
   */
 void DAC5578_ActivateClearPin(DAC5578_HandleTypeDef *hdac) {
+    DIAG_WARN("PA", "DAC5578_ActivateClearPin: CLR -> LOW (emergency clear), addr=0x%02X", hdac->i2c_addr);
     if (hdac->clr_port != NULL) {
         HAL_GPIO_WritePin(hdac->clr_port, hdac->clr_pin, GPIO_PIN_RESET);
+    } else {
+        DIAG_ERR("PA", "  CLR port is NULL -- cannot activate hardware clear!");
     }
 }
 
@@ -332,11 +353,14 @@ void DAC5578_DeactivateClearPin(DAC5578_HandleTypeDef *hdac) {
   * @retval None
   */
 void DAC5578_ClearOutputs(DAC5578_HandleTypeDef *hdac) {
+    DIAG_WARN("PA", "DAC5578_ClearOutputs: pulsing CLR pin, addr=0x%02X", hdac->i2c_addr);
     if (hdac->clr_port != NULL) {
         /* Generate a pulse on CLR pin (active low) */
         HAL_GPIO_WritePin(hdac->clr_port, hdac->clr_pin, GPIO_PIN_RESET);
         HAL_Delay(1); // Hold for at least 50ns (1ms is plenty)
         HAL_GPIO_WritePin(hdac->clr_port, hdac->clr_pin, GPIO_PIN_SET);
+    } else {
+        DIAG_ERR("PA", "  CLR port is NULL -- cannot pulse clear!");
     }
 }
 
@@ -366,6 +390,9 @@ bool DAC5578_CommandWrite(DAC5578_HandleTypeDef *hdac, uint8_t command, uint16_t
     buffer[2] = value & 0xFF;        // LSB (actual 8-bit data)
 
     HAL_StatusTypeDef status = HAL_I2C_Master_Transmit(hdac->hi2c, hdac->i2c_addr, buffer, 3, HAL_MAX_DELAY);
+    if (status != HAL_OK) {
+        DIAG_ERR("PA", "DAC5578 I2C write FAILED: addr=0x%02X cmd=0x%02X HAL=%d", hdac->i2c_addr, command, (int)status);
+    }
     return (status == HAL_OK);
 }
 
@@ -382,16 +409,19 @@ bool DAC5578_CommandRead(DAC5578_HandleTypeDef *hdac, uint8_t command, uint16_t
     /* First write the command to set up readback */
     HAL_StatusTypeDef status = HAL_I2C_Master_Transmit(hdac->hi2c, hdac->i2c_addr, &command, 1, HAL_MAX_DELAY);
     if (status != HAL_OK) {
+        DIAG_ERR("PA", "DAC5578 I2C read setup FAILED: addr=0x%02X cmd=0x%02X HAL=%d", hdac->i2c_addr, command, (int)status);
         return false;
     }
 
     /* Then read 3 bytes back */
     status = HAL_I2C_Master_Receive(hdac->hi2c, hdac->i2c_addr, buffer, 3, HAL_MAX_DELAY);
     if (status != HAL_OK) {
+        DIAG_ERR("PA", "DAC5578 I2C read data FAILED: addr=0x%02X cmd=0x%02X HAL=%d", hdac->i2c_addr, command, (int)status);
         return false;
     }
 
     /* Extract the 8-bit value from the response */
     *value = buffer[2] & 0xFF;
+    DIAG("PA", "DAC5578_Read: addr=0x%02X cmd=0x%02X => 0x%02X", hdac->i2c_addr, command, *value);
     return true;
 }
diff --git a/9_Firmware/9_1_Microcontroller/9_1_1_C_Cpp_Libraries/USBHandler.cpp b/9_Firmware/9_1_Microcontroller/9_1_1_C_Cpp_Libraries/USBHandler.cpp
index 2e56eb5..d689365 100644
--- a/9_Firmware/9_1_Microcontroller/9_1_1_C_Cpp_Libraries/USBHandler.cpp
+++ b/9_Firmware/9_1_Microcontroller/9_1_1_C_Cpp_Libraries/USBHandler.cpp
@@ -1,11 +1,14 @@
 #include "USBHandler.h"
+#include "diag_log.h"
 #include <cstring>
 
 USBHandler::USBHandler() {
+    DIAG("USB", "USBHandler constructed, calling reset()");
     reset();
 }
 
 void USBHandler::reset() {
+    DIAG("USB", "USBHandler::reset(): state -> WAITING_FOR_START");
     current_state = USBState::WAITING_FOR_START;
     start_flag_received = false;
     buffer_index = 0;
@@ -14,9 +17,12 @@ void USBHandler::reset() {
 
 void USBHandler::processUSBData(const uint8_t* data, uint32_t length) {
     if (data == nullptr || length == 0) {
+        DIAG_WARN("USB", "processUSBData: null/empty data");
         return;
     }
     
+    DIAG("USB", "processUSBData: %lu bytes, state=%d", (unsigned long)length, (int)current_state);
+    
     switch (current_state) {
         case USBState::WAITING_FOR_START:
             processStartFlag(data, length);
@@ -28,7 +34,7 @@ void USBHandler::processUSBData(const uint8_t* data, uint32_t length) {
             
         case USBState::READY_FOR_DATA:
             // Ready to receive radar data commands
-            // Add additional command processing here if needed
+            DIAG("USB", "  READY_FOR_DATA: ignoring %lu bytes", (unsigned long)length);
             break;
     }
 }
@@ -43,17 +49,17 @@ void USBHandler::processStartFlag(const uint8_t* data, uint32_t length) {
             start_flag_received = true;
             current_state = USBState::RECEIVING_SETTINGS;
             buffer_index = 0;  // Reset buffer for settings data
-            
-            // You can send an acknowledgment back here if needed
-            // sendUSBAcknowledgment();
+            DIAG("USB", "START FLAG found at offset %lu, state -> RECEIVING_SETTINGS", (unsigned long)i);
             
             // If there's more data after the start flag, process it
             if (length > i + 4) {
+                DIAG("USB", "  %lu trailing bytes after start flag, forwarding to settings parser", (unsigned long)(length - i - 4));
                 processSettingsData(data + i + 4, length - i - 4);
             }
             return;
         }
     }
+    DIAG("USB", "  no start flag in %lu bytes", (unsigned long)length);
 }
 
 void USBHandler::processSettingsData(const uint8_t* data, uint32_t length) {
@@ -63,6 +69,7 @@ void USBHandler::processSettingsData(const uint8_t* data, uint32_t length) {
     
     memcpy(usb_buffer + buffer_index, data, bytes_to_copy);
     buffer_index += bytes_to_copy;
+    DIAG("USB", "  settings buffer: +%lu bytes, total=%lu/%u", (unsigned long)bytes_to_copy, (unsigned long)buffer_index, MAX_BUFFER_SIZE);
     
     // Check if we have a complete settings packet (contains "SET" and "END")
     if (buffer_index >= 74) {  // Minimum size for valid settings packet
@@ -70,16 +77,19 @@ void USBHandler::processSettingsData(const uint8_t* data, uint32_t length) {
         bool has_set = (memcmp(usb_buffer, "SET", 3) == 0);
         bool has_end = false;
         
+        DIAG_BOOL("USB", "  packet starts with SET", has_set);
+        
         for (uint32_t i = 3; i <= buffer_index - 3; i++) {
             if (memcmp(usb_buffer + i, "END", 3) == 0) {
                 has_end = true;
+                DIAG("USB", "  END marker found at offset %lu, packet_len=%lu", (unsigned long)i, (unsigned long)(i + 3));
                 
                 // Parse the complete packet up to "END"
                 if (has_set && current_settings.parseFromUSB(usb_buffer, i + 3)) {
                     current_state = USBState::READY_FOR_DATA;
-                    
-                    // You can send settings acknowledgment back here
-                    // sendSettingsAcknowledgment();
+                    DIAG("USB", "  Settings parsed OK, state -> READY_FOR_DATA");
+                } else {
+                    DIAG_ERR("USB", "  Settings parse FAILED (has_set=%d)", has_set);
                 }
                 break;
             }
@@ -87,6 +97,7 @@ void USBHandler::processSettingsData(const uint8_t* data, uint32_t length) {
         
         // If we didn't find a valid packet but buffer is full, reset
         if (buffer_index >= MAX_BUFFER_SIZE && !has_end) {
+            DIAG_WARN("USB", "  Buffer full (%u) without END marker -- resetting", MAX_BUFFER_SIZE);
             buffer_index = 0;  // Reset buffer to avoid overflow
         }
     }
diff --git a/9_Firmware/9_1_Microcontroller/9_1_3_C_Cpp_Code/main.cpp b/9_Firmware/9_1_Microcontroller/9_1_3_C_Cpp_Code/main.cpp
index 06c6e4d..5666070 100644
--- a/9_Firmware/9_1_Microcontroller/9_1_3_C_Cpp_Code/main.cpp
+++ b/9_Firmware/9_1_Microcontroller/9_1_3_C_Cpp_Code/main.cpp
@@ -330,6 +330,7 @@ extern "C" {
 
     // USB CDC receive callback (called by STM32 HAL)
     void CDC_Receive_FS(uint8_t* Buf, uint32_t *Len) {
+        DIAG("USB", "CDC_Receive_FS callback: %lu bytes received", *Len);
         // Process received USB data
         usbHandler.processUSBData(Buf, *Len);
 
@@ -340,49 +341,67 @@ extern "C" {
 }
 
 void systemPowerUpSequence() {
+    DIAG_SECTION("PWR", "systemPowerUpSequence");
     uint8_t msg[] = "Starting Power Up Sequence...\r\n";
     HAL_UART_Transmit(&huart3, msg, sizeof(msg)-1, 1000);
 
     // Step 1: Initialize ADTR1107 power sequence
+    DIAG("PWR", "Step 1: initializeADTR1107Sequence()");
     if (!adarManager.initializeADTR1107Sequence()) {
+        DIAG_ERR("PWR", "ADTR1107 power sequence FAILED -- calling Error_Handler()");
         uint8_t err[] = "ERROR: ADTR1107 power sequence failed!\r\n";
         HAL_UART_Transmit(&huart3, err, sizeof(err)-1, 1000);
         Error_Handler();
     }
+    DIAG("PWR", "Step 1 OK: ADTR1107 sequence complete");
 
     // Step 2: Initialize all ADAR1000 devices
+    DIAG("PWR", "Step 2: initializeAllDevices()");
     if (!adarManager.initializeAllDevices()) {
+        DIAG_ERR("PWR", "ADAR1000 initialization FAILED -- calling Error_Handler()");
         uint8_t err[] = "ERROR: ADAR1000 initialization failed!\r\n";
         HAL_UART_Transmit(&huart3, err, sizeof(err)-1, 1000);
         Error_Handler();
     }
+    DIAG("PWR", "Step 2 OK: All ADAR1000 devices initialized");
 
     // Step 3: Perform system calibration
+    DIAG("PWR", "Step 3: performSystemCalibration()");
     if (!adarManager.performSystemCalibration()) {
+        DIAG_WARN("PWR", "System calibration returned issues (non-fatal)");
         uint8_t warn[] = "WARNING: System calibration issues\r\n";
         HAL_UART_Transmit(&huart3, warn, sizeof(warn)-1, 1000);
+    } else {
+        DIAG("PWR", "Step 3 OK: System calibration passed");
     }
 
     // Step 4: Set to safe TX mode
+    DIAG("PWR", "Step 4: setAllDevicesTXMode()");
     adarManager.setAllDevicesTXMode();
+    DIAG("PWR", "Step 4 OK: All devices set to TX mode");
 
     uint8_t success[] = "Power Up Sequence Completed Successfully\r\n";
     HAL_UART_Transmit(&huart3, success, sizeof(success)-1, 1000);
+    DIAG("PWR", "systemPowerUpSequence COMPLETE");
 }
 
 void systemPowerDownSequence() {
+    DIAG_SECTION("PWR", "systemPowerDownSequence");
     uint8_t msg[] = "Starting Power Down Sequence...\r\n";
     HAL_UART_Transmit(&huart3, msg, sizeof(msg)-1, 1000);
 
     // Step 1: Set all devices to RX mode (safest state)
+    DIAG("PWR", "Step 1: setAllDevicesRXMode()");
     adarManager.setAllDevicesRXMode();
     HAL_Delay(10);
 
     // Step 2: Disable PA power supplies
+    DIAG("PWR", "Step 2: Disable PA 5V supplies (EN_P_5V0_PA1/PA2/PA3 LOW)");
     HAL_GPIO_WritePin(EN_P_5V0_PA1_GPIO_Port, EN_P_5V0_PA1_Pin | EN_P_5V0_PA2_Pin | EN_P_5V0_PA3_Pin, GPIO_PIN_RESET);
     HAL_Delay(10);
 
     // Step 3: Set PA biases to safe values
+    DIAG("PWR", "Step 3: Setting PA bias to safe value 0x20 on all 4 devices, 4 channels each");
     for (uint8_t dev = 0; dev < 4; dev++) {
         adarManager.adarWrite(dev, REG_PA_CH1_BIAS_ON, 0x20, BROADCAST_OFF);
         adarManager.adarWrite(dev, REG_PA_CH2_BIAS_ON, 0x20, BROADCAST_OFF);
@@ -392,22 +411,26 @@ void systemPowerDownSequence() {
     HAL_Delay(10);
 
     // Step 4: Disable LNA power supply
+    DIAG("PWR", "Step 4: Disable LNA 3.3V supply (EN_P_3V3_ADTR LOW)");
     HAL_GPIO_WritePin(EN_P_3V3_ADTR_GPIO_Port, EN_P_3V3_ADTR_Pin, GPIO_PIN_RESET);
     HAL_Delay(10);
 
     // Step 5: Set LNA bias to safe value
+    DIAG("PWR", "Step 5: Setting LNA bias to 0x00 on all 4 devices");
     for (uint8_t dev = 0; dev < 4; dev++) {
         adarManager.adarWrite(dev, REG_LNA_BIAS_ON, 0x00, BROADCAST_OFF);
     }
     HAL_Delay(10);
 
     // Step 6: Disable switch power supplies
+    DIAG("PWR", "Step 6: Disable switch supplies (EN_P_3V3_VDD_SW, EN_P_3V3_SW LOW)");
     HAL_GPIO_WritePin(EN_P_3V3_VDD_SW_GPIO_Port, EN_P_3V3_VDD_SW_Pin, GPIO_PIN_RESET);
     HAL_GPIO_WritePin(EN_P_3V3_SW_GPIO_Port, EN_P_3V3_SW_Pin, GPIO_PIN_RESET);
     HAL_Delay(10);
 
     uint8_t success[] = "Power Down Sequence Completed\r\n";
     HAL_UART_Transmit(&huart3, success, sizeof(success)-1, 1000);
+    DIAG("PWR", "systemPowerDownSequence COMPLETE");
 }
 
 void initializeBeamMatrices() {
@@ -444,6 +467,10 @@ void initializeBeamMatrices() {
 }
 
 void executeChirpSequence(int num_chirps, float T1, float PRI1, float T2, float PRI2) {
+    // NOTE: No per-chirp DIAG — this is a us/ns timing-critical path.
+    // Only log entry params for post-mortem analysis.
+    DIAG("SYS", "executeChirpSequence: num_chirps=%d T1=%.2f PRI1=%.2f T2=%.2f PRI2=%.2f",
+         num_chirps, T1, PRI1, T2, PRI2);
     // First chirp sequence (microsecond timing)
     for(int i = 0; i < num_chirps; i++) {
         HAL_GPIO_TogglePin(GPIOD, GPIO_PIN_8); // New chirp signal to FPGA
@@ -472,8 +499,11 @@ void runRadarPulseSequence() {
 
     snprintf(msg, sizeof(msg), "Starting RADAR Sequence #%d\r\n", ++sequence_count);
     HAL_UART_Transmit(&huart3, (uint8_t*)msg, strlen(msg), 1000);
+    DIAG("SYS", "runRadarPulseSequence #%d: m_max=%d n_max=%d y_max=%d",
+         sequence_count, m_max, n_max, y_max);
 
     // Configure for fast switching
+    DIAG("BF", "Enabling fast-switch mode for beam sweep");
     adarManager.setFastSwitchMode(true);
 
     int m = 1; // Chirp counter
@@ -483,6 +513,7 @@ void runRadarPulseSequence() {
     // Main beam steering sequence
     for(int beam_pos = 0; beam_pos < 15; beam_pos++) {
     	HAL_GPIO_TogglePin(GPIOD, GPIO_PIN_9);// Notify FPGA of elevation change
+    	DIAG("SYS", "Beam pos %d/15: elevation GPIO toggle, patterns matrix1/vector_0/matrix2", beam_pos);
         // Pattern 1: matrix1 (positive steering angles)
         adarManager.setCustomBeamPattern16(matrix1[beam_pos], ADAR1000Manager::BeamDirection::TX);
         adarManager.setCustomBeamPattern16(matrix1[beam_pos], ADAR1000Manager::BeamDirection::RX);
@@ -513,6 +544,7 @@ void runRadarPulseSequence() {
     }
 
     HAL_GPIO_TogglePin(GPIOD,GPIO_PIN_10);//Tell FPGA that there is a new azimuth
+    DIAG("SYS", "Azimuth GPIO toggle (GPIOD pin 10), stepping motor");
 
     y++; if(y>y_max)y=1;
 	  //Rotate stepper to next y position
@@ -522,11 +554,13 @@ void runRadarPulseSequence() {
 		  HAL_GPIO_WritePin(STEPPER_CLK_P_GPIO_Port, STEPPER_CLK_P_Pin, GPIO_PIN_RESET);
 		  delay_us(500);
 	  }
+    DIAG("MOT", "Stepper moved %d steps for azimuth position y=%d", (int)(Stepper_steps/y_max), y);
 
 
 
     snprintf(msg, sizeof(msg), "RADAR Sequence #%d Completed\r\n", sequence_count);
     HAL_UART_Transmit(&huart3, (uint8_t*)msg, strlen(msg), 1000);
+    DIAG("SYS", "runRadarPulseSequence #%d COMPLETE", sequence_count);
 }
 
 
@@ -590,9 +624,13 @@ SystemError_t checkSystemHealth(void) {
     // 1. Check AD9523 Clock Generator
     static uint32_t last_clock_check = 0;
     if (HAL_GetTick() - last_clock_check > 5000) {
-        if (HAL_GPIO_ReadPin(AD9523_STATUS0_GPIO_Port, AD9523_STATUS0_Pin) == GPIO_PIN_RESET ||
-            HAL_GPIO_ReadPin(AD9523_STATUS1_GPIO_Port, AD9523_STATUS1_Pin) == GPIO_PIN_RESET) {
+        GPIO_PinState s0 = HAL_GPIO_ReadPin(AD9523_STATUS0_GPIO_Port, AD9523_STATUS0_Pin);
+        GPIO_PinState s1 = HAL_GPIO_ReadPin(AD9523_STATUS1_GPIO_Port, AD9523_STATUS1_Pin);
+        DIAG_GPIO("CLK", "AD9523 STATUS0", s0);
+        DIAG_GPIO("CLK", "AD9523 STATUS1", s1);
+        if (s0 == GPIO_PIN_RESET || s1 == GPIO_PIN_RESET) {
             current_error = ERROR_AD9523_CLOCK;
+            DIAG_ERR("CLK", "AD9523 clock health check FAILED (STATUS0=%d STATUS1=%d)", s0, s1);
         }
         last_clock_check = HAL_GetTick();
     }
@@ -600,31 +638,40 @@ SystemError_t checkSystemHealth(void) {
     // 2. Check ADF4382 Lock Status
     bool tx_locked, rx_locked;
     if (ADF4382A_CheckLockStatus(&lo_manager, &tx_locked, &rx_locked) == ADF4382A_MANAGER_OK) {
-        if (!tx_locked) current_error = ERROR_ADF4382_TX_UNLOCK;
-        if (!rx_locked) current_error = ERROR_ADF4382_RX_UNLOCK;
+        if (!tx_locked) {
+            current_error = ERROR_ADF4382_TX_UNLOCK;
+            DIAG_ERR("LO", "Health check: TX LO UNLOCKED");
+        }
+        if (!rx_locked) {
+            current_error = ERROR_ADF4382_RX_UNLOCK;
+            DIAG_ERR("LO", "Health check: RX LO UNLOCKED");
+        }
     }
 
     // 3. Check ADAR1000 Communication and Temperature
     for (int i = 0; i < 4; i++) {
         if (!adarManager.verifyDeviceCommunication(i)) {
             current_error = ERROR_ADAR1000_COMM;
+            DIAG_ERR("BF", "Health check: ADAR1000 #%d comm FAILED", i);
             break;
         }
 
         float temp = adarManager.readTemperature(i);
         if (temp > 85.0f) {
             current_error = ERROR_ADAR1000_TEMP;
+            DIAG_ERR("BF", "Health check: ADAR1000 #%d OVERTEMP %.1fC > 85C", i, temp);
             break;
         }
     }
 
-    // 4. Check IMU Communication
-    static uint32_t last_imu_check = 0;
-    if (HAL_GetTick() - last_imu_check > 10000) {
-        if (!GY85_Update(&imu)) {
-            current_error = ERROR_IMU_COMM;
-        }
-        last_imu_check = HAL_GetTick();
+    // 4. Check IMU Communication
+    static uint32_t last_imu_check = 0;
+    if (HAL_GetTick() - last_imu_check > 10000) {
+        if (!GY85_Update(&imu)) {
+            current_error = ERROR_IMU_COMM;
+            DIAG_ERR("IMU", "Health check: GY85_Update() FAILED");
+        }
+        last_imu_check = HAL_GetTick();
     }
 
     // 5. Check BMP180 Communication
@@ -633,6 +680,7 @@ SystemError_t checkSystemHealth(void) {
         double pressure = myBMP.getPressure();
         if (pressure < 30000.0 || pressure > 110000.0 || isnan(pressure)) {
             current_error = ERROR_BMP180_COMM;
+            DIAG_ERR("SYS", "Health check: BMP180 pressure out of range: %.0f", pressure);
         }
         last_bmp_check = HAL_GetTick();
     }
@@ -644,6 +692,7 @@ SystemError_t checkSystemHealth(void) {
     }
     if (HAL_GetTick() - last_gps_fix > 30000) {
         current_error = ERROR_GPS_COMM;
+        DIAG_WARN("SYS", "Health check: GPS no fix for >30s");
     }
 
     // 7. Check RF Power Amplifier Current
@@ -651,10 +700,12 @@ SystemError_t checkSystemHealth(void) {
         for (int i = 0; i < 16; i++) {
             if (Idq_reading[i] > 2.5f) {
                 current_error = ERROR_RF_PA_OVERCURRENT;
+                DIAG_ERR("PA", "Health check: PA ch%d OVERCURRENT Idq=%.3fA > 2.5A", i, Idq_reading[i]);
                 break;
             }
             if (Idq_reading[i] < 0.1f) {
                 current_error = ERROR_RF_PA_BIAS;
+                DIAG_ERR("PA", "Health check: PA ch%d BIAS FAULT Idq=%.3fA < 0.1A", i, Idq_reading[i]);
                 break;
             }
         }
@@ -663,20 +714,27 @@ SystemError_t checkSystemHealth(void) {
     // 8. Check System Temperature
     if (temperature > 75.0f) {
         current_error = ERROR_TEMPERATURE_HIGH;
+        DIAG_ERR("SYS", "Health check: System OVERTEMP %.1fC > 75C", temperature);
     }
 
     // 9. Simple watchdog check
     static uint32_t last_health_check = 0;
     if (HAL_GetTick() - last_health_check > 60000) {
         current_error = ERROR_WATCHDOG_TIMEOUT;
+        DIAG_ERR("SYS", "Health check: Watchdog timeout (>60s since last check)");
     }
     last_health_check = HAL_GetTick();
 
+    if (current_error != ERROR_NONE) {
+        DIAG_ERR("SYS", "checkSystemHealth returning error code %d", current_error);
+    }
     return current_error;
-}
+}
 
 // Error recovery function
 void attemptErrorRecovery(SystemError_t error) {
+    DIAG_SECTION("SYS", "attemptErrorRecovery");
+    DIAG("SYS", "Attempting recovery from error code %d", error);
     char recovery_msg[80];
     snprintf(recovery_msg, sizeof(recovery_msg),
              "Attempting recovery from error: %d\r\n", error);
@@ -686,44 +744,57 @@ void attemptErrorRecovery(SystemError_t error) {
         case ERROR_ADF4382_TX_UNLOCK:
         case ERROR_ADF4382_RX_UNLOCK:
             // Re-initialize LO
+            DIAG("LO", "Recovery: Re-initializing LO manager (SYNC_METHOD_TIMED)");
             ADF4382A_Manager_Init(&lo_manager, SYNC_METHOD_TIMED);
             HAL_Delay(100);
+            DIAG("LO", "Recovery: LO re-init complete");
             break;
 
         case ERROR_ADAR1000_COMM:
             // Reset ADAR1000 communication
+            DIAG("BF", "Recovery: Re-initializing all ADAR1000 devices");
             adarManager.initializeAllDevices();
             HAL_Delay(50);
+            DIAG("BF", "Recovery: ADAR1000 re-init complete");
             break;
 
         case ERROR_IMU_COMM:
             // Re-initialize IMU
+            DIAG("IMU", "Recovery: Re-initializing GY85 IMU");
             GY85_Init();
             HAL_Delay(100);
+            DIAG("IMU", "Recovery: IMU re-init complete");
             break;
 
         case ERROR_GPS_COMM:
             // GPS will auto-recover when signal returns
+            DIAG("SYS", "Recovery: GPS error -- no action (auto-recover on signal)");
             break;
 
         default:
             // For other errors, just log and continue
+            DIAG_WARN("SYS", "Recovery: No specific handler for error %d", error);
             break;
     }
 
     snprintf(recovery_msg, sizeof(recovery_msg),
              "Recovery attempt completed.\r\n");
     HAL_UART_Transmit(&huart3, (uint8_t*)recovery_msg, strlen(recovery_msg), 1000);
+    DIAG("SYS", "attemptErrorRecovery COMPLETE");
 }
 
 ////////////////////////////////////////////////////////////////////////////////
 //:::::RF POWER AMPLIFIER DAC5578 Emergency stop function using CLR pin/////////
 ////////////////////////////////////////////////////////////////////////////////
 void Emergency_Stop(void) {
+    DIAG_ERR("PA", ">>> EMERGENCY_STOP ACTIVATED <<<");
     /* Immediately clear all DAC outputs to zero using hardware CLR */
+    DIAG_ERR("PA", "Clearing DAC1 outputs via CLR pin");
     DAC5578_ActivateClearPin(&hdac1);
+    DIAG_ERR("PA", "Clearing DAC2 outputs via CLR pin");
     DAC5578_ActivateClearPin(&hdac2);
 
+    DIAG_ERR("PA", "DACs cleared -- entering infinite hold loop (manual reset required)");
     /* Keep outputs cleared until reset */
     while (1) {
         HAL_Delay(100);
@@ -736,6 +807,7 @@ void handleSystemError(SystemError_t error) {
 
     error_count++;
     last_error = error;
+    DIAG_ERR("SYS", "handleSystemError: error=%d error_count=%lu", error, error_count);
 
     char error_msg[100];
     const char* error_strings[] = {
@@ -764,6 +836,7 @@ void handleSystemError(SystemError_t error) {
     HAL_UART_Transmit(&huart3, (uint8_t*)error_msg, strlen(error_msg), 1000);
 
     // Blink LED pattern based on error code
+    DIAG("SYS", "Blinking LED3 %d times for error code %d", (error % 5) + 1, error);
     for (int i = 0; i < (error % 5) + 1; i++) {
         HAL_GPIO_TogglePin(LED_3_GPIO_Port, LED_3_Pin);
         HAL_Delay(200);
@@ -771,6 +844,7 @@ void handleSystemError(SystemError_t error) {
 
     // Critical errors trigger emergency shutdown
     if (error >= ERROR_RF_PA_OVERCURRENT && error <= ERROR_POWER_SUPPLY) {
+        DIAG_ERR("SYS", "CRITICAL ERROR (code %d: %s) -- initiating Emergency_Stop()", error, error_strings[error]);
         snprintf(error_msg, sizeof(error_msg),
                  "CRITICAL ERROR! Initiating emergency shutdown.\r\n");
         HAL_UART_Transmit(&huart3, (uint8_t*)error_msg, strlen(error_msg), 1000);
@@ -781,6 +855,7 @@ void handleSystemError(SystemError_t error) {
 
     // For non-critical errors, attempt recovery
     if (!system_emergency_state) {
+        DIAG("SYS", "Non-critical error -- attempting recovery");
         attemptErrorRecovery(error);
     }
 }
@@ -791,10 +866,13 @@ bool checkSystemHealthStatus(void) {
     SystemError_t error = checkSystemHealth();
 
     if (error != ERROR_NONE) {
+        DIAG_ERR("SYS", "checkSystemHealthStatus: error detected (code %d), calling handleSystemError()", error);
         handleSystemError(error);
 
         // If we're in emergency state or too many errors, shutdown
         if (system_emergency_state || error_count > 10) {
+            DIAG_ERR("SYS", "checkSystemHealthStatus returning FALSE (emergency=%s error_count=%lu)",
+                     system_emergency_state ? "true" : "false", error_count);
             return false;
         }
     }
@@ -1677,39 +1755,52 @@ int main(void)
   /************RF Power Amplifier Powering up sequence************/
   /***************************************************************/
   if(PowerAmplifier){
+	  DIAG_SECTION("PA", "RF Power Amplifier power-up sequence (PowerAmplifier=true)");
 	  /* Initialize DACs */
 	  /* DAC1: Address 0b1001000 = 0x48 */
+	  DIAG("PA", "Initializing DAC1 (I2C1, addr=0x48, 8-bit)");
 	  if (!DAC5578_Init(&hdac1, &hi2c1, 0x48, 8,
 			  DAC_1_VG_LDAC_GPIO_Port, DAC_1_VG_LDAC_Pin,
 			  DAC_1_VG_CLR_GPIO_Port, DAC_1_VG_CLR_Pin)) {
+		  DIAG_ERR("PA", "DAC1 init FAILED -- calling Error_Handler()");
 		  Error_Handler();
 	  }
+	  DIAG("PA", "DAC1 init OK");
 
 	  /* DAC2: Address 0b1001001 = 0x49 */
+	  DIAG("PA", "Initializing DAC2 (I2C1, addr=0x49, 8-bit)");
 	  if (!DAC5578_Init(&hdac2, &hi2c1, 0x49, 8,
 			  DAC_2_VG_LDAC_GPIO_Port, DAC_2_VG_LDAC_Pin,
 			  DAC_2_VG_CLR_GPIO_Port, DAC_2_VG_CLR_Pin)) {
+		  DIAG_ERR("PA", "DAC2 init FAILED -- calling Error_Handler()");
 		  Error_Handler();
 	  }
+	  DIAG("PA", "DAC2 init OK");
+
 	  /* Configure clear code behavior */
+	  DIAG("PA", "Setting clear code to ZERO on both DACs");
 	  DAC5578_SetClearCode(&hdac1, DAC5578_CLR_CODE_ZERO); // Clear to 0V on CLR pulse
 	  DAC5578_SetClearCode(&hdac2, DAC5578_CLR_CODE_ZERO); // Clear to 0V on CLR pulse
 
 	  /* Configure LDAC so all channels update simultaneously on hardware LDAC */
+	  DIAG("PA", "Setting LDAC mask=0xFF on both DACs (all channels respond)");
 	  DAC5578_SetupLDAC(&hdac1, 0xFF); // All channels respond to LDAC
 	  DAC5578_SetupLDAC(&hdac2, 0xFF); // All channels respond to LDAC
 
 	  //set Vg [1-8] to -3.98V -> input opamp = 1.63058V->126(8bits)
+	  DIAG("PA", "Writing initial DAC_val=%d to DAC1 channels 0-7", DAC_val);
 	  for(int channel = 0; channel < 8; channel++){
 		  DAC5578_WriteAndUpdateChannelValue(&hdac1, channel, DAC_val);
 	  }
 
 	  //set Vg [9-16] to -3.98V -> input opamp = 1.63058V->126(8bits)
+	  DIAG("PA", "Writing initial DAC_val=%d to DAC2 channels 0-7", DAC_val);
 	  for(int channel = 0; channel < 8; channel++){
 		  DAC5578_WriteAndUpdateChannelValue(&hdac2, channel, DAC_val);
 	  }
 
 	  /* Optional: Use hardware LDAC for simultaneous update of all channels */
+	  DIAG("PA", "Pulsing LDAC on both DACs for simultaneous update");
 	  HAL_GPIO_WritePin(DAC_1_VG_LDAC_GPIO_Port, DAC_1_VG_LDAC_Pin, GPIO_PIN_RESET);
 	  HAL_GPIO_WritePin(DAC_2_VG_LDAC_GPIO_Port, DAC_2_VG_LDAC_Pin, GPIO_PIN_RESET);
 	  HAL_Delay(1);
@@ -1717,39 +1808,53 @@ int main(void)
 	  HAL_GPIO_WritePin(DAC_2_VG_LDAC_GPIO_Port, DAC_2_VG_LDAC_Pin, GPIO_PIN_SET);
 
 	  //Enable RF Power Amplifier VDD = 22V
+	  DIAG("PA", "Enabling RFPA VDD=22V (EN_DIS_RFPA_VDD HIGH)");
 	  HAL_GPIO_WritePin(EN_DIS_RFPA_VDD_GPIO_Port, EN_DIS_RFPA_VDD_Pin, GPIO_PIN_SET);
 
 	  /* Initialize ADCs with correct addresses */
 	  /* ADC1: Address 0x48, Single-Ended mode, Internal Ref ON + ADC ON */
+	  DIAG("PA", "Initializing ADC1 (I2C2, addr=0x48, single-ended, ref+adc ON)");
 	  if (!ADS7830_Init(&hadc1, &hi2c2, 0x48,
 						ADS7830_SDMODE_SINGLE, ADS7830_PDIRON_ADON)) {
+		  DIAG_ERR("PA", "ADC1 init FAILED -- calling Error_Handler()");
 		  Error_Handler();
 	  }
+	  DIAG("PA", "ADC1 init OK");
 
 	  /* ADC2: Address 0x4A, Single-Ended mode, Internal Ref ON + ADC ON */
+	  DIAG("PA", "Initializing ADC2 (I2C2, addr=0x4A, single-ended, ref+adc ON)");
 	  if (!ADS7830_Init(&hadc2, &hi2c2, 0x4A,
 						ADS7830_SDMODE_SINGLE, ADS7830_PDIRON_ADON)) {
+		  DIAG_ERR("PA", "ADC2 init FAILED -- calling Error_Handler()");
 		  Error_Handler();
 	  }
+	  DIAG("PA", "ADC2 init OK");
 
 	  /* Read all 8 channels from ADC1 and calculate Idq */
+	  DIAG("PA", "Reading initial Idq from ADC1 channels 0-7");
 	  for (uint8_t channel = 0; channel < 8; channel++) {
 		  adc1_readings[channel] = ADS7830_Measure_SingleEnded(&hadc1, channel);
 		  Idq_reading[channel]= (3.3/255)*adc1_readings[channel]/(50*0.005);//Idq=Vadc/(GxRshunt)//G_INA241A3=50;Rshunt=5mOhms
+		  DIAG("PA", "  ADC1 ch%d: raw=%d Idq=%.3fA", channel, adc1_readings[channel], Idq_reading[channel]);
 	  }
 
 	  /* Read all 8 channels from ADC2 and calculate Idq*/
+	  DIAG("PA", "Reading initial Idq from ADC2 channels 0-7");
 	  for (uint8_t channel = 0; channel < 8; channel++) {
 		  adc2_readings[channel] = ADS7830_Measure_SingleEnded(&hadc2, channel);
 		  Idq_reading[channel+8]= (3.3/255)*adc2_readings[channel]/(50*0.005);//Idq=Vadc/(GxRshunt)//G_INA241A3=50;Rshunt=5mOhms
+		  DIAG("PA", "  ADC2 ch%d: raw=%d Idq=%.3fA", channel, adc2_readings[channel], Idq_reading[channel+8]);
 	  }
 
+	  DIAG("PA", "Starting Idq calibration loop for DAC1 channels 0-7 (target=1.680A)");
 	  for (uint8_t channel = 0; channel < 8; channel++){
 	      uint8_t safety_counter = 0;
 	      DAC_val = 126; // Reset for each channel
 
 	      do {
 	          if (safety_counter++ > 50) { // Prevent infinite loop
+	              DIAG_WARN("PA", "  DAC1 ch%d: safety limit reached (50 iterations), DAC_val=%d Idq=%.3fA",
+	                        channel, DAC_val, Idq_reading[channel]);
 	              break;
 	          }
 	          DAC_val = DAC_val - 4;
@@ -1757,14 +1862,19 @@ int main(void)
 	          adc1_readings[channel] = ADS7830_Measure_SingleEnded(&hadc1, channel);
 	          Idq_reading[channel] = (3.3/255) * adc1_readings[channel] / (50 * 0.005);
 	      } while (DAC_val > 38 && abs(Idq_reading[channel] - 1.680) < 0.2); // Fixed logic
+	      DIAG("PA", "  DAC1 ch%d calibrated: DAC_val=%d Idq=%.3fA iters=%d",
+	           channel, DAC_val, Idq_reading[channel], safety_counter);
 	  }
 
+	  DIAG("PA", "Starting Idq calibration loop for DAC2 channels 0-7 (target=1.680A)");
 	  for (uint8_t channel = 0; channel < 8; channel++){
 	      uint8_t safety_counter = 0;
 	      DAC_val = 126; // Reset for each channel
 
 	      do {
 	          if (safety_counter++ > 50) { // Prevent infinite loop
+	              DIAG_WARN("PA", "  DAC2 ch%d: safety limit reached (50 iterations), DAC_val=%d Idq=%.3fA",
+	                        channel, DAC_val, Idq_reading[channel+8]);
 	              break;
 	          }
 	          DAC_val = DAC_val - 4;
@@ -1772,7 +1882,10 @@ int main(void)
 	          adc1_readings[channel] = ADS7830_Measure_SingleEnded(&hadc2, channel);
 	          Idq_reading[channel+8] = (3.3/255) * adc2_readings[channel] / (50 * 0.005);
 	      } while (DAC_val > 38 && abs(Idq_reading[channel+8] - 1.680) < 0.2); // Fixed logic
+	      DIAG("PA", "  DAC2 ch%d calibrated: DAC_val=%d Idq=%.3fA iters=%d",
+	           channel, DAC_val, Idq_reading[channel+8], safety_counter);
 	  }
+	  DIAG("PA", "PA IDQ calibration sequence COMPLETE");
   }
 
   //RESET FPGA
@@ -1789,16 +1902,20 @@ int main(void)
   HAL_GPIO_WritePin(GPIOD, GPIO_PIN_11, GPIO_PIN_SET);
 
   /* T°C sensor TMP37 ADC3: Address 0x49, Single-Ended mode, Internal Ref ON + ADC ON */
+  DIAG("SYS", "Initializing temperature sensor ADC3 (I2C2, addr=0x49, TMP37)");
   if (!ADS7830_Init(&hadc3, &hi2c2, 0x49,
 					ADS7830_SDMODE_SINGLE, ADS7830_PDIRON_ADON)) {
+	  DIAG_ERR("SYS", "Temperature sensor ADC3 init FAILED -- calling Error_Handler()");
 	  Error_Handler();
   }
+  DIAG("SYS", "Temperature sensor ADC3 init OK");
 
   /***************************************************************/
   /************************ERRORS HANDLERS************************/
   /***************************************************************/
 
   // Initialize error handler system
+  DIAG("SYS", "Initializing error handler: clearing error_count, last_error, emergency_state");
   error_count = 0;
   last_error = ERROR_NONE;
   system_emergency_state = false;
@@ -1808,10 +1925,14 @@ int main(void)
   /***************************************************************/
 
   // Send initial status to GUI
+  DIAG("USB", "Sending initial system status to GUI via USB CDC");
   char initial_status[500];
   getSystemStatusForGUI(initial_status, sizeof(initial_status));
   // Send via USB to GUI
   CDC_Transmit_FS((uint8_t*)initial_status, strlen(initial_status));
+  DIAG("USB", "Initial status sent (%d bytes)", (int)strlen(initial_status));
+
+  DIAG("SYS", "=== INIT COMPLETE -- entering main loop ===");