pNexus/balance-board
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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

Browse Source 
- 采样次数 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 检测：采集异常短时触发寄存器恢复
This commit is contained in:
pNexus
2026-05-19 14:50:10 +08:00
parent d95cce5cf2
commit a2d6ef8893
2 changed files with 64 additions and 43 deletions
Download Patch File Download Diff File Expand all files Collapse all files
inc/calibration.h
+12 -5
View File
@@ -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
+47 -33
View File
@@ -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,12 +163,10 @@ 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_force = 0;
*out_x = 0;
*out_y = 0;
return false;
@@ -163,12 +175,15 @@ static bool compute_cop(int16_t *out_x, int16_t *out_y, int16_t *out_force, floa
} 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) {
@@ -215,33 +231,31 @@ 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 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);
}
/* 调试日志（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);
}
}
}