diff --git a/inc/calibration.h b/inc/calibration.h index 14de0ef..9e4d1c7 100644 --- a/inc/calibration.h +++ b/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 /* ─── 标定常量 ─── */ diff --git a/src/sensor.c b/src/sensor.c index 6fd794d..9015aac 100644 --- a/src/sensor.c +++ b/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); } } }