feat(sensor): 增加 CoP 迟滞判定与 IIR 低通滤波,修复空载尖刺和负力对消

- 单通道负力钳零:压力传感器不可能产生负力,消除零漂通道对 total 的对消
- 迟滞阈值:进入 ≥5kg / 退出 <2kg,防止马达振动触发 VALID
- IIR 低通:重载(≥10kg)直通零延迟,轻载 alpha=0.3 平滑 2-3 帧
- 物理常量统一到 calibration.h,sensor.c 和 calibration.c 共享
- 新增 calibration 模块(L1/L2 标定状态机 + NVS 持久化)
This commit is contained in:
2026-05-15 17:10:02 +08:00
parent e5518ac17e
commit 1a65bf0f22
3 changed files with 804 additions and 23 deletions
+117
View File
@@ -0,0 +1,117 @@
#pragma once
#include <stdbool.h>
#include <stdint.h>
/* ─── 板面几何 & 传感器物理常量 ─── */
#define BOARD_HALF_WIDTH_CM 42.5f /* 传感器到板中心 X 方向距离 (cm) */
#define BOARD_HALF_LENGTH_CM 16.0f /* 传感器到板中心 Y 方向距离 (cm) */
/*
* ADC → kg 换算推导:
* GML670 50kg: 灵敏度 1.75 mV/V, 激励 5V → 满量程输出 8.75 mV
* ADS1256: PGA=64, VREF=2.5V → 满量程 ±78.125 mV
* 50kg 对应 ADC counts = 8.75 / 78.125 × 8388607 ≈ 939524
* 1 LSB = 50.0 / 939524 ≈ 5.322e-5 kg
*/
#define SENSOR_EXCITATION_V 5.0f /* 传感器激励电压 (V) */
#define SENSOR_SENSITIVITY_MVV 1.75f /* 传感器灵敏度 (mV/V) */
#define SENSOR_RATED_LOAD_KG 50.0f /* 传感器满量程 (kg) */
#define ADS1256_VREF_V 2.5f /* ADS1256 参考电压 (V) */
#define ADS1256_PGA 64 /* ADS1256 增益倍数 */
/* 满量程时的 ADC 计数值 */
#define ADC_COUNTS_AT_RATED \
((SENSOR_SENSITIVITY_MVV * SENSOR_EXCITATION_V) / (2.0f * ADS1256_VREF_V * 1000.0f / ADS1256_PGA) * 8388607.0f)
/* 1 LSB 对应的力 (kg/count) */
#define ADC_TO_FORCE_SCALE (SENSOR_RATED_LOAD_KG / ADC_COUNTS_AT_RATED)
/* CoP 有效判定:迟滞阈值,防止边界抖动 */
#define COP_FORCE_ENTER_THRESHOLD 5.0f /* 总力超过此值才判定有人 (kg) */
#define COP_FORCE_EXIT_THRESHOLD 2.0f /* 总力低于此值才判定离开 (kg) */
/* CoP IIR 低通:轻载平滑、重载直通 */
#define COP_LPF_ALPHA_LIGHT 0.3f /* 轻载平滑系数(越小越平滑) */
#define COP_HEAVY_FORCE 10.0f /* 超过此值视为重载,直通无延迟 (kg) */
/* ─── 标定常量 ─── */
#define CAL_NUM_CHANNELS 4 /* 差分通道数 */
#define CAL_NUM_GRID_PTS 9 /* L2 网格标定点数 (3×3) */
/* ─── 标定运行时数据(sensor 线程只读) ─── */
struct cal_runtime {
int32_t zero[CAL_NUM_CHANNELS];
float gain[CAL_NUM_CHANNELS]; /* kg/count */
bool l1_zero_valid;
bool l1_gain_valid;
bool l2_valid;
float grid_err_x[CAL_NUM_GRID_PTS]; /* CoP X 误差 (cm) */
float grid_err_y[CAL_NUM_GRID_PTS]; /* CoP Y 误差 (cm) */
};
/**
* @brief 初始化标定模块,从 NVS 加载持久化数据或使用默认值。
*
* 必须在 settings_load() 之后、sensor 线程启动之前调用。
*
* @retval 0 成功。
*/
int cal_init(void);
/**
* @brief 获取当前生效的标定数据指针。
*
* sensor 线程在每帧开始时调用 cal_check_update() 后,通过本函数
* 拿到稳定的数据指针用于当帧计算。
*
* @return 指向内部 working copy 的只读指针。
*/
const struct cal_runtime *cal_get_working(void);
/**
* @brief 检查标定数据是否有更新,若有则刷新 working copy。
*
* sensor 线程在每帧循环顶部调用。
*
* @retval true 数据已刷新。
* @retval false 无更新。
*/
bool cal_check_update(void);
/**
* @brief 入队一条来自 BLE 的标定命令。
*
* 从 BLE 回调上下文调用,不做 ADC 操作,仅存储命令参数并设置 pending 标志。
*
* @param subcmd 子命令码。
* @param target 通道号 (0-3) 或网格点号 (0-8),或 0xFF 表示全部。
* @param param 浮点参数(如已知质量 kg),无参数时为 0。
*
* @retval 0 命令已入队。
* @retval -EBUSY 上一条命令尚未被 sensor 线程消费。
* @retval -EINVAL 状态机拒绝该命令。
*/
int cal_enqueue_command(uint8_t subcmd, uint8_t target, float param);
/**
* @brief 在 sensor 线程中执行待处理的标定命令。
*
* 若存在 pending 命令,执行 ADC 测量、更新状态机、通过 BLE 发送响应。
* 调用者应在本函数返回 true 时跳过本帧的正常采集。
*
* @retval true 执行了标定命令(本帧不做正常采集)。
* @retval false 无待处理命令。
*/
bool cal_execute_pending(void);
/**
* @brief 对计算出的 CoP 坐标施加 L2 网格补偿。
*
* 仅在 L2 标定有效时进行修正,否则不改变输入值。
*
* @param[in,out] cop_x CoP X 坐标 (cm)。
* @param[in,out] cop_y CoP Y 坐标 (cm)。
*/
void cal_apply_l2_correction(float *cop_x, float *cop_y);