pNexus
1a65bf0f22
- 单通道负力钳零：压力传感器不可能产生负力，消除零漂通道对 total 的对消 - 迟滞阈值：进入 ≥5kg / 退出 <2kg，防止马达振动触发 VALID - IIR 低通：重载(≥10kg)直通零延迟，轻载 alpha=0.3 平滑 2-3 帧 - 物理常量统一到 calibration.h，sensor.c 和 calibration.c 共享 - 新增 calibration 模块（L1/L2 标定状态机 + NVS 持久化）
118 lines
4.2 KiB
C
118 lines
4.2 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 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);
|