refactor(sensor): 重构采样策略，Butterworth 20Hz 滤波 + 50Hz 输出

- 采样次数 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 检测：采集异常短时触发寄存器恢复

inc/calibration.h

@@ -5,7 +5,7 @@
 
 /* ─── 板面几何 & 传感器物理常量 ─── */
 
-#define BOARD_HALF_WIDTH_CM  42.5f  /* 传感器到板中心 X 方向距离 (cm) */
+#define BOARD_HALF_WIDTH_CM  42.5f /* 传感器到板中心 X 方向距离 (cm) */
 #define BOARD_HALF_LENGTH_CM 16.0f /* 传感器到板中心 Y 方向距离 (cm) */
 
 /*
@@ -28,12 +28,19 @@
 #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) */
+#define COP_FORCE_ENTER_THRESHOLD 3.0f /* 总力超过此值才判定有人 (kg) */
+#define COP_FORCE_EXIT_THRESHOLD  1.0f /* 总力低于此值才判定离开 (kg) */
 
-/* CoP IIR 低通：轻载平滑、重载直通 */
-#define COP_LPF_ALPHA_LIGHT 0.3f  /* 轻载平滑系数（越小越平滑） */
-#define COP_HEAVY_FORCE     10.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
 
 /* ─── 标定常量 ─── */
 

src/sensor.c

@@ -1,5 +1,6 @@
 #include "sensor.h"
 #include "ads1256.h"
+#include "ble_transport.h"
 #include "calibration.h"
 #include "comm_protocol.h"
 
@@ -11,13 +12,13 @@
 LOG_MODULE_REGISTER(sensor, LOG_LEVEL_INF);
 
 /*
- * 每通道每帧采 17 次取中值。3750 SPS 下：
- *    17 次各 0.27ms = 4.86ms/通道
- *   4 通道 = 19.4ms，20ms 帧周期内刚好用满
- * 中值滤波对脉冲/尖峰干扰的抑制力强于均值
+ * 每通道每帧采 9 次取中值。3750 SPS 下：
+ *    9 次各 0.27ms = 2.4ms/通道
+ *   4 通道 = 9.7ms，帧率 ~100Hz
+ * 提高帧率将奈奎斯特频率抬至 ~50Hz，避免低速马达振动混叠
  */
-#define AVG_COUNT          17
-#define DEADZONE_THRESHOLD 1000
+#define AVG_COUNT          9
+#define DEADZONE_THRESHOLD 250
 
 /* MUX 通道配置：4 路差分 */
 static const uint8_t mux_channels[4] = { 0x01, 0x23, 0x45, 0x67 };
@@ -37,8 +38,11 @@ static const float sensor_y[4] = {
 };
 
 /* --- 内部状态 --- */
-static int32_t sensor_offsets[4]; // 去皮的零点偏移值
+static int32_t sensor_offsets[4];
 static int32_t filtered[4];
+/* 二阶 Butterworth 滤波器状态 (Direct Form II Transposed) */
+static float lp_z1[4]; /* 延迟节点 1 */
+static float lp_z2[4]; /* 延迟节点 2 */
 static atomic_t tare_requested;
 
 /* --- 线程 --- */
@@ -95,6 +99,8 @@ static void do_tare(void) {
         "Tare offsets=[%ld,%ld,%ld,%ld]", (long)sensor_offsets[0], (long)sensor_offsets[1], (long)sensor_offsets[2],
         (long)sensor_offsets[3]);
     memset(filtered, 0, sizeof(filtered));
+    memset(lp_z1, 0, sizeof(lp_z1));
+    memset(lp_z2, 0, sizeof(lp_z2));
 }
 
 /**
@@ -112,14 +118,23 @@ static void acquire_cycle(void) {
         ads1256_sync_wakeup();
 
         for (int k = 0; k < AVG_COUNT; k++) {
-            ads1256_wait_drdy(50);
+            int drdy_err = ads1256_wait_drdy(50);
+            if (drdy_err) {
+                LOG_WRN("ch%d sample%d DRDY timeout", ch, k);
+            }
             samples[k] = ads1256_read_data();
         }
 
         sort_array(samples, AVG_COUNT);
         int32_t median = samples[AVG_COUNT / 2] - sensor_offsets[ch];
         if (median > -DEADZONE_THRESHOLD && median < DEADZONE_THRESHOLD) median = 0;
-        filtered[ch] = median;
+
+        /* 二阶 Butterworth 低通 (Direct Form II Transposed) */
+        float x      = (float)median;
+        float y      = LPF_B0 * x + lp_z1[ch];
+        lp_z1[ch]    = LPF_B1 * x - LPF_A1 * y + lp_z2[ch];
+        lp_z2[ch]    = LPF_B2 * x - LPF_A2 * y;
+        filtered[ch] = (int32_t)y;
     }
 }
 
@@ -128,13 +143,12 @@ static void acquire_cycle(void) {
  *
  * @param[out] out_x 输出 CoP 的 X 坐标 (cm)；
  * @param[out] out_y 输出 CoP 的 Y 坐标 (cm)；
- * @param[out] out_force 输出总力值 (kg)，int16 截断；
- * @param[out] out_total 输出 float 精度总力，供 IIR alpha 判断用。
+ * @param[out] out_force 输出总力值 (kg)。
  *
  * @retval true 总力高于有效阈值，当前 CoP 可用于对外发布。
  * @retval false 总力不足，CoP 被强制归零以避免在几乎无载荷时放大数值噪声。
  */
-static bool compute_cop(int16_t *out_x, int16_t *out_y, int16_t *out_force, float *out_total) {
+static bool compute_cop(int16_t *out_x, int16_t *out_y, int16_t *out_force) {
     static bool force_valid_state = false;
 
     const struct cal_runtime *cal = cal_get_working();
@@ -149,26 +163,27 @@ static bool compute_cop(int16_t *out_x, int16_t *out_y, int16_t *out_force, floa
         total += forces[i];
     }
 
-    *out_force = (int16_t)total;
-    *out_total = total;
-
     /* 迟滞判定：避免阈值附近反复切换 */
     if (!force_valid_state) {
         if (total < COP_FORCE_ENTER_THRESHOLD) {
-            *out_x = 0;
-            *out_y = 0;
+            *out_force = 0;
+            *out_x     = 0;
+            *out_y     = 0;
             return false;
         }
         force_valid_state = true;
     } else {
         if (total < COP_FORCE_EXIT_THRESHOLD) {
             force_valid_state = false;
+            *out_force        = 0;
             *out_x            = 0;
             *out_y            = 0;
             return false;
         }
     }
 
+    *out_force = (int16_t)total;
+
     float wx = 0.0f, wy = 0.0f;
     for (int i = 0; i < 4; i++) {
         wx += forces[i] * sensor_x[i];
@@ -194,6 +209,7 @@ static void sensor_thread_fn(void *p1, void *p2, void *p3) {
     (void)p3;
 
     uint8_t debug_log_divider = 0;
+    uint8_t send_divider      = 0;
     int64_t frame_ts          = k_uptime_get();
 
     while (1) {
@@ -214,34 +230,32 @@ static void sensor_thread_fn(void *p1, void *p2, void *p3) {
         /* 采集 */
         int64_t t0 = k_uptime_get();
         acquire_cycle();
-        int64_t t1 = k_uptime_get();
+        int64_t t1  = k_uptime_get();
+        int64_t acq = t1 - t0;
 
-        /* CoP 计算 + IIR 平滑 + 发送 */
-        static float cop_x_lpf = 0.0f;
-        static float cop_y_lpf = 0.0f;
+        /* EMI 自恢复：acq 异常短说明寄存器被干扰改写 */
+        if (acq < 10) {
+            LOG_ERR("EMI detected (acq=%lld ms)", (long long)acq);
+            ads1256_recover(10);
+            continue;
+        }
 
+        /* CoP 计算 + 发送 */
         int16_t cop_x, cop_y, force;
-        float total_f;
-        bool valid    = compute_cop(&cop_x, &cop_y, &force, &total_f);
+        bool valid    = compute_cop(&cop_x, &cop_y, &force);
         uint8_t flags = valid ? COP_FLAG_FORCE_VALID : 0;
         if (valid) {
             float fx = (float)cop_x;
             float fy = (float)cop_y;
-
             cal_apply_l2_correction(&fx, &fy);
-
-            /* 重载直通、轻载平滑 */
-            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;
+            cop_x = (int16_t)fx;
+            cop_y = (int16_t)fy;
+        }
+        /* 2:1 降采样：内部 ~100Hz 计算，50Hz 输出 */
+        if (++send_divider >= 2) {
+            send_divider = 0;
+            comm_protocol_send_cop(flags, cop_x, cop_y, force);
         }
-        comm_protocol_send_cop(flags, cop_x, cop_y, force);
 
         /* 调试日志（5 Hz） */
         if (++debug_log_divider >= 10) {
@@ -250,7 +264,7 @@ static void sensor_thread_fn(void *p1, void *p2, void *p3) {
                 "FR=%.2f\t BR=%.2f\t BL=%.2f\t FL=%.2f\t | total=%d kg | cop=(%d,%d) cm | %s", (double)filtered[0],
                 (double)filtered[1], (double)filtered[2], (double)filtered[3], force, cop_x, cop_y,
                 valid ? "VALID" : "low");
-            LOG_INF("timing: acq=%lld total=%lld ms/frame", (long long)(t1 - t0), (long long)frame_ms);
+            LOG_INF("timing: acq=%lld frame=%lld ms", (long long)(t1 - t0), (long long)frame_ms);
         }
     }
 }