STM32F4-Base/App/algo/phasor.c

/*
 * 模块名称：相量计算层（L2 相量计算）
 * 模块功能：全周傅氏算法（单频 DFT）——系数表预乘 √2/N，fAmSQ 直接即有效值。
 *          覆盖 L2-1 系数表 / L2-2 实例 / L2-3 有效值 / L2-6 滑动窗 / L2-7 双区交换。
 * 适用平台：STM32F407ZGTx（Cortex-M4F，硬件 FPU）
 * 作者：王建锋
 * 创建日期：2026-10-10
 * 修改记录：
 *   v1.0  2026-10-10  王建锋  创建：L2-1/2/3/6/7
 */

#include "phasor.h"

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

#include "sample_ring.h"

/* ===== L2-1 全周傅氏系数表 =====
 * g_fFFTSin20[k] = sin(2πk/N)·(√2/N)
 * g_fFFTCos20[k] = cos(2πk/N)·(√2/N)      N = PHASOR_N = 20
 * 权威口径：docs/算法/傅氏算法详解.md §2.3（"把 √2 藏进系数表"）。
 * 本表为**离线生成**的 float 常量（不在运行期调 sinf/cosf）：确定、无 libm 精度依赖。
 * 主机侧单测 test/host/phasor/ 会**逐项按上式复核本表**（防转录错），
 * 并以合成正弦做端到端复核（读数误差 ≤ 1%）。
 * 精确零点（sin(π)=0、cos(π/2)=0）直接写 0.0f，不保留浮点噪声。 */
static const float k_fft_sin[PHASOR_N] = {
    +0.000000000e+00f, +2.185080200e-02f, +4.156269506e-02f, +5.720613897e-02f, +6.724984944e-02f,
    +7.071068138e-02f, +6.724984944e-02f, +5.720613897e-02f, +4.156269506e-02f, +2.185080200e-02f,
    +0.000000000e+00f, -2.185080200e-02f, -4.156269506e-02f, -5.720613897e-02f, -6.724984944e-02f,
    -7.071068138e-02f, -6.724984944e-02f, -5.720613897e-02f, -4.156269506e-02f, -2.185080200e-02f,
};

static const float k_fft_cos[PHASOR_N] = {
    +7.071068138e-02f, +6.724984944e-02f, +5.720613897e-02f, +4.156269506e-02f, +2.185080200e-02f,
    +0.000000000e+00f, -2.185080200e-02f, -4.156269506e-02f, -5.720613897e-02f, -6.724984944e-02f,
    -7.071068138e-02f, -6.724984944e-02f, -5.720613897e-02f, -4.156269506e-02f, -2.185080200e-02f,
    +0.000000000e+00f, +2.185080200e-02f, +4.156269506e-02f, +5.720613897e-02f, +6.724984944e-02f,
};

/* √2：幅值 = √2 × 有效值（与系数表的 √2 折算互逆） */
#define PHASOR_SQRT2   1.4142135623730951f

/* ===== L2-2 相量实例表 =====
 * 参考项目为 **4 个实例**（C01 基波 I/U、C02 零序基波、C03 二次谐波、C04 五次谐波，
 * `APP/CLS100_Dsp/InstCalCell.c`）。本项目按 V0.1 **有效通道**（`UA`/`UN`/`IA`，
 * 见 docs/算法/继电保护功能复现规划.md §1.6：`UB/UC/IB/IC/IN` 无中点偏置 → 削顶不可用）展开——C01 的"I/U"
 * 拆为两行（IA 电流 / UA 电压），并补出 C04 的五次谐波轨（L1-9）。
 * 新增实例只需在表尾加一行，其余代码自动适配。 */
static const phasor_inst_t k_inst[] = {
    { "C01-IA-F1", ADC_SEM_IA, 1u },    /* A 相电流基波（L3-1 过流 / L3-3 过负荷） */
    { "C02-UA-F1", ADC_SEM_UA, 1u },    /* A 相电压基波（L3-4 过压/欠压、L3-5 频率） */
    { "C03-UN-F1", ADC_SEM_UN, 1u },    /* 零序电压基波（L3-6 零序过压） */
    { "C04-IA-F2", ADC_SEM_IA, 2u },    /* 二次谐波（L3-7 涌流制动） */
    { "C05-IA-F5", ADC_SEM_IA, 5u },    /* 五次谐波（L1-9 选线） */
};

#define PHASOR_INST_N   (sizeof(k_inst) / sizeof(k_inst[0]))

/* ===== L2-7 双区（计算区 / 使用区）=====
 * 本拍结果先写入"计算区"，全部实例写完后**一次性**切换 s_use_zone
 * （单字节 store，Cortex-M 上原子），消费侧读"使用区" → 无锁、无撕裂。
 * s_use_zone 声明 volatile：阻止编译器把结果写入挪到切换之后
 * （本平台单核顺序执行，无需额外内存屏障）。
 * 写者 = adcTask（同拍）；读者跨两个上下文：adcTask 心跳与 protectTask 判据
 * （prot_elm_run → phasor_get）→ 无锁读正是为后者服务，勿改成加锁或去掉双区。 */
static phasor_t         s_zone[2][PHASOR_INST_N];
static volatile uint8_t s_use_zone;
static uint8_t          s_ready;    /* 使用区是否已产出过有效结果 */

/*
 * 函数功能：清零一个相量
 * 入口参数：p - 目标相量
 * 返 回 值：无
 * 限定条件：p 非空
 * 函数说明：静态辅助。
 */
static void phasor_zero(phasor_t *p) {
    p->fRe   = 0.0f;
    p->fIm   = 0.0f;
    p->fAm   = 0.0f;
    p->fAmSQ = 0.0f;
}

/*
 * 函数功能：对 n 点窗口算 harm 次相量（L2-1 系数表 + L2-3 有效值）
 * 入口参数：x    - 采样窗（按时间序，旧 → 新）
 *           n    - 窗长（必须等于 PHASOR_N）
 *           harm - 谐波次数（>=1）
 *           out  - 输出相量
 * 返 回 值：无
 * 限定条件：无
 * 函数说明：纯函数。索引步进 = 谐波次数，故同一张表可算任意次谐波。
 */
void phasor_calc(const float *x, uint8_t n, uint8_t harm, phasor_t *out) {
    float   re = 0.0f;
    float   im = 0.0f;
    uint8_t i;

    if ((x == NULL) || (out == NULL) || (n != (uint8_t)PHASOR_N) || (harm == 0u)) {
        return;
    }

    for (i = 0u; i < n; i++) {
        uint8_t idx = (uint8_t)(((unsigned int)i * harm) % n);
        re += x[i] * k_fft_sin[idx];
        im += x[i] * k_fft_cos[idx];
    }

    out->fRe   = re;
    out->fIm   = im;
    out->fAmSQ = sqrtf((re * re) + (im * im));   /* 系数已折算 √2/N → 直接即有效值 */
    out->fAm   = out->fAmSQ * PHASOR_SQRT2;      /* 幅值 = √2 × 有效值 */
}

/*
 * 函数功能：相量层初始化
 * 入口参数：无
 * 返 回 值：0 - 成功
 * 限定条件：内核启动前调用
 * 函数说明：清双区结果与有效标志，并复位采样环缓冲；可重复调用（幂等）。
 */
int phasor_init(void) {
    uint8_t i;

    for (i = 0u; i < (uint8_t)PHASOR_INST_N; i++) {
        phasor_zero(&s_zone[0][i]);
        phasor_zero(&s_zone[1][i]);
    }
    s_use_zone = 0u;
    s_ready    = 0u;
    sample_ring_init();
    return 0;
}

/*
 * 函数功能：取相量实例数（L2-2）
 * 入口参数：无
 * 返 回 值：实例个数
 * 限定条件：无
 * 函数说明：供遍历 / 日志打印。
 */
uint8_t phasor_inst_count(void) {
    return (uint8_t)PHASOR_INST_N;
}

/*
 * 函数功能：取相量实例描述
 * 入口参数：idx - 实例下标
 * 返 回 值：指向静态实例表的指针；idx 越界返回 NULL
 * 限定条件：无
 * 函数说明：返回静态存储地址，调用方不得修改。
 */
const phasor_inst_t *phasor_inst(uint8_t idx) {
    if (idx >= (uint8_t)PHASOR_INST_N) {
        return NULL;
    }
    return &k_inst[idx];
}

/*
 * 函数功能：重算全部实例 + 双区原子切换（L2-6 / L2-7）
 * 入口参数：无
 * 返 回 值：无
 * 限定条件：仅任务上下文调用；须先 sample_ring_push_frame() 压入本拍数据
 * 函数说明：任一实例数据不足一周波 → 本拍整体不产出（不切换使用区），
 *          避免出现"部分新部分旧"的混合结果。
 */
void phasor_run(void) {
    float   win[PHASOR_N];
    uint8_t w = (uint8_t)(1u - s_use_zone);   /* 计算区 = 非使用区 */
    uint8_t i;

    for (i = 0u; i < (uint8_t)PHASOR_INST_N; i++) {
        if (sample_ring_window(k_inst[i].sem, (uint8_t)PHASOR_N, win) != 0) {
            return;   /* 尚未攒满一周波：本拍不产出 */
        }
        phasor_calc(win, (uint8_t)PHASOR_N, k_inst[i].harm, &s_zone[w][i]);
    }

    s_use_zone = w;   /* 原子切换（单字节 store） */
    s_ready    = 1u;
}

/*
 * 函数功能：读实例结果（使用区，无锁）
 * 入口参数：idx - 实例下标
 *           out - 输出相量
 * 返 回 值：0 - 成功；-1 - 参数错；-2 - 尚无有效结果
 * 限定条件：无
 * 函数说明：先取一次使用区下标再索引，避免读写期间下标变化。
 */
int phasor_get(uint8_t idx, phasor_t *out) {
    uint8_t z;

    if ((out == NULL) || (idx >= (uint8_t)PHASOR_INST_N)) {
        return -1;
    }
    if (s_ready == 0u) {
        return -2;
    }
    z    = s_use_zone;
    *out = s_zone[z][idx];
    return 0;
}