feat(gml670-v2): 添加传感器采集线程与主入口

- sensor: 独立 Zephyr 线程，4 通道×17 次均值采集→CoP→BLE 发送，原子去皮请求，5Hz 调试日志 - main: 薄编排层，仅初始化 BLE 和传感器模块 Co-Authored-By: Claude Opus 4.6 (1M context) <noreply@anthropic.com>
2026-04-30 17:37:38 +08:00
parent 1ad7a9d4ad
commit 1e8809cf89
3 changed files with 248 additions and 0 deletions
@@ -0,0 +1,28 @@
+/*
+ * GML670 Balance Board — CoP Binary Protocol
+ * Features:
+ * - 4-channel ADS1256 with averaging filter.
+ * - Board-side CoP (Center of Pressure) computation.
+ * - Binary protocol over BLE NUS (17-byte CoP frame, 50 Hz).
+ * - Downlink: resistance params, spotter mode, heartbeat.
+ */
+
+#include "ble_transport.h"
+#include "sensor.h"
+
+#include <zephyr/kernel.h>
+#include <zephyr/logging/log.h>
+
+LOG_MODULE_REGISTER(main, LOG_LEVEL_INF);
+
+int main(void) {
+    k_msleep(2000);
+    LOG_INF("--- GML670 System (CoP Binary Protocol) ---");
+
+    ble_transport_init();
+    ble_transport_adv_start();
+
+    if (sensor_init()) return 0;
+
+    return 0;
+}
@@ -0,0 +1,206 @@
+#include "sensor.h"
+#include "ads1256.h"
+#include "ble_transport.h"
+#include "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 下：
+ *   首次切 MUX 建立 0.44ms + 后续 16 次各 0.27ms = 4.76ms/通道
+ *   4 通道 = 19.0ms，20ms 帧周期内刚好用满
+ * 17 次均值提供 √17 ≈ 4.12× 噪声抑制 (≈ 2.04 bit)
+ */
+#define AVG_COUNT          17
+#define DEADZONE_THRESHOLD 250
+
+/* 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_MM, /* S0: FR */
+    +BOARD_HALF_WIDTH_MM, /* S1: BR */
+    -BOARD_HALF_WIDTH_MM, /* S2: BL */
+    -BOARD_HALF_WIDTH_MM, /* S3: FL */
+};
+static const float sensor_y[4] = {
+    +BOARD_HALF_LENGTH_MM, /* S0: FR */
+    -BOARD_HALF_LENGTH_MM, /* S1: BR */
+    -BOARD_HALF_LENGTH_MM, /* S2: BL */
+    +BOARD_HALF_LENGTH_MM, /* 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;
+
+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;
+            }
+        }
+    }
+}
+
+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));
+}
+
+static void acquire_cycle(void) {
+    for (int ch = 0; ch < 4; ch++) {
+        int64_t sum = 0;
+
+        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();
+        }
+
+        int32_t avg = (int32_t)(sum / AVG_COUNT);
+        avg -= sensor_offsets[ch];
+        if (avg > -DEADZONE_THRESHOLD && avg < DEADZONE_THRESHOLD) avg = 0;
+        filtered[ch] = avg;
+    }
+}
+
+static bool compute_cop(float *out_x, float *out_y, float *out_force) {
+    float forces[4];
+    float total = 0.0f;
+
+    for (int i = 0; i < 4; i++) {
+        forces[i] = (float)filtered[i] * ADC_TO_FORCE_SCALE;
+        total += forces[i];
+    }
+
+    *out_force = total;
+
+    if (total < COP_MIN_FORCE_THRESHOLD) {
+        *out_x = 0.0f;
+        *out_y = 0.0f;
+        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 = wx / total;
+    *out_y = wy / total;
+    return true;
+}
+
+static void sensor_thread_fn(void *p1, void *p2, void *p3) {
+    (void)p1;
+    (void)p2;
+    (void)p3;
+
+    uint8_t cop_seq           = 0;
+    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;
+
+        /* 去皮请求 */
+        if (atomic_cas(&tare_requested, 1, 0)) {
+            do_tare();
+        }
+
+        /* 采集 */
+        int64_t t0 = k_uptime_get();
+        acquire_cycle();
+        int64_t t1 = k_uptime_get();
+
+        /* CoP + 打包 */
+        float cop_x, cop_y, force;
+        bool valid = compute_cop(&cop_x, &cop_y, &force);
+
+        union cop_pkt_t pkt;
+        pkt.frame.sync0 = PROTO_SYNC0;
+        pkt.frame.sync1 = PROTO_SYNC1;
+        pkt.frame.type  = PROTO_TYPE_COP;
+        pkt.frame.seq   = cop_seq++;
+        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;
+
+        /* 发送 */
+        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);
+        }
+    }
+}
+
+int sensor_init(void) {
+    int err = ads1256_init();
+    if (err) return err;
+
+    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;
+}
+
+void sensor_perform_tare(void) {
+    atomic_set(&tare_requested, 1);
+}