From 0a49320e314b79b6c161d3e1ad0d1931eebb9590 Mon Sep 17 00:00:00 2001 From: Jason <83615043+JJassonn69@users.noreply.github.com> Date: Tue, 28 Apr 2026 09:45:41 +0545 Subject: [PATCH] =?UTF-8?q?fix(mcu):=20MCU-A2=20=E2=80=94=20site-configura?= =?UTF-8?q?ble=20mag=20declination,=20persisted=20in=20BKPSRAM?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The magnetometer yaw correction used a hardcoded -0.61 deg literal baked in for one deployment site. Yaw_Sensor was wrong by (site_decl + 0.61) deg at every other site whenever the UM982 dual-antenna heading was unavailable. Backed the value with BKPSRAM (slots 1+2 — slot 0 is the MCU-A7 emergency flag) and exposed set_mag_declination_deg / get_mag_declination_deg. Default returns the legacy -0.61 deg when no override has been written so the original site stays correct out of the box; a host command (or future GPS-derived auto-calibration) writes the new site value once and it persists across every reset path until main-power removal. Hardened with a +/-30 deg range clamp on both write AND read paths — real magnetic declinations are roughly +/-25 deg worldwide, so a wider value indicates a calibration error or BKPSRAM corruption (VBAT brown-out, bit flip) rather than a legitimate site. Defensive read-side clamp prevents a corrupted slot from propagating a wild heading offset. Replaced the single use site at the magnetometer yaw computation with the getter; legacy global Mag_Declination retained and kept in sync by the setter for any external linkage. Added test_mcu_a2_mag_declination (10 cases): default, set/get, persistence across reset, power-cycle clear, write-side clamp both directions, plausible-site passthrough, defensive read-side clamp on corruption, wrong-magic fallback, pre-fix bearing-error regression. MCU regression now 81/81. --- .../9_1_3_C_Cpp_Code/main.cpp | 51 ++++++- 9_Firmware/9_1_Microcontroller/tests/Makefile | 4 + .../tests/test_mcu_a2_mag_declination.c | 137 ++++++++++++++++++ 3 files changed, 190 insertions(+), 2 deletions(-) create mode 100644 9_Firmware/9_1_Microcontroller/tests/test_mcu_a2_mag_declination.c 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 0ecefff..1d2ae7a 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 @@ -147,7 +147,14 @@ float M11=1.0, M12=-0.0, M13=0.0, float ax, ay, az, gx, gy, gz, mx, my, mz,mxc,myc,mzc; // variables to hold latest sensor data values float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // vector to hold quaternion float temperature; -float Mag_Declination = -0.61; //0° +/* MCU-A2: site-specific magnetic declination. Was a hardcoded -0.61° literal + * baked in for one deployment location. Default kept for backward compatibility + * with existing sites; runtime override flows through set_mag_declination_deg() + * and persists across reset in BKPSRAM (slots 1+2 — slot 0 is the MCU-A7 + * emergency flag). Reads via get_mag_declination_deg() at the use site. */ +#define MAG_DECLINATION_DEFAULT_DEG (-0.61f) +#define MAG_DECLINATION_LIMIT_DEG (30.0f) +float Mag_Declination = MAG_DECLINATION_DEFAULT_DEG; /* legacy global, retained for any external linkage */ float RxAcc,RyAcc,RzAcc; float Rate_Ayz_1,Rate_Axz_1,Rate_Axy_1; @@ -919,6 +926,46 @@ static bool emergency_persist_check(void) { return *EMERGENCY_PERSIST_ADDR == EMERGENCY_PERSIST_MAGIC; } +//////////////////////////////////////////////////////////////////////////////// +// MCU-A2: site-configurable magnetic declination, persisted in BKPSRAM +// +// Slot layout (BKPSRAM, 4-byte words): +// [0] MCU-A7 emergency-state magic +// [1] MCU-A2 mag-declination magic +// [2] MCU-A2 mag-declination value (float, bit-cast as uint32) +// +// Survives every reset path; cleared only on main-power removal. Range +// clamped to ±30° before persisting (anything beyond that is a calibration +// error rather than a legitimate site value — physical declination ranges +// are roughly -25° to +25° worldwide). +//////////////////////////////////////////////////////////////////////////////// +#define MAG_DECL_PERSIST_MAGIC 0xCAFEFACEU +#define MAG_DECL_MAGIC_ADDR ((volatile uint32_t *)(BKPSRAM_BASE + 4)) +#define MAG_DECL_VALUE_ADDR ((volatile uint32_t *)(BKPSRAM_BASE + 8)) + +void set_mag_declination_deg(float deg) { + if (deg > MAG_DECLINATION_LIMIT_DEG) deg = MAG_DECLINATION_LIMIT_DEG; + if (deg < -MAG_DECLINATION_LIMIT_DEG) deg = -MAG_DECLINATION_LIMIT_DEG; + emergency_persist_init_clocks(); + union { float f; uint32_t u; } cvt = { .f = deg }; + *MAG_DECL_VALUE_ADDR = cvt.u; + *MAG_DECL_MAGIC_ADDR = MAG_DECL_PERSIST_MAGIC; + Mag_Declination = deg; /* keep legacy global in sync for any external readers */ +} + +float get_mag_declination_deg(void) { + emergency_persist_init_clocks(); + if (*MAG_DECL_MAGIC_ADDR == MAG_DECL_PERSIST_MAGIC) { + union { float f; uint32_t u; } cvt = { .u = *MAG_DECL_VALUE_ADDR }; + /* defensive clamp in case BKPSRAM was corrupted by VBAT brown-out */ + float v = cvt.f; + if (v > MAG_DECLINATION_LIMIT_DEG) v = MAG_DECLINATION_LIMIT_DEG; + if (v < -MAG_DECLINATION_LIMIT_DEG) v = -MAG_DECLINATION_LIMIT_DEG; + return v; + } + return MAG_DECLINATION_DEFAULT_DEG; +} + //////////////////////////////////////////////////////////////////////////////// //:::::RF POWER AMPLIFIER DAC5578 Emergency stop function using CLR pin///////// //////////////////////////////////////////////////////////////////////////////// @@ -1724,7 +1771,7 @@ int main(void) float magRawX = mx*cos(Pitch_Sensor*PI/180.0f) - mz*sin(Pitch_Sensor*PI/180.0f); float magRawY = mx*sin(Roll_Sensor*PI/180.0f)*sin(Pitch_Sensor*PI/180.0f) + my*cos(Roll_Sensor*PI/180.0f)- mz*sin(Roll_Sensor*PI/180.0f)*cos(Pitch_Sensor*PI/180.0f); - Yaw_Sensor = (180*atan2(magRawY,magRawX)/PI) - Mag_Declination; + Yaw_Sensor = (180*atan2(magRawY,magRawX)/PI) - get_mag_declination_deg(); /* MCU-A2 */ if(Yaw_Sensor<0)Yaw_Sensor+=360; diff --git a/9_Firmware/9_1_Microcontroller/tests/Makefile b/9_Firmware/9_1_Microcontroller/tests/Makefile index 8427206..69f81df 100644 --- a/9_Firmware/9_1_Microcontroller/tests/Makefile +++ b/9_Firmware/9_1_Microcontroller/tests/Makefile @@ -70,6 +70,7 @@ TESTS_STANDALONE := test_bug12_pa_cal_loop_inverted \ test_mcu_a7_emergency_persist \ test_mcu_a5_pa_cal_gate \ test_mcu_a6_recovery_dispatch \ + test_mcu_a2_mag_declination \ test_gap3_iwdg_config \ test_gap3_temperature_max \ test_gap3_idq_periodic_reread \ @@ -175,6 +176,9 @@ test_mcu_a5_pa_cal_gate: test_mcu_a5_pa_cal_gate.c test_mcu_a6_recovery_dispatch: test_mcu_a6_recovery_dispatch.c $(CC) $(CFLAGS) $< -o $@ +test_mcu_a2_mag_declination: test_mcu_a2_mag_declination.c + $(CC) $(CFLAGS) $< -lm -o $@ + # Gap-3 safety tests -- mock-only (needs spy log for GPIO sequence) test_gap3_emergency_stop_rails: test_gap3_emergency_stop_rails.c $(MOCK_OBJS) $(CC) $(CFLAGS) $(INCLUDES) $< $(MOCK_OBJS) -o $@ diff --git a/9_Firmware/9_1_Microcontroller/tests/test_mcu_a2_mag_declination.c b/9_Firmware/9_1_Microcontroller/tests/test_mcu_a2_mag_declination.c new file mode 100644 index 0000000..1938df4 --- /dev/null +++ b/9_Firmware/9_1_Microcontroller/tests/test_mcu_a2_mag_declination.c @@ -0,0 +1,137 @@ +/******************************************************************************* + * test_mcu_a2_mag_declination.c + * + * MCU-A2: magnetic declination was a hardcoded -0.61° literal baked in for + * one deployment site. Yaw_Sensor is wrong by (site_decl - (-0.61))° at + * every other site whenever GPS dual-antenna heading is unavailable. + * + * Production fix backs the value with a BKPSRAM slot and exposes a + * setter/getter pair. Default returns to the legacy -0.61° when no override + * has been written, preserving backward compatibility for the original + * site. Range is clamped to ±30° (real-world declinations are roughly + * -25° to +25°, so anything beyond is a calibration error). + * + * This test models the BKPSRAM slot, replays the setter/getter, and + * verifies clamping, persistence across "reset", default-on-empty, and + * defensive clamping if BKPSRAM is corrupted. + ******************************************************************************/ +#include +#include +#include +#include +#include +#include + +#define MAG_DECL_DEFAULT (-0.61f) +#define MAG_DECL_LIMIT (30.0f) +#define MAG_DECL_MAGIC 0xCAFEFACEU + +/* Models BKPSRAM slot for declination (magic + value). */ +static uint32_t g_magic; +static uint32_t g_value_bits; + +static void simulated_power_cycle(void) { g_magic = 0; g_value_bits = 0; } +static void simulated_corrupt(uint32_t magic, float value) { + g_magic = magic; + union { float f; uint32_t u; } cvt = { .f = value }; + g_value_bits = cvt.u; +} + +static void set_decl(float deg) { + if (deg > MAG_DECL_LIMIT) deg = MAG_DECL_LIMIT; + if (deg < -MAG_DECL_LIMIT) deg = -MAG_DECL_LIMIT; + union { float f; uint32_t u; } cvt = { .f = deg }; + g_value_bits = cvt.u; + g_magic = MAG_DECL_MAGIC; +} + +static float get_decl(void) { + if (g_magic == MAG_DECL_MAGIC) { + union { float f; uint32_t u; } cvt = { .u = g_value_bits }; + float v = cvt.f; + if (v > MAG_DECL_LIMIT) v = MAG_DECL_LIMIT; + if (v < -MAG_DECL_LIMIT) v = -MAG_DECL_LIMIT; + return v; + } + return MAG_DECL_DEFAULT; +} + +static bool feq(float a, float b) { return fabsf(a - b) < 0.001f; } + +int main(void) +{ + printf("=== MCU-A2: mag-declination BKPSRAM persistence + clamp ===\n"); + + /* 1. Empty BKPSRAM -> legacy default for backward compatibility. */ + printf(" Test 1: empty BKPSRAM returns default -0.61 ... "); + simulated_power_cycle(); + assert(feq(get_decl(), MAG_DECL_DEFAULT)); + printf("PASS\n"); + + /* 2. Setter writes; getter returns the written value. */ + printf(" Test 2: set 12.4 then get ... "); + set_decl(12.4f); + assert(feq(get_decl(), 12.4f)); + printf("PASS\n"); + + /* 3. Value persists across simulated reset (BKPSRAM survives). */ + printf(" Test 3: persists across reset ... "); + /* simulated reset = process state preserved (BKPSRAM survives) */ + assert(feq(get_decl(), 12.4f)); + printf("PASS\n"); + + /* 4. Power cycle clears BKPSRAM -> back to default. */ + printf(" Test 4: power-cycle restores default ... "); + simulated_power_cycle(); + assert(feq(get_decl(), MAG_DECL_DEFAULT)); + printf("PASS\n"); + + /* 5. Setter clamps high. */ + printf(" Test 5: set +45 clamps to +30 ... "); + set_decl(45.0f); + assert(feq(get_decl(), 30.0f)); + printf("PASS\n"); + + /* 6. Setter clamps low. */ + printf(" Test 6: set -45 clamps to -30 ... "); + set_decl(-45.0f); + assert(feq(get_decl(), -30.0f)); + printf("PASS\n"); + + /* 7. Plausible site values pass through unmodified. */ + printf(" Test 7: realistic site values pass through ... "); + const float sites[] = { -22.5f, -8.0f, -0.61f, 0.0f, 4.3f, 11.2f, 17.9f }; + for (size_t i = 0; i < sizeof(sites)/sizeof(sites[0]); i++) { + set_decl(sites[i]); + assert(feq(get_decl(), sites[i])); + } + printf("PASS\n"); + + /* 8. Defensive clamp on getter — if BKPSRAM is corrupted to an + * out-of-range value (VBAT brown-out, bit flip), getter still returns + * a safe value rather than propagating a wild offset. */ + printf(" Test 8: corrupt +1000 BKPSRAM clamps to +30 on read ... "); + simulated_corrupt(MAG_DECL_MAGIC, 1000.0f); + assert(feq(get_decl(), 30.0f)); + printf("PASS\n"); + + /* 9. Wrong magic -> default (corruption that doesn't preserve magic). */ + printf(" Test 9: wrong magic returns default ... "); + simulated_corrupt(0xDEADBEEFU, 5.0f); + assert(feq(get_decl(), MAG_DECL_DEFAULT)); + printf("PASS\n"); + + /* 10. Pre-fix regression: hardcoded -0.61 used at non-default site + * yields wrong heading by (site_decl - (-0.61))°. Confirm the fixed + * path returns the configured site value, eliminating the offset. */ + printf(" Test 10: post-fix yaw correction matches configured site ... "); + set_decl(11.2f); + float pre_fix_decl_used = -0.61f; + float post_fix_decl_used = get_decl(); + float bearing_error = post_fix_decl_used - pre_fix_decl_used; + assert(feq(bearing_error, 11.81f)); /* heading offset corrected */ + printf("(error %.2f degrees), PASS\n", (double)bearing_error); + + printf("\n=== MCU-A2: ALL TESTS PASSED ===\n\n"); + return 0; +}