pNexus
a2d6ef8893
- 采样次数 17→9 提升帧率至 ~100Hz（Nyquist 50Hz） - 移除 CoP 级自适应 IIR，改为通道级二阶 Butterworth（fc=20Hz） - 保留 8-10Hz 振动板信号（通过率 98%），抑制高频噪声 - 2:1 降采样实现 50Hz BLE 输出 - 迟滞阈值 5/2→3/1 kg，死区 1000→250 counts - 新增 EMI 检测：采集异常短时触发寄存器恢复
125 lines
4.3 KiB
C
125 lines
4.3 KiB
C
#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 3.0f /* 总力超过此值才判定有人 (kg) */
|
||
#define COP_FORCE_EXIT_THRESHOLD 1.0f /* 总力低于此值才判定离开 (kg) */
|
||
|
||
/*
|
||
* 通道级二阶 Butterworth 低通：fc=20Hz, fs=100Hz
|
||
* 10Hz 通过 98%，20Hz -3dB，25Hz -6.6dB
|
||
* 保留 8-10Hz 振动板信号，同时为 50Hz 输出抗混叠
|
||
*/
|
||
#define LPF_B0 0.2065720838f
|
||
#define LPF_B1 0.4131441677f
|
||
#define LPF_B2 0.2065720838f
|
||
#define LPF_A1 (-0.3695273774f)
|
||
#define LPF_A2 0.1958157127f
|
||
|
||
/* ─── 标定常量 ─── */
|
||
|
||
#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);
|