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 持久化）

src/calibration.c

@@ -0,0 +1,630 @@
+#include "calibration.h"
+#include "ads1256.h"
+#include "comm_protocol.h"
+
+#include <errno.h>
+#include <string.h>
+#include <zephyr/logging/log.h>
+#include <zephyr/settings/settings.h>
+#include <zephyr/sys/atomic.h>
+
+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;
+}

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 <string.h>
 #include <zephyr/kernel.h>
@@ -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) {