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 持久化)
This commit is contained in:
2026-05-15 17:10:02 +08:00
parent e5518ac17e
commit 1a65bf0f22
3 changed files with 804 additions and 23 deletions
+57 -23
View File
@@ -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) {