STM32F4-Base/App/algo/freq.c

/*
 * 模块名称：频率测量层（L2 派生量）——实现
 * 模块功能：见 freq.h。相邻两拍基波相量共轭乘积取角（= 每拍相位增量，免
 *           显式解缠绕）+ **自适应窗长滑动平均** + 瞬时值钳制 + 幅值门。
 *           输入 = phasor 使用区基波相量（UA 优先，UA 无幅值用 IA）。
 *
 * 精度问题的根与解（2026-10-12 单测实测驱动，数据见 test/host/freq 输出）：
 *   非整周波窗长让 20 点矩形窗 DFT 混入**负序泄漏项** S2（45/55 Hz 处
 *   |S2/S1| ≈ 3~5%），相量端轨迹是椭圆 → 逐拍相位差带周期纹波
 *   δ(φ)，周期 = π/ω = fs/(2f) ≈ 9.1~11.1 拍。固定窗长平均时残差
 *   = [δ(m+K)-δ(m)]/K，K 不整除纹波周期 → 扫频最坏 0.2~0.32 Hz（两轮实测：
 *   固定 40 拍算术平均 0.317 Hz；固定 60 拍复数和 Kay 0.197 Hz——Kay 的
 *   交叉项和角同样不随 K² 衰减，勿再尝试）。
 *   **自适应窗长**：K_eff = round(M·Fs/(2·f_hat))（M=8，≈4 个泄漏纹波周期
 *   ≈4 个信号周波），使窗长恰为纹波周期的整数倍 → δ(m+K)≈δ(m)，残差进入
 *   0.01 Hz 量级。f_hat 用上一拍读数（收敛后误差 <0.1 Hz → K_eff 偏差 <1 拍，
 *   自洽）。
 *   **精度优先于响应的显式取舍（Lead 批准 2026-10-12）**：4 周波窗换来
 *   0.317→0.0101 Hz，代价是频率骤变后的窗冲刷（J4 阶跃整定 ≤120 ms，实测
 *   98 ms）。OF/UF 定时限默认 1000 ms → 测频响应占保护总动作时间 <10%；
 *   暂态假值另有 J6（跳变后 ≤60 ms 回 ±0.05 Hz）兜底。故 J4 的 120 ms 是
 *   工程整定，不是判据失守。
 *   另：单拍假值钳制带取 35~65 Hz（宽于系统合理带 40.5~60.5）——贴边钳制
 *   会非对称丢弃纹波半周 → 均值偏置（实测 45 Hz 点 +0.16 Hz，已修）。
 * 创建日期：2026-10-12（B8）
 */
#include "freq.h"

#include <math.h>
#include <stddef.h>

#include "phasor.h"

/* 圆周率（本层自带，避免依赖 math.h 的 M_* 扩展宏；ARMCC math.h 不保证定义） */
#define FREQ_PI   3.14159265358979f

/* 相位增量 -> 频率 换算系数：Fs/(2π)（Hz/rad） */
#define FREQ_HZ_PER_RAD   (FREQ_FS_HZ / (2.0f * FREQ_PI))

/* 自适应窗长目标纹波周期数 M：K_eff = round(M·Fs/(2·f_hat))。
 * 45~55 Hz 段 K_eff ∈ [73, 89] ⊂ [FREQ_AVG_MIN, FREQ_AVG_TAPS]。 */
#define FREQ_RIPPLE_M     8u

/* 源相量候选（优先级序）：UA 基波 = "C02"，IA 基波 = "C01"。
 * 按**名字前缀**解析索引（同 prot_elm 口径，不写死下标），开机解析一次。 */
#define FREQ_SRC_N   2
static const char *const k_src_prefix[FREQ_SRC_N] = { "C02", "C01" };

/* 测频内部状态（BSS ≈ 96*(4+1) + 常数 ≈ 520 B） */
static float    fq_d[FREQ_AVG_TAPS];     /* 每拍相位增量环（rad） */
static uint8_t  fq_d_ok[FREQ_AVG_TAPS];  /* 对应拍是否有效（假值拍 = 0） */
static uint8_t  fq_head;                 /* 环写指针 */
static uint8_t  fq_count;                /* 已入环样本数（饱和于 TAPS） */
static float    fq_last;                 /* 最近有效读数（Hz） */
static float    fq_prev_re;              /* 上一拍源相量（实部） */
static float    fq_prev_im;              /* 上一拍源相量（虚部） */
static uint8_t  fq_prev_valid;           /* 上一拍是否有相量（首拍建基准） */
static uint8_t  fq_src_idx[FREQ_SRC_N];  /* 候选实例索引；0xFF = 未找到 */
static uint8_t  fq_src_resolved;         /* 源已解析标志 */
static uint8_t  fq_prev_src;             /* 上一拍所用候选（切换即废基准） */
static uint8_t  fq_started;              /* 启动收敛完成（样本数 >= START_TAPS） */
static uint8_t  fq_gate_ok;              /* 本拍幅值门 OK 且已收敛 */

/*
 * 函数功能：解析候选源相量实例索引（开机一次）
 * 入口参数：无
 * 返 回 值：无（结果写 fq_src_idx[]，未找到 = 0xFF）
 * 限定条件：phasor_init() 之后（开机一次）
 * 函数说明：按实例名前缀匹配（"C02"=UA 基波 / "C01"=IA 基波）且 harm==1，
 *          与 prot_elm 同款朴素比较；**不写死实例下标**。
 */
static void fq_resolve_src(void) {
    uint8_t p;

    for (p = 0u; p < FREQ_SRC_N; p++) {
        uint8_t i, n = phasor_inst_count();

        fq_src_idx[p] = 0xFFu;
        for (i = 0u; i < n; i++) {
            const phasor_inst_t *pi = phasor_inst(i);
            uint8_t j = 0u, hit = 1u;

            if (pi == 0) {
                continue;
            }
            while ((k_src_prefix[p][j] != '\0') && (pi->name[j] != '\0')) {
                if (k_src_prefix[p][j] != pi->name[j]) {
                    hit = 0u;
                    break;
                }
                j++;
            }
            if ((hit != 0u) && (k_src_prefix[p][j] == '\0') && (pi->harm == 1u)) {
                fq_src_idx[p] = i;
                break;
            }
        }
    }
    fq_src_resolved = 1u;
}

/*
 * 函数功能：把相位增量入环
 * 入口参数：d - 本拍相位增量（rad）；ok - 本拍是否有效（钳制失败 = 0）
 * 返 回 值：无
 * 限定条件：无
 * 函数说明：假值拍也**占位**（ok=0），保持环时序 = 拍时序（自适应窗长
 *          的"窗 = 最近 K_eff 拍"语义依赖这一点）；求和时跳过占位拍。
 */
static void fq_push(float d, uint8_t ok) {
    fq_d[fq_head]    = d;
    fq_d_ok[fq_head] = ok;
    fq_head++;
    if (fq_head >= FREQ_AVG_TAPS) {
        fq_head = 0u;
    }
    if (fq_count < FREQ_AVG_TAPS) {
        fq_count++;
        if ((fq_started == 0u) && (fq_count >= FREQ_START_TAPS)) {
            fq_started = 1u;
        }
    }
}

/*
 * 函数功能：按自适应窗长求最近 K_eff 拍的相位增量均值 -> 频率
 * 入口参数：无
 * 返 回 值：频率读数（Hz）；窗内无有效样本时返回 fq_last（保持）
 * 限定条件：fq_started != 0
 * 函数说明：K_eff = clamp(round(M·Fs/(2·f_hat)), FREQ_AVG_MIN, FREQ_AVG_TAPS)
 *          （f_hat = 上一读数，首轮用标称 50 Hz 起步）。窗 = 最近 K_eff 拍
 *          （含占位拍，占位不计和但计窗推进），恰为泄漏纹波周期整数倍
 *          -> δ(m+K)≈δ(m) 相消。每拍重算和（<=96 次加法，us 级）。
 */
static float fq_read_freq(void) {
    float    f_hat = (fq_last > 30.0f) ? fq_last : 50.0f;
    uint16_t k_eff;
    uint16_t i;
    uint16_t idx;
    float    sum = 0.0f;
    uint16_t n_ok = 0u;

    /* K_eff = M*Fs/(2*f_hat)：四舍五入 + 钳到环容量区间 */
    k_eff = (uint16_t)(((float)FREQ_RIPPLE_M * FREQ_FS_HZ) / (2.0f * f_hat) + 0.5f);
    if (k_eff < FREQ_AVG_MIN) {
        k_eff = FREQ_AVG_MIN;
    }
    if (k_eff > FREQ_AVG_TAPS) {
        k_eff = FREQ_AVG_TAPS;
    }
    if (k_eff > (uint16_t)fq_count) {
        k_eff = (uint16_t)fq_count;
    }

    /* 从写指针回退 K_eff 拍求和（跳过占位拍） */
    idx = fq_head;
    for (i = 0u; i < k_eff; i++) {
        if (idx == 0u) {
            idx = FREQ_AVG_TAPS;
        }
        idx--;
        if (fq_d_ok[idx] != 0u) {
            sum += fq_d[idx];
            n_ok++;
        }
    }
    if (n_ok == 0u) {
        return fq_last;
    }
    return (sum / (float)n_ok) * FREQ_HZ_PER_RAD;
}

/*
 * 函数功能：频率测量层初始化（幂等）
 * 入口参数：无
 * 返 回 值：0 - 成功
 * 限定条件：内核启动前调用（无 RTOS 依赖）
 * 函数说明：清增量环/占位标志/计数/读数/相位基准/有效标志，并复位源解析。
 */
int freq_init(void) {
    uint8_t i;

    for (i = 0u; i < FREQ_AVG_TAPS; i++) {
        fq_d[i]    = 0.0f;
        fq_d_ok[i] = 0u;
    }
    fq_head        = 0u;
    fq_count       = 0u;
    fq_last        = 0.0f;
    fq_prev_re     = 0.0f;
    fq_prev_im     = 0.0f;
    fq_prev_valid  = 0u;
    fq_src_idx[0]  = 0xFFu;
    fq_src_idx[1]  = 0xFFu;
    fq_src_resolved = 0u;
    fq_prev_src    = 0u;
    fq_started     = 0u;
    fq_gate_ok     = 0u;
    return 0;
}

/*
 * 函数功能：每拍测频步进
 * 入口参数：无
 * 返 回 值：无
 * 限定条件：仅采集消费者任务上下文（adcTask），须在 phasor_run() 之后同拍调用
 * 函数说明：流程 = 选源（UA->IA 过幅值门）-> 共轭乘积 P = Z_now·conj(Z_prev)
 *          取角（免显式解缠绕）-> 钳制（假值拍占位 ok=0）-> 自适应窗均值。
 *          相量未就绪/全部候选门失败：本拍不更新（读数保持）、清 gate、废基准。
 *          基准在"断流"或**源切换**后重建（UA/IA 相位无共同基准）。
 */
void freq_run(void) {
    phasor_t ph = { 0.0f, 0.0f, 0.0f, 0.0f };
    float    pr;
    float    pi;
    float    dphi;
    float    f_inst;
    uint8_t  pick = FREQ_SRC_N;          /* FREQ_SRC_N = 无合格源 */
    uint8_t  p;

    if (fq_src_resolved == 0u) {
        fq_resolve_src();
    }

    /* 源选择（每拍动态）：优先级 UA(C02) -> IA(C01)，取第一个过幅值门的。
     * 台架只注电流时 UA 门失败 -> 自动落 IA；两路都无幅值 = 无有效波形。 */
    for (p = 0u; p < FREQ_SRC_N; p++) {
        if ((fq_src_idx[p] == 0xFFu) || (phasor_get(fq_src_idx[p], &ph) != 0)) {
            continue;
        }
        if (ph.fAmSQ >= FREQ_AMP_MIN) {
            pick = p;
            break;
        }
    }
    if (pick >= FREQ_SRC_N) {
        /* 相量未就绪或全部候选幅值门失败（浮空/断线 rms≈0 -> 相位无意义）：
         * 停更新、清 gate、废基准（读数保持） */
        fq_prev_valid = 0u;
        fq_gate_ok    = 0u;
        return;
    }

    if ((fq_prev_valid == 0u) || (pick != fq_prev_src)) {
        /* 首拍 / 断流后首拍 / 源切换：只建基准，不出数 */
        fq_prev_re    = ph.fRe;
        fq_prev_im    = ph.fIm;
        fq_prev_src   = pick;
        fq_prev_valid = 1u;
        fq_gate_ok    = 0u;
        return;
    }

    /* P = Z_now * conj(Z_prev)：arg(P) = 本拍相对上拍的相位增量
     * （纯音 = +2*pi*f/Fs；|P| 大 = 信号强，钳制用角不受幅值影响） */
    pr = (ph.fRe * fq_prev_re) + (ph.fIm * fq_prev_im);
    pi = (ph.fIm * fq_prev_re) - (ph.fRe * fq_prev_im);
    fq_prev_re = ph.fRe;
    fq_prev_im = ph.fIm;

    dphi   = atan2f(pi, pr);
    f_inst = dphi * FREQ_HZ_PER_RAD;

    /* 假值钳制：单拍折算越出 35~65 Hz（采样间隙窗滑 2 拍 -> ≈2f=90~110、
     * 相位跳变、深度畸变过渡段）-> 占位不入和，读数保持；基准已推进。
     * ⚠ 钳制带刻意宽于系统合理带（见 freq.h FREQ_INST_* 注释）：贴边会
     * 非对称丢弃泄漏纹波半周造成均值偏置。 */
    if ((f_inst < FREQ_INST_MIN_HZ) || (f_inst > FREQ_INST_MAX_HZ)) {
        fq_push(0.0f, 0u);
    } else {
        fq_push(dphi, 1u);
    }

    if (fq_started != 0u) {
        fq_last    = fq_read_freq();
        fq_gate_ok = 1u;
    } else {
        fq_gate_ok = 0u;
    }
}

/*
 * 函数功能：读测得频率（见 freq.h）
 * 入口参数：out - 输出（NULL -> -1）
 * 返 回 值：0 已有读数 / -1 参数错 / -2 从未启动收敛
 * 限定条件：任意上下文（float 单字读原子）
 */
int freq_get(float *out) {
    if (out == 0) {
        return -1;
    }
    if (fq_started == 0u) {
        *out = 0.0f;
        return -2;
    }
    *out = fq_last;
    return 0;
}

/*
 * 函数功能：读"本拍有效"标志（见 freq.h）
 * 入口参数：无
 * 返 回 值：1 新鲜 / 0 未启动或幅值门失效
 * 限定条件：无
 */
uint8_t freq_valid(void) {
    return fq_gate_ok;
}

/*
 * 函数功能：读最近一次有效读数（见 freq.h）
 * 入口参数：无
 * 返 回 值：Hz；从未收敛返回 0.0
 * 限定条件：无
 */
float freq_last(void) {
    return fq_last;
}