From cb758045f8856326b95f45b91aff27ba9ad18b3c Mon Sep 17 00:00:00 2001 From: pNexus Date: Mon, 11 May 2026 15:25:02 +0800 Subject: [PATCH] =?UTF-8?q?fix(sensor):=20=E6=9B=B4=E6=96=B0=E4=BC=A0?= =?UTF-8?q?=E6=84=9F=E5=99=A8=E5=8E=BB=E7=9A=AE=E9=80=BB=E8=BE=91=EF=BC=8C?= =?UTF-8?q?=E6=94=B9=E7=94=A8=E4=B8=AD=E5=80=BC=E6=BB=A4=E6=B3=A2=E4=BB=A5?= =?UTF-8?q?=E6=8F=90=E9=AB=98=E5=99=AA=E5=A3=B0=E6=8A=91=E5=88=B6=E6=95=88?= =?UTF-8?q?=E6=9E=9C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/sensor.c | 101 ++++++++++++++++++++++++++++++++++++++++----------- 1 file changed, 80 insertions(+), 21 deletions(-) diff --git a/src/sensor.c b/src/sensor.c index 6828661..fd9a738 100644 --- a/src/sensor.c +++ b/src/sensor.c @@ -11,10 +11,10 @@ LOG_MODULE_REGISTER(sensor, LOG_LEVEL_INF); /* - * 每通道每帧采 17 次取均值。3750 SPS 下： - * 首次切 MUX 建立 0.44ms + 后续 16 次各 0.27ms = 4.76ms/通道 - * 4 通道 = 19.0ms，20ms 帧周期内刚好用满 - * 17 次均值提供 √17 ≈ 4.12× 噪声抑制 (≈ 2.04 bit) + * 每通道每帧采 17 次取中值。3750 SPS 下： + * 17 次各 0.27ms = 4.86ms/通道 + * 4 通道 = 19.4ms，20ms 帧周期内刚好用满 + * 中值滤波对脉冲/尖峰干扰的抑制力强于均值 */ #define AVG_COUNT 17 #define DEADZONE_THRESHOLD 250 @@ -37,7 +37,7 @@ static const float sensor_y[4] = { }; /* --- 内部状态 --- */ -static int32_t sensor_offsets[4]; +static int32_t sensor_offsets[4]; // 去皮的零点偏移值 static int32_t filtered[4]; static atomic_t tare_requested; @@ -47,6 +47,16 @@ static atomic_t tare_requested; static K_THREAD_STACK_DEFINE(sensor_stack, SENSOR_STACK_SIZE); static struct k_thread sensor_thread; +/** + * @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++) { @@ -59,6 +69,13 @@ static void sort_array(int32_t *arr, int n) { } } +/** + * @brief 执行一次四路传感器去皮，更新每路零点偏移。 + * + * @param 无。 + * + * @return 无返回值。 + */ static void do_tare(void) { LOG_INF("Taring..."); int32_t sorted_buf[AVG_COUNT]; @@ -80,25 +97,42 @@ static void do_tare(void) { memset(filtered, 0, sizeof(filtered)); } +/** + * @brief 完成一帧四通道采集，并写入去皮后的滤波结果。 + * + * @param 无。 + * + * @return 无返回值。 + */ static void acquire_cycle(void) { - for (int ch = 0; ch < 4; ch++) { - int64_t sum = 0; + int32_t samples[AVG_COUNT]; + for (int ch = 0; ch < 4; ch++) { ads1256_write_reg(ADS1256_REG_MUX, mux_channels[ch]); ads1256_sync_wakeup(); for (int k = 0; k < AVG_COUNT; k++) { ads1256_wait_drdy(50); - sum += ads1256_read_data(); + samples[k] = ads1256_read_data(); } - int32_t avg = (int32_t)(sum / AVG_COUNT); - avg -= sensor_offsets[ch]; - if (avg > -DEADZONE_THRESHOLD && avg < DEADZONE_THRESHOLD) avg = 0; - filtered[ch] = avg; + 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; } } +/** + * @brief 根据四路受力结果计算压力中心（CoP）和总力。 + * + * @param[out] out_x 输出 CoP 的 X 坐标，单位由板级几何参数决定； + * @param[out] out_y 输出 CoP 的 Y 坐标；与 @p out_x 配套写回， + * @param[out] out_force 输出总力值； + * + * @retval true 总力高于有效阈值，当前 CoP 可用于对外发布。 + * @retval false 总力不足，CoP 被强制归零以避免在几乎无载荷时放大数值噪声。 + */ static bool compute_cop(float *out_x, float *out_y, float *out_force) { float forces[4]; float total = 0.0f; @@ -126,6 +160,15 @@ static bool compute_cop(float *out_x, float *out_y, float *out_force) { return true; } +/** + * @brief 传感器后台线程主循环，负责采集、去皮处理、CoP 计算和蓝牙发送。 + * + * @param p1 Zephyr 线程入口预留参数，当前未使用； + * @param p2 Zephyr 线程入口预留参数，当前未使用； + * @param p3 Zephyr 线程入口预留参数，当前未使用； + * + * @return 无返回值。 + */ static void sensor_thread_fn(void *p1, void *p2, void *p3) { (void)p1; (void)p2; @@ -167,28 +210,37 @@ static void sensor_thread_fn(void *p1, void *p2, void *p3) { /* 发送 */ ble_transport_send(pkt.bytes, sizeof(pkt.bytes)); - int64_t t2 = k_uptime_get(); /* 调试日志（5 Hz） */ if (++debug_log_divider >= 10) { debug_log_divider = 0; - float w[4]; - for (int i = 0; i < 4; i++) - w[i] = (float)filtered[i] * ADC_TO_FORCE_SCALE; LOG_INF( - "FR=%.2f BR=%.2f BL=%.2f FL=%.2f | total=%.2f kg | cop=(%.1f,%.1f) mm | %s", (double)w[0], (double)w[1], - (double)w[2], (double)w[3], (double)force, (double)cop_x, (double)cop_y, valid ? "VALID" : "low"); - LOG_INF( - "timing: acq=%lld ble=%lld total=%lld ms/frame", (long long)(t1 - t0), (long long)(t2 - t1), - (long long)frame_ms); + "FR=%.2f BR=%.2f BL=%.2f FL=%.2f | total=%.2f kg | cop=(%.1f,%.1f) mm | %s", (double)filtered[0], + (double)filtered[1], (double)filtered[2], (double)filtered[3], (double)force, (double)cop_x, + (double)cop_y, valid ? "VALID" : "low"); + LOG_INF("timing: acq=%lld total=%lld ms/frame", (long long)(t1 - t0), (long long)frame_ms); } } } +/** + * @brief 初始化传感器采集链路并启动后台采集线程。 + * + * @param 无。 + * + * @retval 0 初始化成功，传感器线程已经启动并完成一次初始去皮。 + * @retval 负值或其他错误码 ADS1256 初始化失败 + */ int sensor_init(void) { int err = ads1256_init(); if (err) return err; + /* SELFCAL 后 ADC 模拟链路需几个转换周期才能完全建立，空读排空 pipeline */ + for (int i = 0; i < 4; i++) { + ads1256_wait_drdy(50); + (void)ads1256_read_data(); + } + memset(sensor_offsets, 0, sizeof(sensor_offsets)); memset(filtered, 0, sizeof(filtered)); @@ -202,6 +254,13 @@ int sensor_init(void) { return 0; } +/** + * @brief 请求后台线程在下一帧边界执行去皮。 + * + * @param 无。 + * + * @return 无返回值。 + */ void sensor_perform_tare(void) { atomic_set(&tare_requested, 1); }