From 1a65bf0f224f08a9a509d5b64e5907c59ef3f4d5 Mon Sep 17 00:00:00 2001 From: pNexus Date: Fri, 15 May 2026 17:10:02 +0800 Subject: [PATCH] =?UTF-8?q?feat(sensor):=20=E5=A2=9E=E5=8A=A0=20CoP=20?= =?UTF-8?q?=E8=BF=9F=E6=BB=9E=E5=88=A4=E5=AE=9A=E4=B8=8E=20IIR=20=E4=BD=8E?= =?UTF-8?q?=E9=80=9A=E6=BB=A4=E6=B3=A2=EF=BC=8C=E4=BF=AE=E5=A4=8D=E7=A9=BA?= =?UTF-8?q?=E8=BD=BD=E5=B0=96=E5=88=BA=E5=92=8C=E8=B4=9F=E5=8A=9B=E5=AF=B9?= =?UTF-8?q?=E6=B6=88?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - 单通道负力钳零：压力传感器不可能产生负力，消除零漂通道对 total 的对消 - 迟滞阈值：进入 ≥5kg / 退出 <2kg，防止马达振动触发 VALID - IIR 低通：重载(≥10kg)直通零延迟，轻载 alpha=0.3 平滑 2-3 帧 - 物理常量统一到 calibration.h，sensor.c 和 calibration.c 共享 - 新增 calibration 模块（L1/L2 标定状态机 + NVS 持久化） --- inc/calibration.h | 117 +++++++++ src/calibration.c | 630 ++++++++++++++++++++++++++++++++++++++++++++++ src/sensor.c | 80 ++++-- 3 files changed, 804 insertions(+), 23 deletions(-) create mode 100644 inc/calibration.h create mode 100644 src/calibration.c diff --git a/inc/calibration.h b/inc/calibration.h new file mode 100644 index 0000000..14de0ef --- /dev/null +++ b/inc/calibration.h @@ -0,0 +1,117 @@ +#pragma once + +#include +#include + +/* ─── 板面几何 & 传感器物理常量 ─── */ + +#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); diff --git a/src/calibration.c b/src/calibration.c new file mode 100644 index 0000000..c2dce73 --- /dev/null +++ b/src/calibration.c @@ -0,0 +1,630 @@ +#include "calibration.h" +#include "ads1256.h" +#include "comm_protocol.h" + +#include +#include +#include +#include +#include + +LOG_MODULE_REGISTER(calibration, LOG_LEVEL_INF); + +#define CAL_SETTINGS_ROOT "cal" +#define CAL_SETTINGS_VER CAL_SETTINGS_ROOT "/ver" +#define CAL_SETTINGS_ZERO CAL_SETTINGS_ROOT "/zero" +#define CAL_SETTINGS_GAIN CAL_SETTINGS_ROOT "/gain" +#define CAL_SETTINGS_GRID_X CAL_SETTINGS_ROOT "/grid_x" +#define CAL_SETTINGS_GRID_Y CAL_SETTINGS_ROOT "/grid_y" +#define CAL_VERSION_L1_VALID BIT(0) +#define CAL_VERSION_L2_VALID BIT(1) +#define CAL_FLAG_PENDING 0 +#define CAL_FLAG_DIRTY 0 +#define CAL_AVG_COUNT 17 +#define CAL_GRID_X_LEFT (-28.33f) +#define CAL_GRID_X_MID (0.0f) +#define CAL_GRID_X_RIGHT (28.33f) +#define CAL_GRID_Y_BOTTOM (-10.67f) +#define CAL_GRID_Y_MID (0.0f) +#define CAL_GRID_Y_TOP (10.67f) + +enum cal_state { + CAL_STATE_IDLE = 0, + CAL_STATE_L1_IN_PROGRESS, + CAL_STATE_L1_READY, + CAL_STATE_L2_IN_PROGRESS, + CAL_STATE_L2_READY, +}; + +struct cal_pending_cmd { + uint8_t subcmd; + uint8_t target; + float param; +}; + +static struct cal_runtime committed; +static struct cal_runtime working; +static struct cal_pending_cmd pending_cmd; +static atomic_t pending_flags; +static atomic_t cal_flags; +static enum cal_state state = CAL_STATE_IDLE; +static const uint8_t mux_channels[CAL_NUM_CHANNELS] = { 0x01, 0x23, 0x45, 0x67 }; + +/** + * @brief 对采样数组做原地升序排序。 + * + * @param arr 待排序数组。 + * @param n 元素个数。 + * + * @return 无返回值。 + */ +static void sort_array(int32_t *arr, int n) { + for (int i = 0; i < n - 1; i++) { + for (int j = 0; j < n - i - 1; j++) { + if (arr[j] > arr[j + 1]) { + int32_t tmp = arr[j]; + arr[j] = arr[j + 1]; + arr[j + 1] = tmp; + } + } + } +} + +/** + * @brief 将一份运行时标定数据重置为默认值。 + * + * 默认值只保留“未标定但可运行”的最小语义：零点为 0，增益回落到 + * `ADC_TO_FORCE_SCALE`。这样擦除或首次上电后，系统仍能继续输出基础 CoP。 + * + * @param[out] runtime 待重置的标定数据。 + * + * @return 无返回值。 + */ +static void cal_reset_runtime(struct cal_runtime *runtime) { + memset(runtime, 0, sizeof(*runtime)); + for (int i = 0; i < CAL_NUM_CHANNELS; i++) { + runtime->gain[i] = ADC_TO_FORCE_SCALE; + } +} + +/** + * @brief 计算当前 committed 数据对应的版本位图。 + * + * @return 版本位图。 + */ +static uint32_t cal_make_version(void) { + uint32_t version = 0U; + + if (committed.l1_zero_valid && committed.l1_gain_valid) { + version |= CAL_VERSION_L1_VALID; + } + if (committed.l2_valid) { + version |= CAL_VERSION_L2_VALID; + } + + return version; +} + +/** + * @brief 发送标定响应帧。 + * + * `data[0]` 固定回显 `target`，`data[1]` 固定回显当前状态机状态，其余 6 字节 + * 由命令处理逻辑按上下文填充。这样上位机至少总能知道“哪条命令作用在谁身上， + * 执行后落在什么状态”，不需要依赖日志猜测固件内部阶段。 + * + * @param status 响应状态码。 + * @param subcmd 子命令码。 + * @param target 命令目标。 + * @param value4 4 字节上下文值。 + * @param extra2 2 字节补充值。 + * + * @return 无返回值。 + */ +static void cal_send_resp(uint8_t status, uint8_t subcmd, uint8_t target, uint32_t value4, uint16_t extra2) { + uint8_t data[8] = { 0 }; + + data[0] = target; + data[1] = (uint8_t)state; + memcpy(&data[2], &value4, sizeof(value4)); + memcpy(&data[6], &extra2, sizeof(extra2)); + comm_protocol_send_cal_resp(status, subcmd, data); +} + +/** + * @brief 读取指定通道的中值 ADC 原始计数。 + * + * 标定读数必须和正式采样走同一条 MUX/DRDY/中值链路，否则即便数学公式正确， + * 也会因为采样路径不一致而把系统误差带进标定结果。 + * + * @param ch 通道号，范围 0-3。 + * + * @return 中值 ADC 计数。 + */ +static int32_t cal_read_channel_median(uint8_t ch) { + int32_t samples[CAL_AVG_COUNT]; + + ads1256_write_reg(ADS1256_REG_MUX, mux_channels[ch]); + ads1256_sync_wakeup(); + + for (int i = 0; i < CAL_AVG_COUNT; i++) { + ads1256_wait_drdy(50); + samples[i] = ads1256_read_data(); + } + + sort_array(samples, CAL_AVG_COUNT); + return samples[CAL_AVG_COUNT / 2]; +} + +/** + * @brief 将网格点编号映射到 3x3 目标坐标。 + * + * 编号采用行优先顺序：`0..2` 为上排，`3..5` 为中排，`6..8` 为下排。 + * 这样坐标定义集中在一个函数里，后续若手机侧编号不同，只需要改这一处映射。 + * + * @param target 网格点编号。 + * @param[out] x 目标 X 坐标。 + * @param[out] y 目标 Y 坐标。 + * + * @return 无返回值。 + */ +static void cal_grid_target_to_xy(uint8_t target, float *x, float *y) { + uint8_t row = target / 3U; + uint8_t col = target % 3U; + + *x = (col == 0U) ? CAL_GRID_X_LEFT : ((col == 1U) ? CAL_GRID_X_MID : CAL_GRID_X_RIGHT); + *y = (row == 0U) ? CAL_GRID_Y_TOP : ((row == 1U) ? CAL_GRID_Y_MID : CAL_GRID_Y_BOTTOM); +} + +/** + * @brief 用当前 committed 的 L1 参数测一次 CoP。 + * + * L2 标定记录的是“当前测得的 CoP 与已知目标点之间的误差”，所以这里必须 + * 显式使用 committed 里的零点/增益，而不是 working 里旧版本的数据。 + * + * @param[out] cop_x 测得的 CoP X。 + * @param[out] cop_y 测得的 CoP Y。 + * + * @retval 0 测量成功。 + * @retval -ERANGE 总力过低，当前 CoP 无意义。 + */ +static int cal_measure_cop(float *cop_x, float *cop_y) { + static const float sensor_x[CAL_NUM_CHANNELS] = { + +BOARD_HALF_WIDTH_CM, + +BOARD_HALF_WIDTH_CM, + -BOARD_HALF_WIDTH_CM, + -BOARD_HALF_WIDTH_CM, + }; + static const float sensor_y[CAL_NUM_CHANNELS] = { + +BOARD_HALF_LENGTH_CM, + -BOARD_HALF_LENGTH_CM, + -BOARD_HALF_LENGTH_CM, + +BOARD_HALF_LENGTH_CM, + }; + float total = 0.0f; + float wx = 0.0f; + float wy = 0.0f; + + for (int i = 0; i < CAL_NUM_CHANNELS; i++) { + int32_t raw = cal_read_channel_median((uint8_t)i); + float force = (float)(raw - committed.zero[i]) * committed.gain[i]; + if (force < 0.0f) force = 0.0f; + + total += force; + wx += force * sensor_x[i]; + wy += force * sensor_y[i]; + } + + if (total < COP_FORCE_ENTER_THRESHOLD) { + *cop_x = 0.0f; + *cop_y = 0.0f; + return -ERANGE; + } + + *cop_x = wx / total; + *cop_y = wy / total; + return 0; +} + +/** + * @brief 将当前 committed 数据持久化到 NVS。 + * + * @retval 0 保存成功。 + * @retval 负值 settings 子系统返回的具体错误码。 + */ +static int cal_save_all(void) { + uint32_t version = cal_make_version(); + int err = settings_save_one(CAL_SETTINGS_VER, &version, sizeof(version)); + + if (err) return err; + err = settings_save_one(CAL_SETTINGS_ZERO, committed.zero, sizeof(committed.zero)); + if (err) return err; + err = settings_save_one(CAL_SETTINGS_GAIN, committed.gain, sizeof(committed.gain)); + if (err) return err; + err = settings_save_one(CAL_SETTINGS_GRID_X, committed.grid_err_x, sizeof(committed.grid_err_x)); + if (err) return err; + return settings_save_one(CAL_SETTINGS_GRID_Y, committed.grid_err_y, sizeof(committed.grid_err_y)); +} + +/** + * @brief 擦除所有标定键。 + * + * @retval 0 擦除成功。 + * @retval 负值 settings 子系统返回的具体错误码。 + */ +static int cal_delete_all(void) { + int err = settings_delete(CAL_SETTINGS_VER); + + if (err) return err; + err = settings_delete(CAL_SETTINGS_ZERO); + if (err) return err; + err = settings_delete(CAL_SETTINGS_GAIN); + if (err) return err; + err = settings_delete(CAL_SETTINGS_GRID_X); + if (err) return err; + return settings_delete(CAL_SETTINGS_GRID_Y); +} + +/** + * @brief settings 子树加载回调。 + * + * @param key `cal/` 后的子键名。 + * @param len 数据长度。 + * @param read_cb backend 读回调。 + * @param cb_arg backend 私有参数。 + * + * @retval 0 处理成功。 + * @retval 负值 读失败。 + */ +static int cal_settings_set(const char *key, size_t len, settings_read_cb read_cb, void *cb_arg) { + const char *next; + int rc; + + if (settings_name_steq(key, "ver", &next) && !next && len == sizeof(uint32_t)) { + uint32_t version = 0U; + rc = read_cb(cb_arg, &version, sizeof(version)); + if (rc >= 0) { + committed.l1_zero_valid = (version & CAL_VERSION_L1_VALID) != 0U; + committed.l1_gain_valid = (version & CAL_VERSION_L1_VALID) != 0U; + committed.l2_valid = (version & CAL_VERSION_L2_VALID) != 0U; + return 0; + } + return rc; + } + + if (settings_name_steq(key, "zero", &next) && !next && len == sizeof(committed.zero)) { + rc = read_cb(cb_arg, committed.zero, sizeof(committed.zero)); + return (rc < 0) ? rc : 0; + } + + if (settings_name_steq(key, "gain", &next) && !next && len == sizeof(committed.gain)) { + rc = read_cb(cb_arg, committed.gain, sizeof(committed.gain)); + return (rc < 0) ? rc : 0; + } + + if (settings_name_steq(key, "grid_x", &next) && !next && len == sizeof(committed.grid_err_x)) { + rc = read_cb(cb_arg, committed.grid_err_x, sizeof(committed.grid_err_x)); + return (rc < 0) ? rc : 0; + } + + if (settings_name_steq(key, "grid_y", &next) && !next && len == sizeof(committed.grid_err_y)) { + rc = read_cb(cb_arg, committed.grid_err_y, sizeof(committed.grid_err_y)); + return (rc < 0) ? rc : 0; + } + + return 0; +} + +SETTINGS_STATIC_HANDLER_DEFINE(calibration, CAL_SETTINGS_ROOT, NULL, cal_settings_set, NULL, NULL); + +/** + * @brief 初始化标定模块，从 NVS 加载持久化数据或使用默认值。 + * + * @retval 0 成功。 + */ +int cal_init(void) { + cal_reset_runtime(&committed); + cal_reset_runtime(&working); + atomic_clear(&pending_flags); + atomic_clear(&cal_flags); + state = CAL_STATE_IDLE; + + (void)settings_load_subtree(CAL_SETTINGS_ROOT); + + working = committed; + if (working.l2_valid) { + state = CAL_STATE_L2_READY; + } else if (working.l1_zero_valid && working.l1_gain_valid) { + state = CAL_STATE_L1_READY; + } + + LOG_INF("Calibration init: l1=%d l2=%d", working.l1_gain_valid, working.l2_valid); + return 0; +} + +/** + * @brief 获取当前生效的标定数据指针。 + * + * @return 指向内部 working copy 的只读指针。 + */ +const struct cal_runtime *cal_get_working(void) { + return &working; +} + +/** + * @brief 检查标定数据是否有更新，若有则刷新 working copy。 + * + * @retval true 数据已刷新。 + * @retval false 无更新。 + */ +bool cal_check_update(void) { + if (!atomic_test_and_clear_bit(&cal_flags, CAL_FLAG_DIRTY)) { + return false; + } + + working = committed; + return true; +} + +/** + * @brief 入队一条来自 BLE 的标定命令。 + * + * @param subcmd 子命令码。 + * @param target 通道号或网格点号。 + * @param param 浮点参数。 + * + * @retval 0 命令已入队。 + * @retval -EBUSY 上一条命令尚未被消费。 + * @retval -EINVAL 参数非法。 + */ +int cal_enqueue_command(uint8_t subcmd, uint8_t target, float param) { + switch (subcmd) { + case CAL_SUBCMD_TARE_CH: + case CAL_SUBCMD_GAIN_CH: + if (target >= CAL_NUM_CHANNELS) return -EINVAL; + break; + case CAL_SUBCMD_RECORD_GRID: + if (target >= CAL_NUM_GRID_PTS) return -EINVAL; + break; + default: + break; + } + + pending_cmd.subcmd = subcmd; + pending_cmd.target = target; + pending_cmd.param = param; + + if (atomic_test_and_set_bit(&pending_flags, CAL_FLAG_PENDING)) { + return -EBUSY; + } + + return 0; +} + +/** + * @brief 在 sensor 线程中执行待处理的标定命令。 + * + * @retval true 执行了标定命令。 + * @retval false 当前没有待处理命令。 + */ +bool cal_execute_pending(void) { + struct cal_pending_cmd cmd; + uint8_t status = CAL_STATUS_OK; + uint32_t value4 = 0U; + uint16_t extra2 = 0U; + + if (!atomic_test_and_clear_bit(&pending_flags, CAL_FLAG_PENDING)) { + return false; + } + + cmd = pending_cmd; + + switch (cmd.subcmd) { + case CAL_SUBCMD_START_L1: + committed = working; + committed.l1_zero_valid = false; + committed.l1_gain_valid = false; + committed.l2_valid = false; + memset(committed.grid_err_x, 0, sizeof(committed.grid_err_x)); + memset(committed.grid_err_y, 0, sizeof(committed.grid_err_y)); + state = CAL_STATE_L1_IN_PROGRESS; + break; + + case CAL_SUBCMD_TARE_CH: + if (state != CAL_STATE_L1_IN_PROGRESS) { + status = CAL_STATUS_ERR_STATE; + break; + } + committed.zero[cmd.target] = cal_read_channel_median(cmd.target); + memcpy(&value4, &committed.zero[cmd.target], sizeof(committed.zero[cmd.target])); + break; + + case CAL_SUBCMD_GAIN_CH: + if (state != CAL_STATE_L1_IN_PROGRESS) { + status = CAL_STATUS_ERR_STATE; + break; + } + if (cmd.param <= 0.0f) { + status = CAL_STATUS_ERR_PARAM; + break; + } + { + int32_t median = cal_read_channel_median(cmd.target); + int32_t delta = median - committed.zero[cmd.target]; + if (delta == 0) { + status = CAL_STATUS_ERR_PARAM; + break; + } + committed.gain[cmd.target] = cmd.param / (float)delta; + memcpy(&value4, &committed.gain[cmd.target], sizeof(committed.gain[cmd.target])); + } + break; + + case CAL_SUBCMD_COMMIT_L1: + if (state != CAL_STATE_L1_IN_PROGRESS) { + status = CAL_STATUS_ERR_STATE; + break; + } + committed.l1_zero_valid = true; + committed.l1_gain_valid = true; + committed.l2_valid = false; + if (cal_save_all() != 0) { + status = CAL_STATUS_ERR_NVS; + break; + } + state = CAL_STATE_L1_READY; + value4 = cal_make_version(); + atomic_set_bit(&cal_flags, CAL_FLAG_DIRTY); + break; + + case CAL_SUBCMD_ABORT: + committed = working; + state = working.l2_valid + ? CAL_STATE_L2_READY + : ((working.l1_zero_valid && working.l1_gain_valid) ? CAL_STATE_L1_READY : CAL_STATE_IDLE); + break; + + case CAL_SUBCMD_START_L2: + if (!(working.l1_zero_valid && working.l1_gain_valid)) { + status = CAL_STATUS_ERR_STATE; + break; + } + committed = working; + memset(committed.grid_err_x, 0, sizeof(committed.grid_err_x)); + memset(committed.grid_err_y, 0, sizeof(committed.grid_err_y)); + committed.l2_valid = false; + state = CAL_STATE_L2_IN_PROGRESS; + break; + + case CAL_SUBCMD_RECORD_GRID: + if (state != CAL_STATE_L2_IN_PROGRESS) { + status = CAL_STATUS_ERR_STATE; + break; + } + { + float measured_x; + float measured_y; + float target_x; + float target_y; + + if (cal_measure_cop(&measured_x, &measured_y) != 0) { + status = CAL_STATUS_ERR_PARAM; + break; + } + + cal_grid_target_to_xy(cmd.target, &target_x, &target_y); + committed.grid_err_x[cmd.target] = measured_x - target_x; + committed.grid_err_y[cmd.target] = measured_y - target_y; + memcpy(&value4, &committed.grid_err_x[cmd.target], sizeof(committed.grid_err_x[cmd.target])); + memcpy(&extra2, &committed.grid_err_y[cmd.target], sizeof(extra2)); + } + break; + + case CAL_SUBCMD_COMMIT_L2: + if (state != CAL_STATE_L2_IN_PROGRESS) { + status = CAL_STATUS_ERR_STATE; + break; + } + committed.l2_valid = true; + if (cal_save_all() != 0) { + committed.l2_valid = false; + status = CAL_STATUS_ERR_NVS; + break; + } + state = CAL_STATE_L2_READY; + value4 = cal_make_version(); + atomic_set_bit(&cal_flags, CAL_FLAG_DIRTY); + break; + + case CAL_SUBCMD_ERASE: + if (cal_delete_all() != 0) { + status = CAL_STATUS_ERR_NVS; + break; + } + cal_reset_runtime(&committed); + state = CAL_STATE_IDLE; + atomic_set_bit(&cal_flags, CAL_FLAG_DIRTY); + break; + + case CAL_SUBCMD_QUERY: + value4 = cal_make_version(); + extra2 = (uint16_t)((working.l1_zero_valid ? BIT(0) : 0U) | (working.l1_gain_valid ? BIT(1) : 0U) | + (working.l2_valid ? BIT(2) : 0U)); + break; + + default: + status = CAL_STATUS_ERR_PARAM; + break; + } + + cal_send_resp(status, cmd.subcmd, cmd.target, value4, extra2); + return true; +} + +/** + * @brief 对计算出的 CoP 坐标施加 L2 网格补偿。 + * + * @param[in,out] cop_x CoP X 坐标 (cm)。 + * @param[in,out] cop_y CoP Y 坐标 (cm)。 + * + * @return 无返回值。 + */ +void cal_apply_l2_correction(float *cop_x, float *cop_y) { + float x; + float y; + int ix; + int iy; + float x0; + float x1; + float y0; + float y1; + float tx; + float ty; + int idx00; + int idx10; + int idx01; + int idx11; + float ex0; + float ex1; + float ey0; + float ey1; + float err_x; + float err_y; + + if (!working.l2_valid || !cop_x || !cop_y) { + return; + } + + x = *cop_x; + y = *cop_y; + + if (x < CAL_GRID_X_LEFT) x = CAL_GRID_X_LEFT; + if (x > CAL_GRID_X_RIGHT) x = CAL_GRID_X_RIGHT; + if (y < CAL_GRID_Y_BOTTOM) y = CAL_GRID_Y_BOTTOM; + if (y > CAL_GRID_Y_TOP) y = CAL_GRID_Y_TOP; + + ix = (x <= CAL_GRID_X_MID) ? 0 : 1; + iy = (y <= CAL_GRID_Y_MID) ? 0 : 1; + + x0 = (ix == 0) ? CAL_GRID_X_LEFT : CAL_GRID_X_MID; + x1 = (ix == 0) ? CAL_GRID_X_MID : CAL_GRID_X_RIGHT; + y0 = (iy == 0) ? CAL_GRID_Y_BOTTOM : CAL_GRID_Y_MID; + y1 = (iy == 0) ? CAL_GRID_Y_MID : CAL_GRID_Y_TOP; + + tx = (x1 - x0) == 0.0f ? 0.0f : (x - x0) / (x1 - x0); + ty = (y1 - y0) == 0.0f ? 0.0f : (y - y0) / (y1 - y0); + + idx00 = (2 - iy) * 3 + ix; + idx10 = idx00 + 1; + idx01 = (1 - iy) * 3 + ix; + idx11 = idx01 + 1; + + ex0 = working.grid_err_x[idx00] + tx * (working.grid_err_x[idx10] - working.grid_err_x[idx00]); + ex1 = working.grid_err_x[idx01] + tx * (working.grid_err_x[idx11] - working.grid_err_x[idx01]); + ey0 = working.grid_err_y[idx00] + tx * (working.grid_err_y[idx10] - working.grid_err_y[idx00]); + ey1 = working.grid_err_y[idx01] + tx * (working.grid_err_y[idx11] - working.grid_err_y[idx01]); + err_x = ex0 + ty * (ex1 - ex0); + err_y = ey0 + ty * (ey1 - ey0); + + *cop_x = x - err_x; + *cop_y = y - err_y; +} diff --git a/src/sensor.c b/src/sensor.c index 1d5af24..6fd794d 100644 --- a/src/sensor.c +++ b/src/sensor.c @@ -1,7 +1,7 @@ #include "sensor.h" #include "ads1256.h" -#include "ble_transport.h" -#include "protocol.h" +#include "calibration.h" +#include "comm_protocol.h" #include #include @@ -17,7 +17,7 @@ LOG_MODULE_REGISTER(sensor, LOG_LEVEL_INF); * 中值滤波对脉冲/尖峰干扰的抑制力强于均值 */ #define AVG_COUNT 17 -#define DEADZONE_THRESHOLD 250 +#define DEADZONE_THRESHOLD 1000 /* MUX 通道配置：4 路差分 */ static const uint8_t mux_channels[4] = { 0x01, 0x23, 0x45, 0x67 }; @@ -128,26 +128,45 @@ static void acquire_cycle(void) { * * @param[out] out_x 输出 CoP 的 X 坐标 (cm)； * @param[out] out_y 输出 CoP 的 Y 坐标 (cm)； - * @param[out] out_force 输出总力值 (kg)； + * @param[out] out_force 输出总力值 (kg)，int16 截断； + * @param[out] out_total 输出 float 精度总力，供 IIR alpha 判断用。 * * @retval true 总力高于有效阈值，当前 CoP 可用于对外发布。 * @retval false 总力不足，CoP 被强制归零以避免在几乎无载荷时放大数值噪声。 */ -static bool compute_cop(int16_t *out_x, int16_t *out_y, int16_t *out_force) { +static bool compute_cop(int16_t *out_x, int16_t *out_y, int16_t *out_force, float *out_total) { + static bool force_valid_state = false; + + const struct cal_runtime *cal = cal_get_working(); float forces[4]; float total = 0.0f; for (int i = 0; i < 4; i++) { - forces[i] = (float)filtered[i] * ADC_TO_FORCE_SCALE; + float gain = cal->l1_gain_valid ? cal->gain[i] : ADC_TO_FORCE_SCALE; + forces[i] = (float)filtered[i] * gain; + /* 压力传感器不可能产生负力，负值是零漂 */ + if (forces[i] < 0.0f) forces[i] = 0.0f; total += forces[i]; } *out_force = (int16_t)total; + *out_total = total; - if (total < COP_MIN_FORCE_THRESHOLD) { - *out_x = 0; - *out_y = 0; - return false; + /* 迟滞判定：避免阈值附近反复切换 */ + if (!force_valid_state) { + if (total < COP_FORCE_ENTER_THRESHOLD) { + *out_x = 0; + *out_y = 0; + return false; + } + force_valid_state = true; + } else { + if (total < COP_FORCE_EXIT_THRESHOLD) { + force_valid_state = false; + *out_x = 0; + *out_y = 0; + return false; + } } float wx = 0.0f, wy = 0.0f; @@ -182,6 +201,11 @@ static void sensor_thread_fn(void *p1, void *p2, void *p3) { int64_t frame_ms = now - frame_ts; frame_ts = now; + cal_check_update(); + if (cal_execute_pending()) { + continue; + } + /* 去皮请求 */ if (atomic_cas(&tare_requested, 1, 0)) { do_tare(); @@ -192,22 +216,32 @@ static void sensor_thread_fn(void *p1, void *p2, void *p3) { acquire_cycle(); int64_t t1 = k_uptime_get(); - /* CoP + 打包 */ + /* CoP 计算 + IIR 平滑 + 发送 */ + static float cop_x_lpf = 0.0f; + static float cop_y_lpf = 0.0f; + int16_t cop_x, cop_y, force; - bool valid = compute_cop(&cop_x, &cop_y, &force); + float total_f; + bool valid = compute_cop(&cop_x, &cop_y, &force, &total_f); + uint8_t flags = valid ? COP_FLAG_FORCE_VALID : 0; + if (valid) { + float fx = (float)cop_x; + float fy = (float)cop_y; - union cop_pkt_t pkt; - pkt.frame.sync0 = PROTO_SYNC0; - pkt.frame.sync1 = PROTO_SYNC1; - pkt.frame.type = PROTO_TYPE_COP; - pkt.frame.flags = valid ? COP_FLAG_FORCE_VALID : 0; - pkt.frame.cop_x = cop_x; - pkt.frame.cop_y = cop_y; - pkt.frame.force = force; - pkt.frame.crc = crc8(pkt.bytes + 2, sizeof(pkt.bytes) - 3, 0x31, 0x00, true); + cal_apply_l2_correction(&fx, &fy); - /* 发送 */ - ble_transport_send(pkt.bytes, sizeof(pkt.bytes)); + /* 重载直通、轻载平滑 */ + float alpha = (total_f >= COP_HEAVY_FORCE) ? 1.0f : COP_LPF_ALPHA_LIGHT; + cop_x_lpf = alpha * fx + (1.0f - alpha) * cop_x_lpf; + cop_y_lpf = alpha * fy + (1.0f - alpha) * cop_y_lpf; + + cop_x = (int16_t)cop_x_lpf; + cop_y = (int16_t)cop_y_lpf; + } else { + cop_x_lpf = 0.0f; + cop_y_lpf = 0.0f; + } + comm_protocol_send_cop(flags, cop_x, cop_y, force); /* 调试日志（5 Hz） */ if (++debug_log_divider >= 10) {