feat(algo): B8 频率测量层 + 过频/低频保护（L3-5）
新增 App/algo/freq.c/h：由 L2 基波相量导出系统频率。相邻两拍共轭 乘积 P=Z(m)·conj(Z(m-1)) 取角 = 每拍相位增量（免显式解缠绕），再按 自适应窗长 K_eff=round(8·Fs/(2·f_hat))（45~55 Hz 段 73~89 拍）做滑动 平均。定长窗受 20 点矩形窗负序泄漏纹波限制（实测扫频最坏 0.317 Hz； 60 拍 Kay 复数和 0.197 Hz，交叉项和角不按 1/K^2 衰减），自适应窗使 窗长恰为纹波周期 fs/(2f) 整数倍 -> 最坏 0.0101 Hz。推导与两轮弯路 写在 freq.c 文件头。 假值钳制带取 35~65 Hz，刻意宽于系统合理带 40.5~60.5：单拍相位差本身 带 ±5 Hz 泄漏纹波，贴边钳制会非对称丢弃纹波半周 -> 均值偏置（实测 45 Hz 点 +0.16 Hz）。系统合理带由 OF/UF 定值范围在消费侧把关。 幅值门 FREQ_AMP_MIN=50 码（拟定，待 L1-5 标定复核）：低于门判无有效 波形，停更新、valid 清零、读数保持 -> 浮空/PT 断线不发散。源优先级 UA(C02)->IA(C01)，按名前缀解析、每拍动态回退，切换即废相位基准。 prot_set：尾追加 8 项（PS_ID_COUNT 32->40，编号不回收） OF_EN/OF_HZ/OF_DLY/UF_EN/UF_HZ/UF_DLY/RET_OF/RET_UF。OF/UF 逐项通讯 地址不在本仓库 -> 全部 comm_addr=0 本地项（104 C_SE 不可寻址，待对照 参考表补）。频率返回系数独立（0.99/1.01），勿与电气量 0.95/1.05 共用。 bad_mask 随项数扩为 uint64_t。PROT_SET_BLOB_VER 1->2。 prot_set_store：换代与损坏分流。load 返回 2 = 版本/项数换代（本文件 打 INFO "prot set blob version change, defaults in use"），-1 = 真损坏 （defaultTask 按 CORRUPT 报 ERROR + 自检留痕）。B7 旧档（ver=1/32 项） 升级首启属预期路径，不得报成损坏。语义分流收在属主文件内， default_task.c 不动。 prot_elm：实例表加 src 字段（0=phasor rms，1=freq），PE_OF/PE_UF 尾 追加（PE_COUNT 7->9）。freq 无有效读数按方向填"必不启动"值（过量 0 / 欠量 1e30 -> 滞回比较恒假 -> 元件复位），不整拍跳过——频率源缺失不得 饿死电流/电压类元件（B5 行为不回归）。OF/UF 暂未映射独立 104 遥信点， 动作只经 YX15 保护总聚合可见（送检点表 = B10）。 板端接线：adcTask 在 phasor_run() 之后同拍调 freq_run()；心跳/首拍日志 追加 freq(<tag>): f=%ld valid=%u (x0.01)（MicroLIB 无 %f）。app_main 内核前 freq_init()。 主机单测：freq 46 用例（J1 扫频 45~55 Hz <=0.02 Hz 实测最坏 0.0101； J2 噪声 1% <=0.10 实测 0.0160；J3 启动收敛 <=40 ms 实测 20；J4 阶跃 50->51 <=120 ms 实测 98；J5 浮空 valid=0 不发散；J6 跳变 60 ms 内回 ±0.05；另直流 8000 码 / 3-5 次谐波 20%/10% / UA->IA 回退）。 prot_set 90（+TC-PS-011 OF/UF 行 + 64 位 bad_mask bit39 + 旧档拒绝）、 prot_elm 47（+OF/UF 滞回与方向隔离 + run() 测频接线）。回归：phasor 57、iec104 94 全绿。 J4=120 ms 是"精度优先于响应"的显式取舍（Lead 批准）：4 周波窗换 0.317->0.0101 Hz，测频响应 98 ms << OF/UF 定时限默认 1000 ms（占 总动作时间 <10%），暂态另有 J6 <=60 ms 兜底。 注册：freq.c 入 uvprojx algo 组 + check_registration.py SIM_EXEMPT （纯算法、模拟器无消费路径，照 prot_set.c 先例）。 @build 0 错误 0 警告（Code=207468 RO=18304 RW=1068 ZI=153084）。 RAM：RW_IRAM1 96096->96728 B（+632 B，73.3%->73.8%）、RW_CCM 不变 57376 B（87.9%）、Flash 224596->226840 B（+2244 B，21.4%->21.6%）； 台账随合并由 Lead 回填。stack_report：adcTask 静态最坏链 1320 B / 余量 1240 B（48%）改前后完全不变（freq_run 帧未进最坏链，无需抬栈）； protectTask 链 872 B / 余 1176 B（57%）同样不变。 gate: only mem-budget/mem-numbers pending lead ledger backfill -------------------------------------------------------------- 门禁实跑（pre-commit run --all-files，Lead 批准 --no-verify 提交）： 28 Passed / 2 Failed，失败仅 mem-numbers 与 mem-budget 两条——均为 docs/内存管理 受管快照与本 .map 不一致（台账由 Lead 每次合并后 --write 回填，避免多分支同改受管表冲突）。其余含 doc-links / coding-standard / no-mojibake / cppcheck / registration-check / 13 条主机单测钩子全 Passed（gitlint 属 commit-msg 阶段，不在该次 --all-files 内）。ci_local L0.5 全绿（freq 46/46 等）。 字节卫生自查（--no-verify 跳过卫生钩子，故手工核）：17 个暂存文件 全部 BOM=无 / CR=0（纯 LF）/ U+FFFD=0；自研 .c/.h 去注释后字符串 字面量 100% ASCII（唯一非 ASCII 字面量在 test/host/iec104/iec104_test.c 的既有用例名，非本次改动行；test/ 不在 coding-standard 钩子范围）。
This commit is contained in:
+318
@@ -0,0 +1,318 @@
|
||||
/*
|
||||
* 模块名称：频率测量层（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;
|
||||
}
|
||||
Reference in New Issue
Block a user