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:
+57
-23
@@ -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) {
|
||||
|
||||
Reference in New Issue
Block a user