Huge restructurisation of folders

Author: DanielKusyDev

firmware/app/config.h

@@ -47,6 +47,8 @@
 
 // Complementary Filter
 #define CF_ALPHA           0.98f   // 98% gyro, 2% accel
+#define ACC_MAG_MIN        0.75f   // Minimum accel magnitude to trust (g)
+#define ACC_MAG_MAX        1.25f   // Maximum accel magnitude to trust (g)
 
 // Arming
 #define ARM_RAMP_RATE      2.0f    // Arm ramp rate (1/s)

firmware/core/control/attitude.cpp

@@ -50,11 +50,21 @@ void attitude_update(float rollRate_dps, float pitchRate_dps,
         // Gyro integration prediction
         float roll_pred = currentAngles.roll_deg + rollRate_dps * dt_s;
         float pitch_pred = currentAngles.pitch_deg + pitchRate_dps * dt_s;
-        
-        // IMU: Keep CF_ALPHA = 0.98f
-        currentAngles.roll_deg = CF_ALPHA * roll_pred + (1.0f - CF_ALPHA) * roll_acc_lpf;
-        currentAngles.pitch_deg = CF_ALPHA * pitch_pred + (1.0f - CF_ALPHA) * pitch_acc_lpf;
-        
+
+        // Magnitude gating: only apply accel correction if magnitude is close to 1g
+        // This prevents drift during free-fall, high-G maneuvers, or vibrations
+        float acc_magnitude = sqrtf(ax_g*ax_g + ay_g*ay_g + az_g*az_g);
+
+        if (acc_magnitude >= ACC_MAG_MIN && acc_magnitude <= ACC_MAG_MAX) {
+            // Normal complementary filter with accel correction
+            currentAngles.roll_deg = CF_ALPHA * roll_pred + (1.0f - CF_ALPHA) * roll_acc_lpf;
+            currentAngles.pitch_deg = CF_ALPHA * pitch_pred + (1.0f - CF_ALPHA) * pitch_acc_lpf;
+        } else {
+            // Pure gyro integration (accel is unreliable)
+            currentAngles.roll_deg = roll_pred;
+            currentAngles.pitch_deg = pitch_pred;
+        }
+
         // Normalize angles to [-180, +180] range
         currentAngles.roll_deg = math::normalize_angle_deg(currentAngles.roll_deg);
         currentAngles.pitch_deg = math::normalize_angle_deg(currentAngles.pitch_deg);

platformio.ini

@@ -87,8 +87,10 @@ build_flags = ${common.build_flags}
 test_build_src = yes
 build_src_filter =
     +<core/*>                 ; Include core logic (logger_stub.cpp is here)
+    +<stubs/*>                ; Include core logic (logger_stub.cpp is here)
     -<platform/>              ; Exclude all platform-specific code
     -<app/>                   ; Exclude application code
+    -<logger.cpp/>            ; Exclude real logger implementation
 lib_deps =
     doctest/doctest@^2.4.11
     FabioBatSilva/ArduinoFake@^0.3.1

tests/stubs/config_cal_stub.cpp

@@ -0,0 +1,17 @@
+/**
+ * @file config_cal_stub.cpp
+ * @brief Stub implementation of calibration configuration for native testing
+ */
+
+#include "config_cal.h"
+
+// Default calibration configuration
+CalConfig g_cal_cfg = {
+    .samples = 300,
+    .sample_rate_hz = 200.0f,
+    .max_rate_rms_dps = 2.0f,
+    .acc_mag_min_g = 0.95f,
+    .acc_mag_max_g = 1.05f,
+    .max_age_days = 30,
+    .cal_timeout_ms = 10000
+};

tests/stubs/imu_mapping_stub.cpp

@@ -0,0 +1,101 @@
+/**
+ * @file imu_mapping_stub.cpp
+ * @brief Stub implementation of IMU mapping for native testing
+ */
+
+#include "imu_mapping.h"
+#include <cstdio>
+
+// Predefined IMU orientation mappings
+const AxisMapping IMU_ORIENT_STANDARD = {
+    .axis = {0, 1, 2},
+    .sign = {1, -1, 1},
+    .name = "Standard (0°)"
+};
+
+const AxisMapping IMU_ORIENT_ROTATED_90 = {
+    .axis = {1, 0, 2},
+    .sign = {-1, 1, 1},
+    .name = "Rotated 90° CW"
+};
+
+const AxisMapping IMU_ORIENT_ROTATED_180 = {
+    .axis = {0, 1, 2},
+    .sign = {-1, -1, 1},
+    .name = "Rotated 180°"
+};
+
+const AxisMapping IMU_ORIENT_ROTATED_270 = {
+    .axis = {1, 0, 2},
+    .sign = {1, -1, 1},
+    .name = "Rotated 270° CW"
+};
+
+const AxisMapping IMU_ORIENT_INVERTED = {
+    .axis = {0, 1, 2},
+    .sign = {-1, 1, -1},
+    .name = "Inverted (upside down)"
+};
+
+// Current active mapping
+static AxisMapping g_current_mapping = IMU_ORIENT_STANDARD;
+static bool g_mapping_initialized = false;
+
+void imu_map_vector(const float sensor[3], float body[3]) {
+    if (!g_mapping_initialized) {
+        fprintf(stderr, "ERROR: IMU mapping not initialized\n");
+        return;
+    }
+
+    for (int i = 0; i < 3; i++) {
+        int axis_idx = g_current_mapping.axis[i];
+        body[i] = g_current_mapping.sign[i] * sensor[axis_idx];
+    }
+}
+
+bool imu_validate_mapping(const AxisMapping& mapping) {
+    // Validate axis indices
+    for (int i = 0; i < 3; i++) {
+        if (mapping.axis[i] < 0 || mapping.axis[i] > 2) {
+            return false;
+        }
+    }
+
+    // Validate sign multipliers
+    for (int i = 0; i < 3; i++) {
+        if (mapping.sign[i] != 1 && mapping.sign[i] != -1) {
+            return false;
+        }
+    }
+
+    // Validate bijective mapping (each axis used once)
+    bool axis_used[3] = {false, false, false};
+    for (int i = 0; i < 3; i++) {
+        int axis = mapping.axis[i];
+        if (axis_used[axis]) {
+            return false;
+        }
+        axis_used[axis] = true;
+    }
+
+    // Validate name
+    if (mapping.name == nullptr) {
+        return false;
+    }
+
+    return true;
+}
+
+bool imu_set_orientation(const AxisMapping& orientation) {
+    if (!imu_validate_mapping(orientation)) {
+        return false;
+    }
+
+    g_current_mapping = orientation;
+    g_mapping_initialized = true;
+    return true;
+}
+
+const AxisMapping& imu_get_orientation() {
+    return g_current_mapping;
+}

tests/stubs/nvs_cal_stub.cpp

@@ -0,0 +1,59 @@
+/**
+ * @file nvs_cal_stub.cpp
+ * @brief Stub implementation of NVS calibration for native testing
+ */
+
+#include "nvs_cal.h"
+#include <cmath>
+
+// In-memory calibration storage for testing
+static AccelCalData g_stored_cal = {
+    .ax_off = 0.0f,
+    .ay_off = 0.0f,
+    .az_off = 0.0f,
+    .timestamp = 0,
+    .version = 1
+};
+static bool g_cal_exists = false;
+
+// Maximum reasonable offset magnitude (in g)
+static const float MAX_OFFSET_G = 0.2f;
+
+bool nvs_cal_init() {
+    return true;
+}
+
+bool nvs_load_accel_cal(AccelCalData& out_data) {
+    if (!g_cal_exists) {
+        return false;
+    }
+    out_data = g_stored_cal;
+    return true;
+}
+
+bool nvs_save_accel_cal(const AccelCalData& cal_data) {
+    g_stored_cal = cal_data;
+    g_cal_exists = true;
+    return true;
+}
+
+bool nvs_has_fresh_accel_cal(uint32_t max_age_days) {
+    if (!g_cal_exists) {
+        return false;
+    }
+    return nvs_validate_accel_cal(g_stored_cal);
+}
+
+bool nvs_validate_accel_cal(const AccelCalData& cal_data) {
+    // Validate offset bounds
+    if (std::abs(cal_data.ax_off) > MAX_OFFSET_G) return false;
+    if (std::abs(cal_data.ay_off) > MAX_OFFSET_G) return false;
+    if (std::abs(cal_data.az_off) > MAX_OFFSET_G) return false;
+
+    // Check for NaN/Infinity
+    if (!std::isfinite(cal_data.ax_off)) return false;
+    if (!std::isfinite(cal_data.ay_off)) return false;
+    if (!std::isfinite(cal_data.az_off)) return false;
+
+    return true;
+}