pNexus
1a65bf0f22
- 单通道负力钳零：压力传感器不可能产生负力，消除零漂通道对 total 的对消 - 迟滞阈值：进入 ≥5kg / 退出 <2kg，防止马达振动触发 VALID - IIR 低通：重载(≥10kg)直通零延迟，轻载 alpha=0.3 平滑 2-3 帧 - 物理常量统一到 calibration.h，sensor.c 和 calibration.c 共享 - 新增 calibration 模块（L1/L2 标定状态机 + NVS 持久化）
297 lines
9.0 KiB
C
297 lines
9.0 KiB
C
#include "sensor.h"
|
||
#include "ads1256.h"
|
||
#include "calibration.h"
|
||
#include "comm_protocol.h"
|
||
|
||
#include <string.h>
|
||
#include <zephyr/kernel.h>
|
||
#include <zephyr/logging/log.h>
|
||
#include <zephyr/sys/atomic.h>
|
||
|
||
LOG_MODULE_REGISTER(sensor, LOG_LEVEL_INF);
|
||
|
||
/*
|
||
* 每通道每帧采 17 次取中值。3750 SPS 下：
|
||
* 17 次各 0.27ms = 4.86ms/通道
|
||
* 4 通道 = 19.4ms，20ms 帧周期内刚好用满
|
||
* 中值滤波对脉冲/尖峰干扰的抑制力强于均值
|
||
*/
|
||
#define AVG_COUNT 17
|
||
#define DEADZONE_THRESHOLD 1000
|
||
|
||
/* MUX 通道配置：4 路差分 */
|
||
static const uint8_t mux_channels[4] = { 0x01, 0x23, 0x45, 0x67 };
|
||
|
||
/* 传感器坐标：S0=FR, S1=BR, S2=BL, S3=FL */
|
||
static const float sensor_x[4] = {
|
||
+BOARD_HALF_WIDTH_CM, /* S0: FR */
|
||
+BOARD_HALF_WIDTH_CM, /* S1: BR */
|
||
-BOARD_HALF_WIDTH_CM, /* S2: BL */
|
||
-BOARD_HALF_WIDTH_CM, /* S3: FL */
|
||
};
|
||
static const float sensor_y[4] = {
|
||
+BOARD_HALF_LENGTH_CM, /* S0: FR */
|
||
-BOARD_HALF_LENGTH_CM, /* S1: BR */
|
||
-BOARD_HALF_LENGTH_CM, /* S2: BL */
|
||
+BOARD_HALF_LENGTH_CM, /* S3: FL */
|
||
};
|
||
|
||
/* --- 内部状态 --- */
|
||
static int32_t sensor_offsets[4]; // 去皮的零点偏移值
|
||
static int32_t filtered[4];
|
||
static atomic_t tare_requested;
|
||
|
||
/* --- 线程 --- */
|
||
#define SENSOR_STACK_SIZE 2048
|
||
#define SENSOR_PRIORITY 5
|
||
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++) {
|
||
if (arr[j] > arr[j + 1]) {
|
||
int32_t temp = arr[j];
|
||
arr[j] = arr[j + 1];
|
||
arr[j + 1] = temp;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
/**
|
||
* @brief 执行一次四路传感器去皮，更新每路零点偏移。
|
||
*
|
||
* @param 无。
|
||
*
|
||
* @return 无返回值。
|
||
*/
|
||
static void do_tare(void) {
|
||
LOG_INF("Taring...");
|
||
int32_t sorted_buf[AVG_COUNT];
|
||
|
||
for (int i = 0; i < 4; i++) {
|
||
ads1256_write_reg(ADS1256_REG_MUX, mux_channels[i]);
|
||
ads1256_sync_wakeup();
|
||
for (int k = 0; k < AVG_COUNT; k++) {
|
||
ads1256_wait_drdy(50);
|
||
sorted_buf[k] = ads1256_read_data();
|
||
}
|
||
sort_array(sorted_buf, AVG_COUNT);
|
||
sensor_offsets[i] = sorted_buf[AVG_COUNT / 2];
|
||
}
|
||
|
||
LOG_INF(
|
||
"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));
|
||
}
|
||
|
||
/**
|
||
* @brief 完成一帧四通道采集，并写入去皮后的滤波结果。
|
||
*
|
||
* @param 无。
|
||
*
|
||
* @return 无返回值。
|
||
*/
|
||
static void acquire_cycle(void) {
|
||
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);
|
||
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;
|
||
}
|
||
}
|
||
|
||
/**
|
||
* @brief 根据四路受力结果计算压力中心（CoP）和总力。
|
||
*
|
||
* @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 判断用。
|
||
*
|
||
* @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 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++) {
|
||
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 (!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;
|
||
for (int i = 0; i < 4; i++) {
|
||
wx += forces[i] * sensor_x[i];
|
||
wy += forces[i] * sensor_y[i];
|
||
}
|
||
*out_x = (int16_t)(wx / total);
|
||
*out_y = (int16_t)(wy / total);
|
||
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;
|
||
(void)p3;
|
||
|
||
uint8_t debug_log_divider = 0;
|
||
int64_t frame_ts = k_uptime_get();
|
||
|
||
while (1) {
|
||
int64_t now = k_uptime_get();
|
||
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();
|
||
}
|
||
|
||
/* 采集 */
|
||
int64_t t0 = k_uptime_get();
|
||
acquire_cycle();
|
||
int64_t t1 = k_uptime_get();
|
||
|
||
/* CoP 计算 + IIR 平滑 + 发送 */
|
||
static float cop_x_lpf = 0.0f;
|
||
static float cop_y_lpf = 0.0f;
|
||
|
||
int16_t 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;
|
||
|
||
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;
|
||
}
|
||
comm_protocol_send_cop(flags, cop_x, cop_y, force);
|
||
|
||
/* 调试日志（5 Hz） */
|
||
if (++debug_log_divider >= 10) {
|
||
debug_log_divider = 0;
|
||
LOG_INF(
|
||
"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);
|
||
}
|
||
}
|
||
}
|
||
|
||
/**
|
||
* @brief 初始化压力传感器模块：ADS1256 硬件 + 初始去皮 + 创建采集线程。
|
||
*
|
||
* 调用后采集线程自动启动，以 50 Hz 循环采集、计算 CoP 并发送 BLE 数据。
|
||
*
|
||
* @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));
|
||
|
||
do_tare();
|
||
|
||
k_thread_create(
|
||
&sensor_thread, sensor_stack, SENSOR_STACK_SIZE, sensor_thread_fn, NULL, NULL, NULL, SENSOR_PRIORITY, 0,
|
||
K_NO_WAIT);
|
||
k_thread_name_set(&sensor_thread, "sensor");
|
||
|
||
return 0;
|
||
}
|
||
|
||
/**
|
||
* @brief 执行四路去皮（零点校准），可从任意线程调用。
|
||
*
|
||
* @return 无返回值。
|
||
*/
|
||
void sensor_perform_tare(void) {
|
||
atomic_set(&tare_requested, 1);
|
||
}
|