STM32F4-Base/test/host/freq/freq_test.c

/*
 * 模块名称：频率测量层（L2 派生量，B8）主机侧单元测试
 * 模块功能：在 PC 上对 App/algo/freq.c + phasor.c + sample_ring.c 做离线验证。
 *
 * 被测对象：真实源文件（#include 原样纳入，纯算法无需桩）：
 *             ../../../App/algo/sample_ring.c
 *             ../../../App/algo/phasor.c
 *             ../../../App/algo/freq.c
 *
 * 精度判据（与 App/algo/README.md §3.7 同口径，规划 §4.2 测频范围的落实）：
 *   J1 纯音扫频 45.00~55.00 Hz（0.5 Hz 步进）：|f_meas - f_true| <= 0.02 Hz
 *       （实测最坏 0.0101 Hz @55.0 Hz，判据留 2 倍裕度）
 *   J2 噪声工况（幅值 1000 码、噪声 rms = 1% 幅值）：|f_meas - f_true| <= 0.10 Hz
 *       （实测最坏 0.016 Hz，判据留 ~6 倍裕度）
 *   J3 启动收敛：首个有效相量后 <= 40 ms 出有效读数
 *   J4 频率阶跃 50->51 Hz：<= 120 ms 进入 ±0.05 Hz 并保持（自适应窗
 *      K_eff≈73~89 拍，阶跃后窗冲刷决定物理下限；OF/UF 定时限 >=100 ms
 *      叠加其上，总动作时间仍 <220 ms，符合定时限保护常规）
 *   J5 零幅/浮空（rms < 幅值门）：valid=0、读数保持、不发散（无 NaN/无跳变；
 *      信号消失过渡段 <=20 ms 窗内残窗参与均值，读数漂移有界 <=0.3 Hz）
 *   J6 假值钳制：相位/幅值跳变拍产生的单拍折算越出 35~65 Hz 假值带 -> 丢弃，
 *      读数在跳变后 60 ms 内回到 ±0.05 Hz
 *
 * 覆盖点：
 *   TC-FR-001 50 Hz 收敛时序（J3）+ 稳态精度（J1 子集）
 *   TC-FR-002 扫频 45~55 Hz @0.5 Hz（J1，逐点判 + 最坏值打印）
 *   TC-FR-003 噪声鲁棒（J2，最坏值打印）
 *   TC-FR-004 频率阶跃 50->51（J4）
 *   TC-FR-005 零幅浮空不发散（J5）+ 恢复后重新收敛
 *   TC-FR-006 直流偏置抑制（全周傅氏滤直流 -> 测频不受影响）
 *   TC-FR-007 3/5 次谐波污染抑制（相量基波选择性）
 *   TC-FR-008 相位跳变假值钳制（J6）
 *   TC-FR-009 UA 无幅值 -> 自动落 IA 源
 *   TC-FR-010 init 幂等 / get 参数守卫 / 未收敛 -2
 *   TC-FR-011 幅值门中途失效 -> valid 清零 + 读数保持（J5 中途版）
 *
 * 通过：全部 TC PASS（main 返回 0）。
 * 创建日期：2026-10-12（B8）
 */

#include <math.h>
#include <stdint.h>
#include <stdio.h>

#define FR_PI   3.14159265358979323846
#define FR_FS   1000.0

/* ===================== 断言 ===================== */
static int s_total = 0, s_passed = 0, s_failed = 0;

#define CHECK(cond, name) do { \
    s_total++; \
    if (cond) { s_passed++; printf("  [PASS] %s\n", name); } \
    else      { s_failed++; printf("  [FAIL] %s  (line %d)\n", name, __LINE__); } \
} while (0)

/* ===================== 被测代码 ===================== */
#include "sample_ring.c"
#include "phasor.c"
#include "freq.c"

/* ===================== 波形驱动 ===================== */

/* 全局采样时刻（拍）：驱动函数按此生成"当前时刻"的样本，跨调用连续 */
static uint32_t s_t = 0u;

/* 确定性伪随机（LCG）：噪声可复现 */
static uint32_t s_rng = 0x12345678u;
static double s_noise_unit(void) {           /* [-0.5, 0.5) */
    s_rng = s_rng * 1103515245u + 12345u;
    return (double)((s_rng >> 8) & 0xFFFFFFu) / (double)0x1000000u - 0.5;
}

/*
 * 函数功能：合成一拍整帧并跑一遍 L2+测频链
 * 入口参数：f        - 信号频率（Hz）
 *           amp      - 基波峰值（码值）
 *           dc       - 直流偏置（码值）
 *           h3, h5   - 3/5 次谐波峰值（码值）
 *           noise_rms- 高斯样近似噪声 rms（码值，均匀分布近似）
 *           ua_en    - UA 通道是否带波形（0 = 只挂 0 码，测 IA 回退）
 * 返 回 值：无
 * 限定条件：无
 * 函数说明：9 路语义通道同波形（除 ua_en=0 时 UA 置 0）；每拍
 *          push_frame -> phasor_run -> freq_run，与 adcTask 同序。
 */
static void drive_beat(double f, double amp, double dc, double h3, double h5,
                       double noise_rms, uint8_t ua_en) {
    float frame[ADC_SEM_COUNT];
    double t = (double)s_t / FR_FS;
    double v = amp * sin(2.0 * FR_PI * f * t)
             + h3   * sin(2.0 * FR_PI * 3.0 * f * t)
             + h5   * sin(2.0 * FR_PI * 5.0 * f * t)
             + dc
             + s_noise_unit() * noise_rms * 1.7320508;   /* 均匀 -> rms 折算 */
    uint8_t c;

    for (c = 0u; c < (uint8_t)ADC_SEM_COUNT; c++) {
        frame[c] = (float)v;
    }
    if (ua_en == 0u) {
        frame[ADC_SEM_UA] = 0.0f;      /* UA 无波形（门失败），IA 仍有 */
    }
    if (f == 0.0) {
        for (c = 0u; c < (uint8_t)ADC_SEM_COUNT; c++) {
            frame[c] = 0.0f;           /* 浮空：全 0 帧 */
        }
    }
    (void)sample_ring_push_frame(frame, (uint8_t)ADC_SEM_COUNT);
    phasor_run();
    freq_run();
    s_t++;
}

/*
 * 函数功能：连续驱动 n 拍
 */
static void drive_n(uint32_t n, double f, double amp, double dc, double h3,
                    double h5, double noise_rms, uint8_t ua_en) {
    uint32_t i;

    for (i = 0u; i < n; i++) {
        drive_beat(f, amp, dc, h3, h5, noise_rms, ua_en);
    }
}

/*
 * 函数功能：复位整条链（采样时刻 + 三层状态）
 */
static void chain_reset(void) {
    s_t = 0u;
    s_rng = 0x12345678u;
    (void)phasor_init();   /* 连带 sample_ring_init */
    (void)freq_init();
}

/* ===================== TC-FR-001：50 Hz 收敛时序 + 稳态精度 ===================== */
static void tc_converge_50(void) {
    float f = 0.0f;
    uint32_t first_valid_t = 0u, phasor_ready_t = 0u;
    uint32_t i;
    int got = 0;

    chain_reset();
    for (i = 0u; i < 200u; i++) {
        drive_beat(50.0, 1000.0, 0.0, 0.0, 0.0, 0.0, 1u);
        if (phasor_ready_t == 0u) {
            phasor_t p;
            if (phasor_get(1u, &p) == 0) {
                phasor_ready_t = i;         /* 首个有效相量拍 */
            }
        }
        if (!got && (freq_valid() != 0u) && (freq_get(&f) == 0)) {
            first_valid_t = i;
            got = 1;
        }
    }
    CHECK(got, "TC-FR-001 freq becomes valid at 50 Hz");
    CHECK((first_valid_t >= phasor_ready_t)
          && ((first_valid_t - phasor_ready_t) <= 40u),
          "TC-FR-001 J3: first valid reading <= 40 ms after first phasor");
    (void)freq_get(&f);
    printf("  [DATA] 50 Hz steady: f=%.4f err=%.4f Hz (first valid @%u ms, phasor @%u ms)\n",
           (double)f, (double)f - 50.0, (unsigned)first_valid_t, (unsigned)phasor_ready_t);
    CHECK(fabsf(f - 50.0f) <= 0.01f, "TC-FR-001 J1(50Hz): |err| <= 0.01 Hz");
}

/* ===================== TC-FR-002：扫频 45~55 Hz @0.5 Hz（J1） ===================== */
static void tc_sweep(void) {
    double worst = 0.0, worst_f = 0.0;
    int ok = 1;
    int k;

    for (k = 0; k <= 20; k++) {
        double f_true = 45.0 + 0.5 * (double)k;
        float  f_meas = 0.0f;
        double err;

        chain_reset();
        drive_n(150u, f_true, 1000.0, 0.0, 0.0, 0.0, 0.0, 1u);
        CHECK(freq_get(&f_meas) == 0, "TC-FR-002 freq valid at each sweep point");
        err = fabs((double)f_meas - f_true);
        if (err > worst) { worst = err; worst_f = f_true; }
        if (err > 0.02) { ok = 0; }
    }
    printf("  [DATA] sweep 45.00~55.00 Hz step 0.5: worst |err| = %.4f Hz @ %.1f Hz (J1 <= 0.02)\n",
           worst, worst_f);
    CHECK(ok, "TC-FR-002 J1: |f_meas - f_true| <= 0.02 Hz over 45~55 Hz");
}

/* ===================== TC-FR-003：噪声鲁棒（J2） ===================== */
static void tc_noise(void) {
    double worst = 0.0;
    int ok = 1;
    int k;

    for (k = 0; k < 5; k++) {
        double f_true = 45.0 + 2.5 * (double)k;   /* 45/47.5/50/52.5/55 */
        float  f_meas = 0.0f;
        double err;

        chain_reset();
        drive_n(200u, f_true, 1000.0, 0.0, 0.0, 0.0, 10.0, 1u);  /* rms = 1% 幅值 */
        (void)freq_get(&f_meas);
        err = fabs((double)f_meas - f_true);
        if (err > worst) { worst = err; }
        if (err > 0.10) { ok = 0; }
    }
    printf("  [DATA] noise rms=1%% amp: worst |err| = %.4f Hz (J2 <= 0.10)\n", worst);
    CHECK(ok, "TC-FR-003 J2: |err| <= 0.10 Hz with 1% noise");
}

/* ===================== TC-FR-004：频率阶跃 50->51（J4） ===================== */
static void tc_step(void) {
    float f = 0.0f;
    uint32_t settle = 0u;
    uint32_t i;
    int ok = 1;

    chain_reset();
    drive_n(150u, 50.0, 1000.0, 0.0, 0.0, 0.0, 0.0, 1u);
    (void)freq_get(&f);
    printf("  [DATA] pre-step f=%.4f\n", (double)f);

    /* 阶跃拍 = t=150；判"进入 ±0.05 且保持" */
    for (i = 0u; i < 100u; i++) {
        drive_beat(51.0, 1000.0, 0.0, 0.0, 0.0, 0.0, 1u);
        (void)freq_get(&f);
        if (settle == 0u) {
            if (fabsf(f - 51.0f) <= 0.05f) {
                settle = i + 1u;            /* 阶跃后第 N 拍进入带内 */
            }
        } else if (fabsf(f - 51.0f) > 0.05f) {
            ok = 0;                          /* 进入后又出带 */
        }
    }
    printf("  [DATA] step 50->51: settle = %u ms, final f=%.4f err=%.4f\n",
           (unsigned)settle, (double)f, (double)f - 51.0);
    CHECK((settle != 0u) && (settle <= 120u), "TC-FR-004 J4: settle <= 120 ms");
    CHECK(ok, "TC-FR-004 J4: stays within +/-0.05 Hz after settle");
    CHECK(fabsf(f - 51.0f) <= 0.01f, "TC-FR-004 J1(51Hz): steady |err| <= 0.01 Hz");
}

/* ===================== TC-FR-005：零幅浮空不发散（J5） ===================== */
static void tc_zero_amp(void) {
    float f = 123.0f;
    uint32_t i;
    int never_valid = 1, no_nan = 1;

    chain_reset();
    for (i = 0u; i < 500u; i++) {
        drive_beat(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1u);   /* 全 0 帧（浮空 rms=0） */
        if (freq_valid() != 0u) { never_valid = 0; }
        (void)freq_get(&f);
        if (!(f == f)) { no_nan = 0; }                  /* NaN 自比较为假 */
    }
    CHECK(never_valid, "TC-FR-005 J5: zero-amplitude never asserts valid");
    CHECK(no_nan && (f == 0.0f), "TC-FR-005 J5: reading stays 0, no divergence/NaN");
    /* 恢复：重新注入波形后应再次收敛 */
    drive_n(150u, 49.0, 1000.0, 0.0, 0.0, 0.0, 0.0, 1u);
    (void)freq_get(&f);
    printf("  [DATA] recover after float: f=%.4f err=%.4f\n", (double)f, (double)f - 49.0);
    CHECK((freq_valid() != 0u) && (fabsf(f - 49.0f) <= 0.01f),
          "TC-FR-005 recovers and meets J1 after waveform returns");
}

/* ===================== TC-FR-006：直流偏置抑制 ===================== */
static void tc_dc_offset(void) {
    float f = 0.0f;

    chain_reset();
    drive_n(150u, 50.3, 1000.0, 8000.0, 0.0, 0.0, 0.0, 1u);   /* DC = 8000 码 */
    (void)freq_get(&f);
    printf("  [DATA] DC=8000: f=%.4f err=%.4f\n", (double)f, (double)f - 50.3);
    CHECK(fabsf(f - 50.3f) <= 0.01f, "TC-FR-006 DC offset rejected: |err| <= 0.01 Hz");
}

/* ===================== TC-FR-007：3/5 次谐波污染 ===================== */
static void tc_harmonics(void) {
    float f = 0.0f;

    chain_reset();
    drive_n(150u, 48.0, 1000.0, 0.0, 200.0, 100.0, 0.0, 1u);  /* 20%/10% 谐波 */
    (void)freq_get(&f);
    printf("  [DATA] h3=20%% h5=10%%: f=%.4f err=%.4f\n", (double)f, (double)f - 48.0);
    CHECK(fabsf(f - 48.0f) <= 0.01f, "TC-FR-007 harmonics rejected: |err| <= 0.01 Hz");
}

/* ===================== TC-FR-008：相位跳变假值钳制（J6） ===================== */
static void tc_phase_jump(void) {
    float f = 0.0f;
    uint32_t i;
    int ok = 1;

    chain_reset();
    drive_n(150u, 50.0, 1000.0, 0.0, 0.0, 0.0, 0.0, 1u);
    /* 跳变：瞬时切到相位偏移 60 度的等价频率段——用 1 拍 55 Hz 高幅制造
     * 窗内畸变（f_inst 出带），随后回 50 Hz，判 60 ms 内回带 */
    drive_beat(55.0, 4000.0, 0.0, 0.0, 0.0, 0.0, 1u);   /* 幅值/频率双跳变 */
    for (i = 0u; i < 60u; i++) {
        drive_beat(50.0, 1000.0, 0.0, 0.0, 0.0, 0.0, 1u);
        (void)freq_get(&f);
        if (i >= 59u) {
            if (fabsf(f - 50.0f) > 0.05f) { ok = 0; }
        }
    }
    printf("  [DATA] after jump: f=%.4f err=%.4f (recover window 60 ms)\n",
           (double)f, (double)f - 50.0);
    CHECK(ok, "TC-FR-008 J6: recovers to +/-0.05 Hz within 60 ms after jump");
    CHECK(freq_valid() != 0u, "TC-FR-008 J6: still valid after clamp path");
}

/* ===================== TC-FR-009：UA 无幅值 -> IA 回退 ===================== */
static void tc_src_fallback(void) {
    float f = 0.0f;

    chain_reset();
    drive_n(150u, 52.0, 1000.0, 0.0, 0.0, 0.0, 0.0, 0u);  /* UA 置 0，IA 带波形 */
    (void)freq_get(&f);
    printf("  [DATA] UA=0 IA=1000: f=%.4f err=%.4f\n", (double)f, (double)f - 52.0);
    CHECK((freq_valid() != 0u) && (fabsf(f - 52.0f) <= 0.01f),
          "TC-FR-009 falls back to IA source when UA below amplitude gate");
}

/* ===================== TC-FR-010：init 幂等 / 参数守卫 ===================== */
static void tc_init_guards(void) {
    float f = 5.0f;

    chain_reset();
    CHECK(freq_get(&f) == -2, "TC-FR-010 get before convergence -> -2");
    CHECK((f == 0.0f), "TC-FR-010 get -2 writes 0 to out");
    CHECK(freq_get(NULL) == -1, "TC-FR-010 NULL out -> -1");
    CHECK(freq_valid() == 0u, "TC-FR-010 valid=0 before convergence");
    CHECK(freq_last() == 0.0f, "TC-FR-010 last=0 before convergence");
    (void)freq_init();
    CHECK(freq_get(&f) == -2, "TC-FR-010 freq_init idempotent (still -2)");
}

/* ===================== TC-FR-011：幅值门中途失效 -> 保持 ===================== */
static void tc_gate_hold(void) {
    float f_before = 0.0f, f_after = 0.0f;

    chain_reset();
    drive_n(150u, 50.0, 1000.0, 0.0, 0.0, 0.0, 0.0, 1u);
    (void)freq_get(&f_before);
    CHECK(freq_valid() == 1u, "TC-FR-011 valid before gate loss");

    drive_n(50u, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1u);       /* 波形消失 */
    CHECK(freq_valid() == 0u, "TC-FR-011 J5: valid cleared on gate loss");
    (void)freq_get(&f_after);
    printf("  [DATA] hold: before=%.4f held=%.4f delta=%.4f\n",
           (double)f_before, (double)f_after, (double)(f_after - f_before));
    CHECK(fabsf(f_after - f_before) <= 0.3f,
          "TC-FR-011 J5: reading bounded (|delta| <= 0.3 Hz) during gate loss");
}

int main(void) {
    printf("=== Freq Tests (B8: phase-unwrap frequency measurement) ===\n");
    tc_converge_50();
    tc_sweep();
    tc_noise();
    tc_step();
    tc_zero_amp();
    tc_dc_offset();
    tc_harmonics();
    tc_phase_jump();
    tc_src_fallback();
    tc_init_guards();
    tc_gate_hold();

    printf("\n=== Freq Tests: %d/%d PASSED (failed=%d) ===\n",
           s_passed, s_total, s_failed);
    return (s_failed == 0) ? 0 : 1;
}