From a7c6e51ba5f62daaa1575e296497fdb6a045815f Mon Sep 17 00:00:00 2001 From: pNexus Date: Wed, 6 May 2026 18:01:15 +0800 Subject: [PATCH 01/21] =?UTF-8?q?refactor(CMakeLists):=20=E4=BD=BF?= =?UTF-8?q?=E7=94=A8=20GLOB=5FRECURSE=20=E7=AE=80=E5=8C=96=E6=BA=90?= =?UTF-8?q?=E6=96=87=E4=BB=B6=E7=AE=A1=E7=90=86=EF=BC=8C=E5=88=9B=E5=BB=BA?= =?UTF-8?q?=E7=AC=A6=E5=8F=B7=E9=93=BE=E6=8E=A5=E4=BB=A5=E5=90=8C=E6=AD=A5?= =?UTF-8?q?=E7=BC=96=E8=AF=91=E5=91=BD=E4=BB=A4?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeLists.txt | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 7b07cf4..b03226d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -20,12 +20,11 @@ if(SYSBUILD) endif() # NORDIC SDK APP START +file(GLOB_RECURSE app_sources src/*.c) target_sources(app PRIVATE - src/main.c - src/ads1256.c - src/sensor.c - src/ble_transport.c + ${app_sources} ) + target_include_directories(app PRIVATE inc ) From 2ab146b49c588c33234afce434d229406fc6becc Mon Sep 17 00:00:00 2001 From: pNexus Date: Wed, 6 May 2026 18:11:41 +0800 Subject: [PATCH 02/21] =?UTF-8?q?docs(GML670x4=5FV2)=EF=BC=9A=E7=A7=BB?= =?UTF-8?q?=E5=8A=A8=E6=96=87=E6=A1=A3=E5=88=B0doc=E7=9B=AE=E5=BD=95?= =?UTF-8?q?=E4=B8=8B?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- BALANCE_BOARD_PROTOCOL.md => doc/BALANCE_BOARD_PROTOCOL.md | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename BALANCE_BOARD_PROTOCOL.md => doc/BALANCE_BOARD_PROTOCOL.md (100%) diff --git a/BALANCE_BOARD_PROTOCOL.md b/doc/BALANCE_BOARD_PROTOCOL.md similarity index 100% rename from BALANCE_BOARD_PROTOCOL.md rename to doc/BALANCE_BOARD_PROTOCOL.md From 0d2fd30b3aea5d6bfbe112ec8cbc2d1aaf025e8c Mon Sep 17 00:00:00 2001 From: pNexus Date: Thu, 7 May 2026 09:03:45 +0800 Subject: [PATCH 03/21] =?UTF-8?q?fix(README):=20=E4=BF=AE=E6=AD=A3?= =?UTF-8?q?=E6=96=87=E6=A1=A3=E4=B8=AD=E5=8D=8F=E8=AE=AE=E5=AD=97=E6=AE=B5?= =?UTF-8?q?=E9=93=BE=E6=8E=A5=E8=B7=AF=E5=BE=84=20fix(main):=20=E4=BF=AE?= =?UTF-8?q?=E6=AD=A3=E4=B8=BB=E5=BE=AA=E7=8E=AF=E4=B8=AD=E5=BB=B6=E6=97=B6?= =?UTF-8?q?=E4=BB=A5=E4=BF=9D=E6=8C=81=E7=A8=8B=E5=BA=8F=E8=BF=90=E8=A1=8C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- README.md | 2 +- src/ads1256.c | 3 +-- src/main.c | 3 +++ 3 files changed, 5 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index 7408e69..177e0c8 100644 --- a/README.md +++ b/README.md @@ -2,7 +2,7 @@ > 当前目录是 `GML670x4_V2`。代码内的 CMake 项目名是 `GML670_V2`，BLE 设备名是 `GML670_System`。 > -> 这份 README 只描述当前 `V2` 工程已经落地的实现。更细的协议字段和示例帧见 [BALANCE_BOARD_PROTOCOL.md](./BALANCE_BOARD_PROTOCOL.md)。 +> 这份 README 只描述当前 `V2` 工程已经落地的实现。更细的协议字段和示例帧见 [BALANCE_BOARD_PROTOCOL.md](./doc/BALANCE_BOARD_PROTOCOL.md)。 --- diff --git a/src/ads1256.c b/src/ads1256.c index ed4d430..eb2fa8e 100644 --- a/src/ads1256.c +++ b/src/ads1256.c @@ -26,15 +26,14 @@ LOG_MODULE_REGISTER(ads1256, LOG_LEVEL_INF); #define SPI_OP (SPI_OP_MODE_MASTER | SPI_MODE_CPHA | SPI_WORD_SET(8) | SPI_LINES_SINGLE) /* --- 硬件资源 --- */ -static const struct device *spi_dev; static struct spi_config spi_cfg; +static const struct device *spi_dev; static const struct gpio_dt_spec cs_spec = GPIO_DT_SPEC_GET(DT_ALIAS(ads_cs), gpios); static const struct gpio_dt_spec drdy_spec = GPIO_DT_SPEC_GET(DT_ALIAS(ads_drdy), gpios); static const struct gpio_dt_spec reset_spec = GPIO_DT_SPEC_GET(DT_ALIAS(ads_reset), gpios); void ads1256_wait_drdy(uint16_t timeout_ms) { /* drdy_spec 配了 GPIO_ACTIVE_LOW，逻辑 1 表示 DRDY 有效（物理拉低） */ - /* 用 k_usleep 代替 k_busy_wait，让出 CPU 给其他线程 */ int64_t deadline = k_uptime_get() + timeout_ms; while (gpio_pin_get_dt(&drdy_spec) == 0) { k_usleep(10); diff --git a/src/main.c b/src/main.c index df2d624..d88b12e 100644 --- a/src/main.c +++ b/src/main.c @@ -23,6 +23,9 @@ int main(void) { ble_transport_adv_start(); if (sensor_init()) return 0; + while (1) { + k_sleep(K_SECONDS(1)); + } return 0; } From dc525ea4e79d86ac93200ebfd8aa7060eb3b46a3 Mon Sep 17 00:00:00 2001 From: pNexus Date: Sat, 9 May 2026 14:38:28 +0800 Subject: [PATCH 04/21] =?UTF-8?q?feat(gml670x4):=20=E6=89=80=E6=9C=89?= =?UTF-8?q?=E5=8D=8F=E8=AE=AE=E5=B8=A7=E6=B7=BB=E5=8A=A0=20CRC-8/MAXIM=20?= =?UTF-8?q?=E6=A0=A1=E9=AA=8C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 使用 Zephyr crc8() 对帧头之后的载荷计算 CRC，上行发送前填充、 下行接收时校验不通过则丢弃并告警。同步更新协议文档。 Co-Authored-By: Claude Opus 4.6 (1M context) --- doc/BALANCE_BOARD_PROTOCOL.md | 41 +++++++++++++++++++++-------------- inc/protocol.h | 26 +++++++++++++--------- prj.conf | 2 ++ src/ble_transport.c | 15 +++++++++++++ src/main.c | 5 ++--- src/sensor.c | 1 + 6 files changed, 61 insertions(+), 29 deletions(-) diff --git a/doc/BALANCE_BOARD_PROTOCOL.md b/doc/BALANCE_BOARD_PROTOCOL.md index 616ede5..d553c9b 100644 --- a/doc/BALANCE_BOARD_PROTOCOL.md +++ b/doc/BALANCE_BOARD_PROTOCOL.md @@ -27,19 +27,21 @@ 当前协议使用双帧头 `0xAA 0x55` 作为包起始标记。所有多字节字段按 Little Endian 编码。 +每帧末尾附加 1 字节 CRC-8/MAXIM 校验（poly=`0x31`, init=`0x00`, refin=true, refout=true, xorout=`0x00`）。校验范围为帧头之后、CRC 字节之前的所有字段（即跳过 `sync0`/`sync1`）。调用方式：`crc8(data + 2, len - 3, 0x31, 0x00, true)`。 + | 类型名 | 值 | 方向 | 长度 | 频率 | 说明 | |--------|----|------|------|------|------| -| `PROTO_TYPE_COP` | `0x01` | 平衡板 -> 基站 | `17` | 50 Hz | 压力中心帧 | -| `PROTO_TYPE_ENCODER` | `0x02` | 平衡板 -> 基站 | `13` | TBD | 电机编码器帧（预留） | -| `PROTO_TYPE_RESISTANCE` | `0x10` | 基站 -> 平衡板 | `15` | 按需 | 阻力参数帧 | -| `PROTO_TYPE_SPOTTER` | `0x11` | 基站 -> 平衡板 | `8` | 按需 | 保护模式帧 | -| `PROTO_TYPE_HEARTBEAT` | `0x20` | 基站 -> 平衡板 | `4` | ~1 Hz | 心跳帧 | +| `PROTO_TYPE_COP` | `0x01` | 平衡板 -> 基站 | `18` | 50 Hz | 压力中心帧 | +| `PROTO_TYPE_ENCODER` | `0x02` | 平衡板 -> 基站 | `14` | TBD | 电机编码器帧（预留） | +| `PROTO_TYPE_RESISTANCE` | `0x10` | 基站 -> 平衡板 | `16` | 按需 | 阻力参数帧 | +| `PROTO_TYPE_SPOTTER` | `0x11` | 基站 -> 平衡板 | `9` | 按需 | 保护模式帧 | +| `PROTO_TYPE_HEARTBEAT` | `0x20` | 基站 -> 平衡板 | `5` | ~1 Hz | 心跳帧 | ## 3. 上行数据包 ### 3.1 压力中心帧 (CoP) -CoP 帧固定 17 字节，结构如下： +CoP 帧固定 18 字节，结构如下： | 字节偏移 | 字段 | 类型 | 值/范围 | 说明 | |----------|------|------|---------|------| @@ -51,6 +53,7 @@ CoP 帧固定 17 字节，结构如下： | `5-8` | `cop_x` | `float32 LE` | - | 压力中心 X 坐标 (mm) | | `9-12` | `cop_y` | `float32 LE` | - | 压力中心 Y 坐标 (mm) | | `13-16` | `force` | `float32 LE` | - | 总压力 (N) | +| `17` | `crc` | `uint8` | - | CRC-8/MAXIM 校验 | #### flags 位定义 @@ -94,7 +97,7 @@ CoP_Y = (F0 * y0 + F1 * y1 + F2 * y2 + F3 * y3) / F_total 下面是一个 CoP 帧示例（CoP 有效）： ```text -AA 55 01 2A 01 00 00 F0 41 00 00 48 42 00 80 BB 44 +AA 55 01 2A 01 00 00 F0 41 00 00 48 42 00 80 BB 44 XX ``` 含义如下： @@ -106,10 +109,11 @@ AA 55 01 2A 01 00 00 F0 41 00 00 48 42 00 80 BB 44 - `00 00 F0 41`：`cop_x = 30.0` mm（LE float32） - `00 00 48 42`：`cop_y = 50.0` mm（LE float32） - `00 80 BB 44`：`force = 1500.0` N（LE float32） +- `XX`：CRC-8/MAXIM（对字节 `[2..16]` 计算） ### 3.2 电机编码器帧（预留） -编码器帧固定 13 字节，结构如下。该帧目前仅在协议中预留定义，固件暂不实现。 +编码器帧固定 14 字节，结构如下。该帧目前仅在协议中预留定义，固件暂不实现。 | 字节偏移 | 字段 | 类型 | 值/范围 | 说明 | |----------|------|------|---------|------| @@ -120,12 +124,13 @@ AA 55 01 2A 01 00 00 F0 41 00 00 48 42 00 80 BB 44 | `4` | `flags` | `uint8` | `0` | 预留 | | `5-8` | `cable_length` | `float32 LE` | - | 拉索长度 (mm) | | `9-12` | `velocity` | `float32 LE` | - | 拉索速度 (mm/s) | +| `13` | `crc` | `uint8` | - | CRC-8/MAXIM 校验 | ## 4. 下行数据包 ### 4.1 阻力参数帧 -阻力参数帧固定 15 字节，用于基站向平衡板下发电机阻力控制参数。 +阻力参数帧固定 16 字节，用于基站向平衡板下发电机阻力控制参数。 | 字节偏移 | 字段 | 类型 | 值/范围 | 说明 | |----------|------|------|---------|------| @@ -135,11 +140,12 @@ AA 55 01 2A 01 00 00 F0 41 00 00 48 42 00 80 BB 44 | `3-6` | `K` | `float32 LE` | - | 刚度 (N/m) | | `7-10` | `B` | `float32 LE` | - | 阻尼 (Ns/m) | | `11-14` | `Tau` | `float32 LE` | - | 时间常数 (s) | +| `15` | `crc` | `uint8` | - | CRC-8/MAXIM 校验 | #### 示例 ```text -AA 55 10 00 00 C8 42 00 00 48 41 CD CC 4C 3E +AA 55 10 00 00 C8 42 00 00 48 41 CD CC 4C 3E XX ``` 含义如下： @@ -148,10 +154,11 @@ AA 55 10 00 00 C8 42 00 00 48 41 CD CC 4C 3E - `00 00 C8 42`：`K = 100.0` N/m - `00 00 48 41`：`B = 12.5` Ns/m - `CD CC 4C 3E`：`Tau = 0.2` s +- `XX`：CRC-8/MAXIM ### 4.2 Spotter Mode 帧 -Spotter 帧固定 8 字节，用于启用或关闭保护模式。 +Spotter 帧固定 9 字节，用于启用或关闭保护模式。 | 字节偏移 | 字段 | 类型 | 值/范围 | 说明 | |----------|------|------|---------|------| @@ -160,18 +167,19 @@ Spotter 帧固定 8 字节，用于启用或关闭保护模式。 | `2` | `type` | `uint8` | `0x11` | Spotter 包类型 | | `3-6` | `threshold` | `float32 LE` | - | 保护力阈值 (N) | | `7` | `enable` | `uint8` | `0` 或 `1` | `0` = 关闭, `1` = 启用 | +| `8` | `crc` | `uint8` | - | CRC-8/MAXIM 校验 | #### 示例 启用 Spotter Mode，阈值 500 N： ```text -AA 55 11 00 00 FA 43 01 +AA 55 11 00 00 FA 43 01 XX ``` ### 4.3 心跳帧 -心跳帧固定 4 字节，基站以约 1 Hz 频率发送，用于活性检测。 +心跳帧固定 5 字节，基站以约 1 Hz 频率发送，用于活性检测。 | 字节偏移 | 字段 | 类型 | 值/范围 | 说明 | |----------|------|------|---------|------| @@ -179,18 +187,19 @@ AA 55 11 00 00 FA 43 01 | `1` | `sync1` | `uint8` | `0x55` | 帧头第 2 字节 | | `2` | `type` | `uint8` | `0x20` | 心跳包类型 | | `3` | `counter` | `uint8` | `0-255` | 滚动计数 | +| `4` | `crc` | `uint8` | - | CRC-8/MAXIM 校验 | #### 示例 ```text -AA 55 20 05 +AA 55 20 05 XX ``` -含义：心跳包，计数 `5`。 +含义：心跳包，计数 `5`，`XX` 为 CRC。 ## 5. 当前实现约束 -- CoP 帧固定 17 字节，可在最小 ATT MTU 23（NUS 有效负载 20 字节）下传输。 +- CoP 帧固定 18 字节，可在最小 ATT MTU 23（NUS 有效负载 20 字节）下传输。 - 50 Hz CoP 上行流隐式充当平衡板到基站的活性检测，因此心跳仅为下行。 - 传感器通道到物理位置的映射：MUX `{0x01, 0x23, 0x45, 0x67}` → FL/FR/BR/BL。 - `BOARD_HALF_WIDTH_MM = 200`，`BOARD_HALF_LENGTH_MM = 200`（后续可能调整）。 diff --git a/inc/protocol.h b/inc/protocol.h index 67ce18a..872f0a0 100644 --- a/inc/protocol.h +++ b/inc/protocol.h @@ -1,6 +1,7 @@ #pragma once #include #include +#include /* ─── 帧常量 ─── */ /* 双帧头比单字节魔数更容易在连续字节流里完成包起始判定。 */ @@ -41,9 +42,9 @@ /* 总重低于此值时 CoP 无意义（除零保护），单位 kg */ #define COP_MIN_FORCE_THRESHOLD 0.5f -/* ─── 上行: CoP 帧 (17 bytes, 50 Hz) ─── +/* ─── 上行: CoP 帧 (18 bytes, 50 Hz) ─── * - * [AA 55 01 seq flags cop_x(4) cop_y(4) force(4)] + * [AA 55 01 seq flags cop_x(4) cop_y(4) force(4) crc] */ struct cop_frame_t { uint8_t sync0; /* = PROTO_SYNC0 */ @@ -54,6 +55,7 @@ struct cop_frame_t { float cop_x; /* 压力中心 X (mm)，板面坐标系 */ float cop_y; /* 压力中心 Y (mm)，板面坐标系 */ float force; /* 总重量 (kg) */ + uint8_t crc; /* CRC-8/MAXIM, 校验 type~force */ } __attribute__((packed)); union cop_pkt_t { @@ -61,9 +63,9 @@ union cop_pkt_t { uint8_t bytes[sizeof(struct cop_frame_t)]; }; -/* ─── 上行: 电机编码器帧 (13 bytes, 预留) ─── +/* ─── 上行: 电机编码器帧 (14 bytes, 预留) ─── * - * [AA 55 02 seq flags cable_length(4) velocity(4)] + * [AA 55 02 seq flags cable_length(4) velocity(4) crc] */ struct encoder_frame_t { uint8_t sync0; /* = PROTO_SYNC0 */ @@ -73,6 +75,7 @@ struct encoder_frame_t { uint8_t flags; /* 预留，填 0 */ float cable_length; /* 拉索长度 (mm) */ float velocity; /* 拉索速度 (mm/s) */ + uint8_t crc; /* CRC-8/MAXIM */ } __attribute__((packed)); union encoder_pkt_t { @@ -80,9 +83,9 @@ union encoder_pkt_t { uint8_t bytes[sizeof(struct encoder_frame_t)]; }; -/* ─── 下行: 阻力参数帧 (15 bytes) ─── +/* ─── 下行: 阻力参数帧 (16 bytes) ─── * - * [AA 55 10 K(4) B(4) Tau(4)] + * [AA 55 10 K(4) B(4) Tau(4) crc] */ struct resistance_frame_t { uint8_t sync0; /* = PROTO_SYNC0 */ @@ -91,6 +94,7 @@ struct resistance_frame_t { float K; /* 刚度 (N/m) */ float B; /* 阻尼 (Ns/m) */ float Tau; /* 时间常数 (s) */ + uint8_t crc; /* CRC-8/MAXIM */ } __attribute__((packed)); union resistance_pkt_t { @@ -98,9 +102,9 @@ union resistance_pkt_t { uint8_t bytes[sizeof(struct resistance_frame_t)]; }; -/* ─── 下行: Spotter Mode 帧 (8 bytes) ─── +/* ─── 下行: Spotter Mode 帧 (9 bytes) ─── * - * [AA 55 11 threshold(4) enable] + * [AA 55 11 threshold(4) enable crc] */ struct spotter_frame_t { uint8_t sync0; /* = PROTO_SYNC0 */ @@ -108,6 +112,7 @@ struct spotter_frame_t { uint8_t type; /* = PROTO_TYPE_SPOTTER */ float threshold; /* 保护力阈值 (N) */ uint8_t enable; /* 0 = 关闭, 1 = 启用 */ + uint8_t crc; /* CRC-8/MAXIM */ } __attribute__((packed)); union spotter_pkt_t { @@ -115,15 +120,16 @@ union spotter_pkt_t { uint8_t bytes[sizeof(struct spotter_frame_t)]; }; -/* ─── 下行: 心跳帧 (4 bytes, ~1 Hz) ─── +/* ─── 下行: 心跳帧 (5 bytes, ~1 Hz) ─── * - * [AA 55 20 counter] + * [AA 55 20 counter crc] */ struct heartbeat_frame_t { uint8_t sync0; /* = PROTO_SYNC0 */ uint8_t sync1; /* = PROTO_SYNC1 */ uint8_t type; /* = PROTO_TYPE_HEARTBEAT */ uint8_t counter; /* 滚动计数，用于活性检测 */ + uint8_t crc; /* CRC-8/MAXIM */ } __attribute__((packed)); union heartbeat_pkt_t { diff --git a/prj.conf b/prj.conf index 1959d0a..6ec0a81 100644 --- a/prj.conf +++ b/prj.conf @@ -27,3 +27,5 @@ CONFIG_LOG_BACKEND_RTT=y CONFIG_LOG_BACKEND_UART=n CONFIG_LOG_MODE_IMMEDIATE=y CONFIG_MAIN_STACK_SIZE=4096 + +CONFIG_CRC=y diff --git a/src/ble_transport.c b/src/ble_transport.c index c425ca5..e5f8011 100644 --- a/src/ble_transport.c +++ b/src/ble_transport.c @@ -78,6 +78,11 @@ static void nus_received_cb(struct bt_conn *conn, const uint8_t *data, uint16_t switch (data[2]) { case PROTO_TYPE_RESISTANCE: if (len >= sizeof(struct resistance_frame_t)) { + uint8_t crc = crc8(data + 2, sizeof(struct resistance_frame_t) - 3, 0x31, 0x00, true); + if (data[sizeof(struct resistance_frame_t) - 1] != crc) { + LOG_WRN("Resistance frame CRC mismatch"); + break; + } union resistance_pkt_t pkt; memcpy(pkt.bytes, data, sizeof(struct resistance_frame_t)); resistance_K = pkt.frame.K; @@ -90,6 +95,11 @@ static void nus_received_cb(struct bt_conn *conn, const uint8_t *data, uint16_t break; case PROTO_TYPE_SPOTTER: if (len >= sizeof(struct spotter_frame_t)) { + uint8_t crc = crc8(data + 2, sizeof(struct spotter_frame_t) - 3, 0x31, 0x00, true); + if (data[sizeof(struct spotter_frame_t) - 1] != crc) { + LOG_WRN("Spotter frame CRC mismatch"); + break; + } union spotter_pkt_t pkt; memcpy(pkt.bytes, data, sizeof(struct spotter_frame_t)); spotter_threshold = pkt.frame.threshold; @@ -99,6 +109,11 @@ static void nus_received_cb(struct bt_conn *conn, const uint8_t *data, uint16_t break; case PROTO_TYPE_HEARTBEAT: if (len >= sizeof(struct heartbeat_frame_t)) { + uint8_t crc = crc8(data + 2, sizeof(struct heartbeat_frame_t) - 3, 0x31, 0x00, true); + if (data[sizeof(struct heartbeat_frame_t) - 1] != crc) { + LOG_WRN("Heartbeat frame CRC mismatch"); + break; + } last_heartbeat_ms = k_uptime_get(); LOG_DBG("RX Heartbeat"); } diff --git a/src/main.c b/src/main.c index d88b12e..ed97819 100644 --- a/src/main.c +++ b/src/main.c @@ -23,9 +23,8 @@ int main(void) { ble_transport_adv_start(); if (sensor_init()) return 0; - while (1) { - k_sleep(K_SECONDS(1)); - } + + k_sleep(K_FOREVER); return 0; } diff --git a/src/sensor.c b/src/sensor.c index 8132566..6828661 100644 --- a/src/sensor.c +++ b/src/sensor.c @@ -163,6 +163,7 @@ static void sensor_thread_fn(void *p1, void *p2, void *p3) { 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); /* 发送 */ ble_transport_send(pkt.bytes, sizeof(pkt.bytes)); From 353f9aca134b6eeb35ba03f662e392588d3b8a24 Mon Sep 17 00:00:00 2001 From: pNexus Date: Mon, 11 May 2026 14:23:05 +0800 Subject: [PATCH 05/21] =?UTF-8?q?feat(button):=20=E6=B7=BB=E5=8A=A0?= =?UTF-8?q?=E6=8C=89=E9=94=AE=E6=A8=A1=E5=9D=97=E5=8F=8A=E5=88=9D=E5=A7=8B?= =?UTF-8?q?=E5=8C=96=E6=8C=89=E9=94=AE=E5=8E=BB=E7=9A=AE=E5=8A=9F=E8=83=BD?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/button.h | 7 +++++ src/button.c | 75 ++++++++++++++++++++++++++++++++++++++++++++++++++++ src/main.c | 11 +++++++- 3 files changed, 92 insertions(+), 1 deletion(-) create mode 100644 inc/button.h create mode 100644 src/button.c diff --git a/inc/button.h b/inc/button.h new file mode 100644 index 0000000..76c467d --- /dev/null +++ b/inc/button.h @@ -0,0 +1,7 @@ +#pragma once + +/** + * @brief 初始化按键模块：Button1 按下触发去皮 + LED1 闪烁反馈。 + * @return 0 成功，负 errno 失败。 + */ +int button_init(void); diff --git a/src/button.c b/src/button.c new file mode 100644 index 0000000..9407485 --- /dev/null +++ b/src/button.c @@ -0,0 +1,75 @@ +#include "button.h" +#include "sensor.h" + +#include +#include +#include + +LOG_MODULE_REGISTER(button, LOG_LEVEL_INF); + +/* DK Button1 = sw0, LED1 = led0 */ +static const struct gpio_dt_spec btn1 = GPIO_DT_SPEC_GET(DT_ALIAS(sw0), gpios); +static const struct gpio_dt_spec led1 = GPIO_DT_SPEC_GET(DT_ALIAS(led0), gpios); + +static struct gpio_callback btn_cb_data; +static struct k_work_delayable led_off_work; + +#define LED_FLASH_MS 200 +#define LED_BLINK_COUNT 3 + +static struct k_work tare_work; +static volatile int blink_remaining; + +static void led_off_handler(struct k_work *work) { + if (blink_remaining <= 0) return; + + blink_remaining--; + /* 奇数次：点亮；偶数次：熄灭 */ + gpio_pin_set_dt(&led1, blink_remaining & 1); + if (blink_remaining > 0) { + k_work_schedule(&led_off_work, K_MSEC(LED_FLASH_MS)); + } +} + +static void tare_work_handler(struct k_work *work) { + LOG_INF("Button1 pressed → tare"); + sensor_perform_tare(); + + /* LED1 闪烁 3 次：亮-灭-亮-灭-亮-灭 = 6 个状态翻转 */ + blink_remaining = LED_BLINK_COUNT * 2; + gpio_pin_set_dt(&led1, 1); + k_work_schedule(&led_off_work, K_MSEC(LED_FLASH_MS)); +} + +static void btn1_isr(const struct device *dev, struct gpio_callback *cb, uint32_t pins) { + k_work_submit(&tare_work); +} + +int button_init(void) { + if (!gpio_is_ready_dt(&btn1) || !gpio_is_ready_dt(&led1)) { + LOG_ERR("GPIO device not ready"); + return -ENODEV; + } + + int err; + + err = gpio_pin_configure_dt(&btn1, GPIO_INPUT); + if (err) return err; + + err = gpio_pin_interrupt_configure_dt(&btn1, GPIO_INT_EDGE_TO_ACTIVE); + if (err) return err; + + err = gpio_pin_configure_dt(&led1, GPIO_OUTPUT_INACTIVE); + if (err) return err; + + gpio_init_callback(&btn_cb_data, btn1_isr, BIT(btn1.pin)); + + err = gpio_add_callback(btn1.port, &btn_cb_data); + if (err) return err; + + k_work_init(&tare_work, tare_work_handler); + k_work_init_delayable(&led_off_work, led_off_handler); + + LOG_INF("Button1 → tare + LED1 flash initialized"); + return 0; +} diff --git a/src/main.c b/src/main.c index ed97819..8a28311 100644 --- a/src/main.c +++ b/src/main.c @@ -8,6 +8,7 @@ */ #include "ble_transport.h" +#include "button.h" #include "sensor.h" #include @@ -22,7 +23,15 @@ int main(void) { ble_transport_init(); ble_transport_adv_start(); - if (sensor_init()) return 0; + if (button_init()) { + LOG_ERR("Failed to initialize buttons"); + return -1; + } + + if (sensor_init()) { + LOG_ERR("Failed to initialize sensor"); + return -1; + } k_sleep(K_FOREVER); From cb758045f8856326b95f45b91aff27ba9ad18b3c Mon Sep 17 00:00:00 2001 From: pNexus Date: Mon, 11 May 2026 15:25:02 +0800 Subject: [PATCH 06/21] =?UTF-8?q?fix(sensor):=20=E6=9B=B4=E6=96=B0?= =?UTF-8?q?=E4=BC=A0=E6=84=9F=E5=99=A8=E5=8E=BB=E7=9A=AE=E9=80=BB=E8=BE=91?= =?UTF-8?q?=EF=BC=8C=E6=94=B9=E7=94=A8=E4=B8=AD=E5=80=BC=E6=BB=A4=E6=B3=A2?= =?UTF-8?q?=E4=BB=A5=E6=8F=90=E9=AB=98=E5=99=AA=E5=A3=B0=E6=8A=91=E5=88=B6?= =?UTF-8?q?=E6=95=88=E6=9E=9C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/sensor.c | 101 ++++++++++++++++++++++++++++++++++++++++----------- 1 file changed, 80 insertions(+), 21 deletions(-) diff --git a/src/sensor.c b/src/sensor.c index 6828661..fd9a738 100644 --- a/src/sensor.c +++ b/src/sensor.c @@ -11,10 +11,10 @@ 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) + * 每通道每帧采 17 次取中值。3750 SPS 下： + * 17 次各 0.27ms = 4.86ms/通道 + * 4 通道 = 19.4ms，20ms 帧周期内刚好用满 + * 中值滤波对脉冲/尖峰干扰的抑制力强于均值 */ #define AVG_COUNT 17 #define DEADZONE_THRESHOLD 250 @@ -37,7 +37,7 @@ static const float sensor_y[4] = { }; /* --- 内部状态 --- */ -static int32_t sensor_offsets[4]; +static int32_t sensor_offsets[4]; // 去皮的零点偏移值 static int32_t filtered[4]; static atomic_t tare_requested; @@ -47,6 +47,16 @@ static atomic_t tare_requested; 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++) { @@ -59,6 +69,13 @@ static void sort_array(int32_t *arr, int n) { } } +/** + * @brief 执行一次四路传感器去皮，更新每路零点偏移。 + * + * @param 无。 + * + * @return 无返回值。 + */ static void do_tare(void) { LOG_INF("Taring..."); int32_t sorted_buf[AVG_COUNT]; @@ -80,25 +97,42 @@ static void do_tare(void) { memset(filtered, 0, sizeof(filtered)); } +/** + * @brief 完成一帧四通道采集，并写入去皮后的滤波结果。 + * + * @param 无。 + * + * @return 无返回值。 + */ static void acquire_cycle(void) { - for (int ch = 0; ch < 4; ch++) { - int64_t sum = 0; + 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); - sum += ads1256_read_data(); + samples[k] = 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; + 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 坐标，单位由板级几何参数决定； + * @param[out] out_y 输出 CoP 的 Y 坐标；与 @p out_x 配套写回， + * @param[out] out_force 输出总力值； + * + * @retval true 总力高于有效阈值，当前 CoP 可用于对外发布。 + * @retval false 总力不足，CoP 被强制归零以避免在几乎无载荷时放大数值噪声。 + */ static bool compute_cop(float *out_x, float *out_y, float *out_force) { float forces[4]; float total = 0.0f; @@ -126,6 +160,15 @@ static bool compute_cop(float *out_x, float *out_y, float *out_force) { 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; @@ -167,28 +210,37 @@ static void sensor_thread_fn(void *p1, void *p2, void *p3) { /* 发送 */ 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); + "FR=%.2f BR=%.2f BL=%.2f FL=%.2f | total=%.2f kg | cop=(%.1f,%.1f) mm | %s", (double)filtered[0], + (double)filtered[1], (double)filtered[2], (double)filtered[3], (double)force, (double)cop_x, + (double)cop_y, valid ? "VALID" : "low"); + LOG_INF("timing: acq=%lld total=%lld ms/frame", (long long)(t1 - t0), (long long)frame_ms); } } } +/** + * @brief 初始化传感器采集链路并启动后台采集线程。 + * + * @param 无。 + * + * @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)); @@ -202,6 +254,13 @@ int sensor_init(void) { return 0; } +/** + * @brief 请求后台线程在下一帧边界执行去皮。 + * + * @param 无。 + * + * @return 无返回值。 + */ void sensor_perform_tare(void) { atomic_set(&tare_requested, 1); } From b72ea5334f9f85f9b4e41e9577d05def54acbf32 Mon Sep 17 00:00:00 2001 From: pNexus Date: Tue, 12 May 2026 15:58:46 +0800 Subject: [PATCH 07/21] =?UTF-8?q?refactor(protocol):=20=E7=A7=BB=E9=99=A4?= =?UTF-8?q?=E4=B8=8A=E8=A1=8C=E5=B8=A7=20seq=20=E5=AD=97=E6=AE=B5=EF=BC=8C?= =?UTF-8?q?=E7=B2=BE=E7=AE=80=E5=8D=8F=E8=AE=AE=E5=BC=80=E9=94=80?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit BLE 链路层已有重传保序机制，应用层 seq 冗余且从未被接收端消费， 移除后 CoP 帧 18→17 字节、Encoder 帧 14→13 字节。 --- doc/BALANCE_BOARD_PROTOCOL.md | 35 ++++++++++++++++------------------- inc/protocol.h | 10 ++++------ src/sensor.c | 2 -- 3 files changed, 20 insertions(+), 27 deletions(-) diff --git a/doc/BALANCE_BOARD_PROTOCOL.md b/doc/BALANCE_BOARD_PROTOCOL.md index d553c9b..cc70fc6 100644 --- a/doc/BALANCE_BOARD_PROTOCOL.md +++ b/doc/BALANCE_BOARD_PROTOCOL.md @@ -31,8 +31,8 @@ | 类型名 | 值 | 方向 | 长度 | 频率 | 说明 | |--------|----|------|------|------|------| -| `PROTO_TYPE_COP` | `0x01` | 平衡板 -> 基站 | `18` | 50 Hz | 压力中心帧 | -| `PROTO_TYPE_ENCODER` | `0x02` | 平衡板 -> 基站 | `14` | TBD | 电机编码器帧（预留） | +| `PROTO_TYPE_COP` | `0x01` | 平衡板 -> 基站 | `17` | 50 Hz | 压力中心帧 | +| `PROTO_TYPE_ENCODER` | `0x02` | 平衡板 -> 基站 | `13` | TBD | 电机编码器帧（预留） | | `PROTO_TYPE_RESISTANCE` | `0x10` | 基站 -> 平衡板 | `16` | 按需 | 阻力参数帧 | | `PROTO_TYPE_SPOTTER` | `0x11` | 基站 -> 平衡板 | `9` | 按需 | 保护模式帧 | | `PROTO_TYPE_HEARTBEAT` | `0x20` | 基站 -> 平衡板 | `5` | ~1 Hz | 心跳帧 | @@ -41,19 +41,18 @@ ### 3.1 压力中心帧 (CoP) -CoP 帧固定 18 字节，结构如下： +CoP 帧固定 17 字节，结构如下： | 字节偏移 | 字段 | 类型 | 值/范围 | 说明 | |----------|------|------|---------|------| | `0` | `sync0` | `uint8` | `0xAA` | 帧头第 1 字节 | | `1` | `sync1` | `uint8` | `0x55` | 帧头第 2 字节 | | `2` | `type` | `uint8` | `0x01` | CoP 包类型 | -| `3` | `seq` | `uint8` | `0-255` | 包序号，循环递增 | -| `4` | `flags` | `uint8` | 见下表 | 状态标志位 | -| `5-8` | `cop_x` | `float32 LE` | - | 压力中心 X 坐标 (mm) | -| `9-12` | `cop_y` | `float32 LE` | - | 压力中心 Y 坐标 (mm) | -| `13-16` | `force` | `float32 LE` | - | 总压力 (N) | -| `17` | `crc` | `uint8` | - | CRC-8/MAXIM 校验 | +| `3` | `flags` | `uint8` | 见下表 | 状态标志位 | +| `4-7` | `cop_x` | `float32 LE` | - | 压力中心 X 坐标 (mm) | +| `8-11` | `cop_y` | `float32 LE` | - | 压力中心 Y 坐标 (mm) | +| `12-15` | `force` | `float32 LE` | - | 总压力 (N) | +| `16` | `crc` | `uint8` | - | CRC-8/MAXIM 校验 | #### flags 位定义 @@ -97,34 +96,32 @@ CoP_Y = (F0 * y0 + F1 * y1 + F2 * y2 + F3 * y3) / F_total 下面是一个 CoP 帧示例（CoP 有效）： ```text -AA 55 01 2A 01 00 00 F0 41 00 00 48 42 00 80 BB 44 XX +AA 55 01 01 00 00 F0 41 00 00 48 42 00 80 BB 44 XX ``` 含义如下： - `AA 55`：双帧头 - `01`：CoP 包 -- `2A`：序号 `0x2A` - `01`：flags = `force_valid` - `00 00 F0 41`：`cop_x = 30.0` mm（LE float32） - `00 00 48 42`：`cop_y = 50.0` mm（LE float32） - `00 80 BB 44`：`force = 1500.0` N（LE float32） -- `XX`：CRC-8/MAXIM（对字节 `[2..16]` 计算） +- `XX`：CRC-8/MAXIM（对字节 `[2..15]` 计算） ### 3.2 电机编码器帧（预留） -编码器帧固定 14 字节，结构如下。该帧目前仅在协议中预留定义，固件暂不实现。 +编码器帧固定 13 字节，结构如下。该帧目前仅在协议中预留定义，固件暂不实现。 | 字节偏移 | 字段 | 类型 | 值/范围 | 说明 | |----------|------|------|---------|------| | `0` | `sync0` | `uint8` | `0xAA` | 帧头第 1 字节 | | `1` | `sync1` | `uint8` | `0x55` | 帧头第 2 字节 | | `2` | `type` | `uint8` | `0x02` | 编码器包类型 | -| `3` | `seq` | `uint8` | `0-255` | 包序号 | -| `4` | `flags` | `uint8` | `0` | 预留 | -| `5-8` | `cable_length` | `float32 LE` | - | 拉索长度 (mm) | -| `9-12` | `velocity` | `float32 LE` | - | 拉索速度 (mm/s) | -| `13` | `crc` | `uint8` | - | CRC-8/MAXIM 校验 | +| `3` | `flags` | `uint8` | `0` | 预留 | +| `4-7` | `cable_length` | `float32 LE` | - | 拉索长度 (mm) | +| `8-11` | `velocity` | `float32 LE` | - | 拉索速度 (mm/s) | +| `12` | `crc` | `uint8` | - | CRC-8/MAXIM 校验 | ## 4. 下行数据包 @@ -199,7 +196,7 @@ AA 55 20 05 XX ## 5. 当前实现约束 -- CoP 帧固定 18 字节，可在最小 ATT MTU 23（NUS 有效负载 20 字节）下传输。 +- CoP 帧固定 17 字节，可在最小 ATT MTU 23（NUS 有效负载 20 字节）下传输。 - 50 Hz CoP 上行流隐式充当平衡板到基站的活性检测，因此心跳仅为下行。 - 传感器通道到物理位置的映射：MUX `{0x01, 0x23, 0x45, 0x67}` → FL/FR/BR/BL。 - `BOARD_HALF_WIDTH_MM = 200`，`BOARD_HALF_LENGTH_MM = 200`（后续可能调整）。 diff --git a/inc/protocol.h b/inc/protocol.h index 872f0a0..3d9f7ee 100644 --- a/inc/protocol.h +++ b/inc/protocol.h @@ -42,15 +42,14 @@ /* 总重低于此值时 CoP 无意义（除零保护），单位 kg */ #define COP_MIN_FORCE_THRESHOLD 0.5f -/* ─── 上行: CoP 帧 (18 bytes, 50 Hz) ─── +/* ─── 上行: CoP 帧 (17 bytes, 50 Hz) ─── * - * [AA 55 01 seq flags cop_x(4) cop_y(4) force(4) crc] + * [AA 55 01 flags cop_x(4) cop_y(4) force(4) crc] */ struct cop_frame_t { uint8_t sync0; /* = PROTO_SYNC0 */ uint8_t sync1; /* = PROTO_SYNC1 */ uint8_t type; /* = PROTO_TYPE_COP */ - uint8_t seq; /* 包序号 0-255 滚动，用于丢包检测 */ uint8_t flags; /* bit0: force_valid */ float cop_x; /* 压力中心 X (mm)，板面坐标系 */ float cop_y; /* 压力中心 Y (mm)，板面坐标系 */ @@ -63,15 +62,14 @@ union cop_pkt_t { uint8_t bytes[sizeof(struct cop_frame_t)]; }; -/* ─── 上行: 电机编码器帧 (14 bytes, 预留) ─── +/* ─── 上行: 电机编码器帧 (13 bytes, 预留) ─── * - * [AA 55 02 seq flags cable_length(4) velocity(4) crc] + * [AA 55 02 flags cable_length(4) velocity(4) crc] */ struct encoder_frame_t { uint8_t sync0; /* = PROTO_SYNC0 */ uint8_t sync1; /* = PROTO_SYNC1 */ uint8_t type; /* = PROTO_TYPE_ENCODER */ - uint8_t seq; /* 包序号 */ uint8_t flags; /* 预留，填 0 */ float cable_length; /* 拉索长度 (mm) */ float velocity; /* 拉索速度 (mm/s) */ diff --git a/src/sensor.c b/src/sensor.c index fd9a738..568d962 100644 --- a/src/sensor.c +++ b/src/sensor.c @@ -174,7 +174,6 @@ static void sensor_thread_fn(void *p1, void *p2, void *p3) { (void)p2; (void)p3; - uint8_t cop_seq = 0; uint8_t debug_log_divider = 0; int64_t frame_ts = k_uptime_get(); @@ -201,7 +200,6 @@ static void sensor_thread_fn(void *p1, void *p2, void *p3) { 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; From f0072c3bd02693e4cdcd2fd5683b73d7b172e6e9 Mon Sep 17 00:00:00 2001 From: pNexus Date: Tue, 12 May 2026 16:11:19 +0800 Subject: [PATCH 08/21] =?UTF-8?q?refactor(protocol):=20CoP=20=E5=B8=A7?= =?UTF-8?q?=E5=AD=97=E6=AE=B5=E4=BB=8E=20float=20=E6=94=B9=E4=B8=BA?= =?UTF-8?q?=E6=95=B4=E5=9E=8B=EF=BC=8C=E5=8D=95=E4=BD=8D=20mm=E2=86=92cm?= =?UTF-8?q?=20/=20kg?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit cop_x/cop_y 改为 int16_t (cm)，force 改为 uint16_t (kg)， 帧长 17→11 字节，降低带宽占用；1 cm 精度满足平衡板场景需求。 --- doc/BALANCE_BOARD_PROTOCOL.md | 26 +++++++++++++------------- inc/protocol.h | 20 ++++++++++---------- src/sensor.c | 12 ++++++------ 3 files changed, 29 insertions(+), 29 deletions(-) diff --git a/doc/BALANCE_BOARD_PROTOCOL.md b/doc/BALANCE_BOARD_PROTOCOL.md index cc70fc6..b7fe22c 100644 --- a/doc/BALANCE_BOARD_PROTOCOL.md +++ b/doc/BALANCE_BOARD_PROTOCOL.md @@ -31,7 +31,7 @@ | 类型名 | 值 | 方向 | 长度 | 频率 | 说明 | |--------|----|------|------|------|------| -| `PROTO_TYPE_COP` | `0x01` | 平衡板 -> 基站 | `17` | 50 Hz | 压力中心帧 | +| `PROTO_TYPE_COP` | `0x01` | 平衡板 -> 基站 | `11` | 50 Hz | 压力中心帧 | | `PROTO_TYPE_ENCODER` | `0x02` | 平衡板 -> 基站 | `13` | TBD | 电机编码器帧（预留） | | `PROTO_TYPE_RESISTANCE` | `0x10` | 基站 -> 平衡板 | `16` | 按需 | 阻力参数帧 | | `PROTO_TYPE_SPOTTER` | `0x11` | 基站 -> 平衡板 | `9` | 按需 | 保护模式帧 | @@ -41,7 +41,7 @@ ### 3.1 压力中心帧 (CoP) -CoP 帧固定 17 字节，结构如下： +CoP 帧固定 11 字节，结构如下： | 字节偏移 | 字段 | 类型 | 值/范围 | 说明 | |----------|------|------|---------|------| @@ -49,10 +49,10 @@ CoP 帧固定 17 字节，结构如下： | `1` | `sync1` | `uint8` | `0x55` | 帧头第 2 字节 | | `2` | `type` | `uint8` | `0x01` | CoP 包类型 | | `3` | `flags` | `uint8` | 见下表 | 状态标志位 | -| `4-7` | `cop_x` | `float32 LE` | - | 压力中心 X 坐标 (mm) | -| `8-11` | `cop_y` | `float32 LE` | - | 压力中心 Y 坐标 (mm) | -| `12-15` | `force` | `float32 LE` | - | 总压力 (N) | -| `16` | `crc` | `uint8` | - | CRC-8/MAXIM 校验 | +| `4-5` | `cop_x` | `int16 LE` | - | 压力中心 X 坐标 (cm) | +| `6-7` | `cop_y` | `int16 LE` | - | 压力中心 Y 坐标 (cm) | +| `8-9` | `force` | `uint16 LE` | - | 总重量 (kg) | +| `10` | `crc` | `uint8` | - | CRC-8/MAXIM 校验 | #### flags 位定义 @@ -93,10 +93,10 @@ CoP_Y = (F0 * y0 + F1 * y1 + F2 * y2 + F3 * y3) / F_total #### 示例 -下面是一个 CoP 帧示例（CoP 有效）： +下面是一个 CoP 帧示例（CoP 有效，cop_x=3 cm, cop_y=5 cm, force=75 kg）： ```text -AA 55 01 01 00 00 F0 41 00 00 48 42 00 80 BB 44 XX +AA 55 01 01 03 00 05 00 4B 00 XX ``` 含义如下： @@ -104,10 +104,10 @@ AA 55 01 01 00 00 F0 41 00 00 48 42 00 80 BB 44 XX - `AA 55`：双帧头 - `01`：CoP 包 - `01`：flags = `force_valid` -- `00 00 F0 41`：`cop_x = 30.0` mm（LE float32） -- `00 00 48 42`：`cop_y = 50.0` mm（LE float32） -- `00 80 BB 44`：`force = 1500.0` N（LE float32） -- `XX`：CRC-8/MAXIM（对字节 `[2..15]` 计算） +- `03 00`：`cop_x = 3` cm（LE int16） +- `05 00`：`cop_y = 5` cm（LE int16） +- `4B 00`：`force = 75` kg（LE uint16） +- `XX`：CRC-8/MAXIM（对字节 `[2..9]` 计算） ### 3.2 电机编码器帧（预留） @@ -196,7 +196,7 @@ AA 55 20 05 XX ## 5. 当前实现约束 -- CoP 帧固定 17 字节，可在最小 ATT MTU 23（NUS 有效负载 20 字节）下传输。 +- CoP 帧固定 11 字节，可在最小 ATT MTU 23（NUS 有效负载 20 字节）下传输。 - 50 Hz CoP 上行流隐式充当平衡板到基站的活性检测，因此心跳仅为下行。 - 传感器通道到物理位置的映射：MUX `{0x01, 0x23, 0x45, 0x67}` → FL/FR/BR/BL。 - `BOARD_HALF_WIDTH_MM = 200`，`BOARD_HALF_LENGTH_MM = 200`（后续可能调整）。 diff --git a/inc/protocol.h b/inc/protocol.h index 3d9f7ee..79e7dd9 100644 --- a/inc/protocol.h +++ b/inc/protocol.h @@ -42,19 +42,19 @@ /* 总重低于此值时 CoP 无意义（除零保护），单位 kg */ #define COP_MIN_FORCE_THRESHOLD 0.5f -/* ─── 上行: CoP 帧 (17 bytes, 50 Hz) ─── +/* ─── 上行: CoP 帧 (11 bytes, 50 Hz) ─── * - * [AA 55 01 flags cop_x(4) cop_y(4) force(4) crc] + * [AA 55 01 flags cop_x(2) cop_y(2) force(2) crc] */ struct cop_frame_t { - uint8_t sync0; /* = PROTO_SYNC0 */ - uint8_t sync1; /* = PROTO_SYNC1 */ - uint8_t type; /* = PROTO_TYPE_COP */ - uint8_t flags; /* bit0: force_valid */ - float cop_x; /* 压力中心 X (mm)，板面坐标系 */ - float cop_y; /* 压力中心 Y (mm)，板面坐标系 */ - float force; /* 总重量 (kg) */ - uint8_t crc; /* CRC-8/MAXIM, 校验 type~force */ + uint8_t sync0; /* = PROTO_SYNC0 */ + uint8_t sync1; /* = PROTO_SYNC1 */ + uint8_t type; /* = PROTO_TYPE_COP */ + uint8_t flags; /* bit0: force_valid */ + int16_t cop_x; /* 压力中心 X (cm)，板面坐标系 */ + int16_t cop_y; /* 压力中心 Y (cm)，板面坐标系 */ + uint16_t force; /* 总重量 (kg) */ + uint8_t crc; /* CRC-8/MAXIM, 校验 type~force */ } __attribute__((packed)); union cop_pkt_t { diff --git a/src/sensor.c b/src/sensor.c index 568d962..4dce60e 100644 --- a/src/sensor.c +++ b/src/sensor.c @@ -201,9 +201,9 @@ static void sensor_thread_fn(void *p1, void *p2, void *p3) { 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.cop_x = (int16_t)(cop_x * 0.1f); + pkt.frame.cop_y = (int16_t)(cop_y * 0.1f); + pkt.frame.force = (uint16_t)force; pkt.frame.crc = crc8(pkt.bytes + 2, sizeof(pkt.bytes) - 3, 0x31, 0x00, true); /* 发送 */ @@ -213,9 +213,9 @@ static void sensor_thread_fn(void *p1, void *p2, void *p3) { if (++debug_log_divider >= 10) { debug_log_divider = 0; LOG_INF( - "FR=%.2f BR=%.2f BL=%.2f FL=%.2f | total=%.2f kg | cop=(%.1f,%.1f) mm | %s", (double)filtered[0], - (double)filtered[1], (double)filtered[2], (double)filtered[3], (double)force, (double)cop_x, - (double)cop_y, valid ? "VALID" : "low"); + "FR=%.2f BR=%.2f BL=%.2f FL=%.2f | total=%.2f kg | cop=(%.1f,%.1f) cm | %s", (double)filtered[0], + (double)filtered[1], (double)filtered[2], (double)filtered[3], (double)force, + (double)(cop_x * 0.1f), (double)(cop_y * 0.1f), valid ? "VALID" : "low"); LOG_INF("timing: acq=%lld total=%lld ms/frame", (long long)(t1 - t0), (long long)frame_ms); } } From ffafbc4c6afeb2434e9238cd0a3b9cc6a2c266c5 Mon Sep 17 00:00:00 2001 From: pNexus Date: Tue, 12 May 2026 16:20:47 +0800 Subject: [PATCH 09/21] =?UTF-8?q?refactor(protocol):=20=E6=9D=BF=E9=9D=A2?= =?UTF-8?q?=E5=87=A0=E4=BD=95=E4=B8=8E=20CoP=20=E8=AE=A1=E7=AE=97=E7=BB=9F?= =?UTF-8?q?=E4=B8=80=E4=B8=BA=20cm=20=E5=8D=95=E4=BD=8D=EF=BC=8Cforce=20?= =?UTF-8?q?=E6=94=B9=20int16?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit compute_cop 直接输出 int16_t (cm/kg)，消除打包时的浮点转换； BOARD_HALF_WIDTH/LENGTH 从 mm 改 cm，传感器坐标表同步更新。 --- doc/BALANCE_BOARD_PROTOCOL.md | 16 ++++++------ inc/protocol.h | 8 +++--- src/sensor.c | 48 +++++++++++++++++------------------ 3 files changed, 36 insertions(+), 36 deletions(-) diff --git a/doc/BALANCE_BOARD_PROTOCOL.md b/doc/BALANCE_BOARD_PROTOCOL.md index b7fe22c..333210e 100644 --- a/doc/BALANCE_BOARD_PROTOCOL.md +++ b/doc/BALANCE_BOARD_PROTOCOL.md @@ -51,7 +51,7 @@ CoP 帧固定 11 字节，结构如下： | `3` | `flags` | `uint8` | 见下表 | 状态标志位 | | `4-5` | `cop_x` | `int16 LE` | - | 压力中心 X 坐标 (cm) | | `6-7` | `cop_y` | `int16 LE` | - | 压力中心 Y 坐标 (cm) | -| `8-9` | `force` | `uint16 LE` | - | 总重量 (kg) | +| `8-9` | `force` | `int16 LE` | - | 总重量 (kg) | | `10` | `crc` | `uint8` | - | CRC-8/MAXIM 校验 | #### flags 位定义 @@ -71,14 +71,14 @@ CoP 帧固定 11 字节，结构如下： S3 (BL) -------- S2 (BR) ``` -传感器坐标（单位：mm，板面中心为原点）： +传感器坐标（单位：cm，板面中心为原点）： | 传感器 | 位置 | X | Y | |--------|------|---|---| -| S1 | 右前 (FR) | `+200` | `+200` | -| S2 | 右后 (BR) | `+200` | `-200` | -| S3 | 左后 (BL) | `-200` | `-200` | -| S4 | 左前 (FL) | `-200` | `+200` | +| S1 | 右前 (FR) | `+10` | `+10` | +| S2 | 右后 (BR) | `+10` | `-10` | +| S3 | 左后 (BL) | `-10` | `-10` | +| S4 | 左前 (FL) | `-10` | `+10` | 计算公式： @@ -89,7 +89,7 @@ CoP_X = (F0 * x0 + F1 * x1 + F2 * x2 + F3 * x3) / F_total CoP_Y = (F0 * y0 + F1 * y1 + F2 * y2 + F3 * y3) / F_total ``` -当 `F_total < COP_MIN_FORCE_THRESHOLD` 时，CoP 坐标无意义，`flags.bit0 = 0`，`cop_x` 和 `cop_y` 填 `0.0f`。 +当 `F_total < COP_MIN_FORCE_THRESHOLD` 时，CoP 坐标无意义，`flags.bit0 = 0`，`cop_x` 和 `cop_y` 填 `0`。 #### 示例 @@ -199,6 +199,6 @@ AA 55 20 05 XX - CoP 帧固定 11 字节，可在最小 ATT MTU 23（NUS 有效负载 20 字节）下传输。 - 50 Hz CoP 上行流隐式充当平衡板到基站的活性检测，因此心跳仅为下行。 - 传感器通道到物理位置的映射：MUX `{0x01, 0x23, 0x45, 0x67}` → FL/FR/BR/BL。 -- `BOARD_HALF_WIDTH_MM = 200`，`BOARD_HALF_LENGTH_MM = 200`（后续可能调整）。 +- `BOARD_HALF_WIDTH_CM = 10`，`BOARD_HALF_LENGTH_CM = 10`（后续可能调整）。 - `ADC_TO_FORCE_SCALE` 当前为占位值 `1.0f`，待 10 kg 砝码标定后修正。 - ARM Cortex-M33 为小端架构，`float32` 在 packed struct 中天然符合 Little Endian 要求。 diff --git a/inc/protocol.h b/inc/protocol.h index 79e7dd9..8496b6f 100644 --- a/inc/protocol.h +++ b/inc/protocol.h @@ -17,9 +17,9 @@ #define COP_FLAG_FORCE_VALID 0x01U /* 总力 > 阈值，CoP 坐标有意义 */ /* ─── 板面几何 & 传感器标定 ─── */ -/* 传感器到板面中心的半宽、半长 (mm)。 */ -#define BOARD_HALF_WIDTH_MM 100.0f -#define BOARD_HALF_LENGTH_MM 100.0f +/* 传感器到板面中心的半宽、半长 (cm)。 */ +#define BOARD_HALF_WIDTH_CM 10.0f +#define BOARD_HALF_LENGTH_CM 10.0f /* * ADC → kg 换算推导： @@ -53,7 +53,7 @@ struct cop_frame_t { uint8_t flags; /* bit0: force_valid */ int16_t cop_x; /* 压力中心 X (cm)，板面坐标系 */ int16_t cop_y; /* 压力中心 Y (cm)，板面坐标系 */ - uint16_t force; /* 总重量 (kg) */ + int16_t force; /* 总重量 (kg) */ uint8_t crc; /* CRC-8/MAXIM, 校验 type~force */ } __attribute__((packed)); diff --git a/src/sensor.c b/src/sensor.c index 4dce60e..e2b1fe8 100644 --- a/src/sensor.c +++ b/src/sensor.c @@ -24,16 +24,16 @@ 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 */ + +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_MM, /* S0: FR */ - -BOARD_HALF_LENGTH_MM, /* S1: BR */ - -BOARD_HALF_LENGTH_MM, /* S2: BL */ - +BOARD_HALF_LENGTH_MM, /* S3: FL */ + +BOARD_HALF_LENGTH_CM, /* S0: FR */ + -BOARD_HALF_LENGTH_CM, /* S1: BR */ + -BOARD_HALF_LENGTH_CM, /* S2: BL */ + +BOARD_HALF_LENGTH_CM, /* S3: FL */ }; /* --- 内部状态 --- */ @@ -126,14 +126,14 @@ static void acquire_cycle(void) { /** * @brief 根据四路受力结果计算压力中心（CoP）和总力。 * - * @param[out] out_x 输出 CoP 的 X 坐标，单位由板级几何参数决定； - * @param[out] out_y 输出 CoP 的 Y 坐标；与 @p out_x 配套写回， - * @param[out] out_force 输出总力值； + * @param[out] out_x 输出 CoP 的 X 坐标 (cm)； + * @param[out] out_y 输出 CoP 的 Y 坐标 (cm)； + * @param[out] out_force 输出总力值 (kg)； * * @retval true 总力高于有效阈值，当前 CoP 可用于对外发布。 * @retval false 总力不足，CoP 被强制归零以避免在几乎无载荷时放大数值噪声。 */ -static bool compute_cop(float *out_x, float *out_y, float *out_force) { +static bool compute_cop(int16_t *out_x, int16_t *out_y, int16_t *out_force) { float forces[4]; float total = 0.0f; @@ -142,11 +142,11 @@ static bool compute_cop(float *out_x, float *out_y, float *out_force) { total += forces[i]; } - *out_force = total; + *out_force = (int16_t)total; if (total < COP_MIN_FORCE_THRESHOLD) { - *out_x = 0.0f; - *out_y = 0.0f; + *out_x = 0; + *out_y = 0; return false; } @@ -155,8 +155,8 @@ static bool compute_cop(float *out_x, float *out_y, float *out_force) { wx += forces[i] * sensor_x[i]; wy += forces[i] * sensor_y[i]; } - *out_x = wx / total; - *out_y = wy / total; + *out_x = (int16_t)(wx / total); + *out_y = (int16_t)(wy / total); return true; } @@ -193,7 +193,7 @@ static void sensor_thread_fn(void *p1, void *p2, void *p3) { int64_t t1 = k_uptime_get(); /* CoP + 打包 */ - float cop_x, cop_y, force; + int16_t cop_x, cop_y, force; bool valid = compute_cop(&cop_x, &cop_y, &force); union cop_pkt_t pkt; @@ -201,9 +201,9 @@ static void sensor_thread_fn(void *p1, void *p2, void *p3) { pkt.frame.sync1 = PROTO_SYNC1; pkt.frame.type = PROTO_TYPE_COP; pkt.frame.flags = valid ? COP_FLAG_FORCE_VALID : 0; - pkt.frame.cop_x = (int16_t)(cop_x * 0.1f); - pkt.frame.cop_y = (int16_t)(cop_y * 0.1f); - pkt.frame.force = (uint16_t)force; + 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); /* 发送 */ @@ -213,9 +213,9 @@ static void sensor_thread_fn(void *p1, void *p2, void *p3) { if (++debug_log_divider >= 10) { debug_log_divider = 0; LOG_INF( - "FR=%.2f BR=%.2f BL=%.2f FL=%.2f | total=%.2f kg | cop=(%.1f,%.1f) cm | %s", (double)filtered[0], - (double)filtered[1], (double)filtered[2], (double)filtered[3], (double)force, - (double)(cop_x * 0.1f), (double)(cop_y * 0.1f), valid ? "VALID" : "low"); + "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); } } From 16683e40d898c6ac06cd1fb6daad055fdea3cc19 Mon Sep 17 00:00:00 2001 From: pNexus Date: Thu, 14 May 2026 21:11:37 +0800 Subject: [PATCH 10/21] =?UTF-8?q?fix(gml670x4):=20=E8=A1=A5=E5=85=85=20BLE?= =?UTF-8?q?=20=E5=B9=BF=E6=92=AD=E4=B8=8E=E6=89=AB=E6=8F=8F=E5=93=8D?= =?UTF-8?q?=E5=BA=94=E5=90=8D=E7=A7=B0=E9=85=8D=E7=BD=AE?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- prj.conf | 10 ++++++++++ src/ble_transport.c | 3 +++ 2 files changed, 13 insertions(+) diff --git a/prj.conf b/prj.conf index 6ec0a81..caf22e6 100644 --- a/prj.conf +++ b/prj.conf @@ -8,6 +8,16 @@ CONFIG_BT_NUS=y # 借鉴 Kiwii: 禁用 NUS 安全限制，提高连接成功率 CONFIG_BT_NUS_SECURITY_ENABLED=n +# SMP 配对与绑定 +CONFIG_BT_SMP=y +CONFIG_BT_SETTINGS=y +CONFIG_BT_KEYS_OVERWRITE_OLDEST=y +CONFIG_FLASH=y +CONFIG_FLASH_MAP=y +CONFIG_NVS=y +CONFIG_SETTINGS=y +CONFIG_SETTINGS_NVS=y + # 借鉴 Kiwii: 优化缓冲区 (匹配硬件) CONFIG_BT_BUF_ACL_TX_COUNT=3 CONFIG_BT_L2CAP_TX_BUF_COUNT=3 diff --git a/src/ble_transport.c b/src/ble_transport.c index e5f8011..107426e 100644 --- a/src/ble_transport.c +++ b/src/ble_transport.c @@ -8,6 +8,7 @@ #include #include #include +#include LOG_MODULE_REGISTER(ble_transport, LOG_LEVEL_INF); @@ -138,6 +139,8 @@ int ble_transport_init(void) { return err; } + settings_load(); + err = bt_nus_init(&nus_cb); if (err) { LOG_ERR("NUS init failed (err %d)", err); From 93e7990fdfef920dd2c6e35536dbf8918a35ef6f Mon Sep 17 00:00:00 2001 From: pNexus Date: Thu, 14 May 2026 21:17:53 +0800 Subject: [PATCH 11/21] =?UTF-8?q?refactor(ads1256):=20=E4=BF=AE=E6=94=B9?= =?UTF-8?q?=E7=AD=89=E5=BE=85=20DRDY=20=E5=92=8C=E7=A1=AC=E4=BB=B6?= =?UTF-8?q?=E5=A4=8D=E4=BD=8D=E5=87=BD=E6=95=B0=E8=BF=94=E5=9B=9E=E5=80=BC?= =?UTF-8?q?=EF=BC=8C=E5=A2=9E=E5=8A=A0=E8=B6=85=E6=97=B6=E5=A4=84=E7=90=86?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/ads1256.h | 6 ++++-- inc/protocol.h | 20 ++++++++++---------- src/ads1256.c | 25 ++++++++++++++++++------- 3 files changed, 32 insertions(+), 19 deletions(-) diff --git a/inc/ads1256.h b/inc/ads1256.h index 4b37b53..115ae72 100644 --- a/inc/ads1256.h +++ b/inc/ads1256.h @@ -13,8 +13,9 @@ int ads1256_init(void); /** * @brief 等待 DRDY 拉低（转换结果可读），带超时保护。 * @param timeout_ms 超时时间（毫秒）。 + * @return 0 成功，-ETIMEDOUT 超时。 */ -void ads1256_wait_drdy(uint16_t timeout_ms); +int ads1256_wait_drdy(uint16_t timeout_ms); /** * @brief 向 ADS1256 写单个寄存器。 @@ -37,8 +38,9 @@ void ads1256_sync_wakeup(void); /** * @brief 硬件复位 ADS1256（通过 RESET 引脚），等待复位后自校准完成。 + * @return 0 成功，-ETIMEDOUT 芯片无响应。 */ -void ads1256_hwreset(void); +int ads1256_hwreset(void); /** * @brief 读取当前 24 位转换结果，符号扩展为 int32_t。 diff --git a/inc/protocol.h b/inc/protocol.h index 8496b6f..1d9ceea 100644 --- a/inc/protocol.h +++ b/inc/protocol.h @@ -18,8 +18,8 @@ /* ─── 板面几何 & 传感器标定 ─── */ /* 传感器到板面中心的半宽、半长 (cm)。 */ -#define BOARD_HALF_WIDTH_CM 10.0f -#define BOARD_HALF_LENGTH_CM 10.0f +#define BOARD_HALF_WIDTH_CM 42.5f +#define BOARD_HALF_LENGTH_CM 16.0f /* * ADC → kg 换算推导： @@ -47,14 +47,14 @@ * [AA 55 01 flags cop_x(2) cop_y(2) force(2) crc] */ struct cop_frame_t { - uint8_t sync0; /* = PROTO_SYNC0 */ - uint8_t sync1; /* = PROTO_SYNC1 */ - uint8_t type; /* = PROTO_TYPE_COP */ - uint8_t flags; /* bit0: force_valid */ - int16_t cop_x; /* 压力中心 X (cm)，板面坐标系 */ - int16_t cop_y; /* 压力中心 Y (cm)，板面坐标系 */ - int16_t force; /* 总重量 (kg) */ - uint8_t crc; /* CRC-8/MAXIM, 校验 type~force */ + uint8_t sync0; /* = PROTO_SYNC0 */ + uint8_t sync1; /* = PROTO_SYNC1 */ + uint8_t type; /* = PROTO_TYPE_COP */ + uint8_t flags; /* bit0: force_valid */ + int16_t cop_x; /* 压力中心 X (cm)，板面坐标系 */ + int16_t cop_y; /* 压力中心 Y (cm)，板面坐标系 */ + int16_t force; /* 总重量 (kg) */ + uint8_t crc; /* CRC-8/MAXIM, 校验 type~force */ } __attribute__((packed)); union cop_pkt_t { diff --git a/src/ads1256.c b/src/ads1256.c index eb2fa8e..4c89b71 100644 --- a/src/ads1256.c +++ b/src/ads1256.c @@ -32,16 +32,17 @@ static const struct gpio_dt_spec cs_spec = GPIO_DT_SPEC_GET(DT_ALIAS(ads_cs), static const struct gpio_dt_spec drdy_spec = GPIO_DT_SPEC_GET(DT_ALIAS(ads_drdy), gpios); static const struct gpio_dt_spec reset_spec = GPIO_DT_SPEC_GET(DT_ALIAS(ads_reset), gpios); -void ads1256_wait_drdy(uint16_t timeout_ms) { +int ads1256_wait_drdy(uint16_t timeout_ms) { /* drdy_spec 配了 GPIO_ACTIVE_LOW，逻辑 1 表示 DRDY 有效（物理拉低） */ int64_t deadline = k_uptime_get() + timeout_ms; while (gpio_pin_get_dt(&drdy_spec) == 0) { k_usleep(10); if (k_uptime_get() >= deadline) { LOG_WRN("DRDY timeout (%u ms)", timeout_ms); - break; + return -ETIMEDOUT; } } + return 0; } void ads1256_write_reg(uint8_t reg, uint8_t val) { @@ -97,7 +98,7 @@ void ads1256_sync_wakeup(void) { gpio_pin_set_dt(&cs_spec, 0); } -void ads1256_hwreset(void) { +int ads1256_hwreset(void) { /* reset_spec 配了 GPIO_ACTIVE_LOW：逻辑 1 = 物理 LOW = 断言复位 */ gpio_pin_set_dt(&reset_spec, 1); /* t16: 最小复位脉宽 4 × tCLKIN ≈ 0.5µs，取 1ms 余量 */ @@ -105,7 +106,7 @@ void ads1256_hwreset(void) { gpio_pin_set_dt(&reset_spec, 0); /* 复位释放后芯片执行自校准，等待完成 */ k_msleep(10); - ads1256_wait_drdy(500); + return ads1256_wait_drdy(500); } int32_t ads1256_read_data(void) { @@ -123,7 +124,7 @@ int32_t ads1256_read_data(void) { gpio_pin_set_dt(&cs_spec, 0); int32_t val = ((int32_t)rx_buf[0] << 16) | ((int32_t)rx_buf[1] << 8) | rx_buf[2]; if (val & 0x800000) val |= 0xFF000000; - return val; + return -val; } int ads1256_init(void) { @@ -143,7 +144,11 @@ int ads1256_init(void) { return -ENODEV; } - ads1256_hwreset(); + int ret = ads1256_hwreset(); + if (ret) { + LOG_ERR("ADS1256 not detected (DRDY no response after reset)"); + return ret; + } /* 关闭连续输出模式，进入命令模式 */ ads1256_write_cmd(CMD_SDATAC); @@ -180,7 +185,13 @@ int ads1256_init(void) { LOG_INF("ADS1256 regs: STATUS=0x%02X ADCON=0x%02X DRATE=0x%02X", status, adcon, drate); if (drate != 0xC0) { - LOG_ERR("ADS1256 register verify failed (DRATE=0x%02X, expected 0xC0)", drate); + if (status == 0x00 && adcon == 0x00 && drate == 0x00) { + LOG_ERR("ADS1256 SPI read failure (all regs 0x00) — check MISO wiring"); + } else if (status == 0xFF && adcon == 0xFF && drate == 0xFF) { + LOG_ERR("ADS1256 SPI read failure (all regs 0xFF) — MISO floating or not connected"); + } else { + LOG_ERR("ADS1256 register verify failed (DRATE=0x%02X, expected 0xC0)", drate); + } return -EIO; } From e5518ac17ee0f1c5239ad03d34b226e07971b50b Mon Sep 17 00:00:00 2001 From: pNexus Date: Thu, 14 May 2026 21:30:24 +0800 Subject: [PATCH 12/21] =?UTF-8?q?refactor(gml670x4):=20=E5=B0=86=E5=87=BD?= =?UTF-8?q?=E6=95=B0=E6=B3=A8=E9=87=8A=E7=BB=9F=E4=B8=80=E8=BF=81=E7=A7=BB?= =?UTF-8?q?=E5=88=B0=E6=BA=90=E6=96=87=E4=BB=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/ads1256.h | 41 ----------------- inc/ble_transport.h | 18 -------- inc/button.h | 4 -- inc/sensor.h | 10 ----- src/ads1256.c | 50 +++++++++++++++++++++ src/ble_transport.c | 107 ++++++++++++++++++++++++++++++++++++++++++-- src/button.c | 33 ++++++++++++++ src/main.c | 6 +++ src/sensor.c | 10 ++--- 9 files changed, 196 insertions(+), 83 deletions(-) diff --git a/inc/ads1256.h b/inc/ads1256.h index 115ae72..12d0131 100644 --- a/inc/ads1256.h +++ b/inc/ads1256.h @@ -4,52 +4,11 @@ /* sensor.c 切换通道时需要此寄存器地址 */ #define ADS1256_REG_MUX 0x01 -/** - * @brief 初始化 ADS1256：配置 GPIO/SPI，复位芯片，写入寄存器，自校准。 - * @return 0 成功，负 errno 失败。 - */ int ads1256_init(void); - -/** - * @brief 等待 DRDY 拉低（转换结果可读），带超时保护。 - * @param timeout_ms 超时时间（毫秒）。 - * @return 0 成功，-ETIMEDOUT 超时。 - */ int ads1256_wait_drdy(uint16_t timeout_ms); - -/** - * @brief 向 ADS1256 写单个寄存器。 - * @param reg 目标寄存器地址。 - * @param val 要写入的值。 - */ void ads1256_write_reg(uint8_t reg, uint8_t val); - -/** - * @brief 从 ADS1256 读单个寄存器。 - * @param reg 目标寄存器地址。 - * @return 寄存器值。 - */ uint8_t ads1256_read_reg(uint8_t reg); - -/** - * @brief 发送 SYNC + WAKEUP 命令，触发一次同步采样。 - */ void ads1256_sync_wakeup(void); - -/** - * @brief 硬件复位 ADS1256（通过 RESET 引脚），等待复位后自校准完成。 - * @return 0 成功，-ETIMEDOUT 芯片无响应。 - */ int ads1256_hwreset(void); - -/** - * @brief 读取当前 24 位转换结果，符号扩展为 int32_t。 - * @return ADC 原始值。 - */ int32_t ads1256_read_data(void); - -/** - * @brief 发送单字节命令到 ADS1256。 - * @param cmd 命令字。 - */ void ads1256_write_cmd(uint8_t cmd); diff --git a/inc/ble_transport.h b/inc/ble_transport.h index 4ba1450..c219b7d 100644 --- a/inc/ble_transport.h +++ b/inc/ble_transport.h @@ -2,27 +2,9 @@ #include #include -/** - * @brief 初始化 BLE 协议栈、NUS 服务和内部状态。 - * @return 0 成功，负 errno 失败。 - */ int ble_transport_init(void); - -/** - * @brief 启动 BLE 广播。 - */ void ble_transport_adv_start(void); - -/** - * @brief 通过 NUS 发送二进制帧，自动重试最多 3 次。未连接或通知未开启时静默丢弃。 - * @param data 待发送字节缓冲区。 - * @param len 字节数。 - */ void ble_transport_send(const uint8_t *data, uint16_t len); - -/** - * @brief 检查是否已连接且通知已使能。 - */ bool ble_transport_is_ready(void); /* --- 下行参数 getter --- */ diff --git a/inc/button.h b/inc/button.h index 76c467d..f3389f2 100644 --- a/inc/button.h +++ b/inc/button.h @@ -1,7 +1,3 @@ #pragma once -/** - * @brief 初始化按键模块：Button1 按下触发去皮 + LED1 闪烁反馈。 - * @return 0 成功，负 errno 失败。 - */ int button_init(void); diff --git a/inc/sensor.h b/inc/sensor.h index c66ef38..44c2f9e 100644 --- a/inc/sensor.h +++ b/inc/sensor.h @@ -1,14 +1,4 @@ #pragma once -/** - * @brief 初始化压力传感器模块：ADS1256 硬件 + 初始去皮 + 创建采集线程。 - * - * 调用后采集线程自动启动，以 50 Hz 循环采集 → CoP → BLE 发送。 - * @return 0 成功，负 errno 失败。 - */ int sensor_init(void); - -/** - * @brief 执行四路去皮（零点校准），可从任意线程调用。 - */ void sensor_perform_tare(void); diff --git a/src/ads1256.c b/src/ads1256.c index 4c89b71..b5cd474 100644 --- a/src/ads1256.c +++ b/src/ads1256.c @@ -32,6 +32,14 @@ static const struct gpio_dt_spec cs_spec = GPIO_DT_SPEC_GET(DT_ALIAS(ads_cs), static const struct gpio_dt_spec drdy_spec = GPIO_DT_SPEC_GET(DT_ALIAS(ads_drdy), gpios); static const struct gpio_dt_spec reset_spec = GPIO_DT_SPEC_GET(DT_ALIAS(ads_reset), gpios); +/** + * @brief 等待 DRDY 拉低（转换结果可读），带超时保护。 + * + * @param timeout_ms 超时时间（毫秒）。 + * + * @retval 0 DRDY 在超时前变为可读状态。 + * @retval -ETIMEDOUT 超时后仍未等到本次转换结果。 + */ int ads1256_wait_drdy(uint16_t timeout_ms) { /* drdy_spec 配了 GPIO_ACTIVE_LOW，逻辑 1 表示 DRDY 有效（物理拉低） */ int64_t deadline = k_uptime_get() + timeout_ms; @@ -45,6 +53,12 @@ int ads1256_wait_drdy(uint16_t timeout_ms) { return 0; } +/** + * @brief 向 ADS1256 写单个寄存器。 + * + * @param reg 目标寄存器地址。 + * @param val 要写入的寄存器值。 + */ void ads1256_write_reg(uint8_t reg, uint8_t val) { ads1256_wait_drdy(50); gpio_pin_set_dt(&cs_spec, 1); @@ -57,6 +71,13 @@ void ads1256_write_reg(uint8_t reg, uint8_t val) { k_busy_wait(2); } +/** + * @brief 从 ADS1256 读单个寄存器。 + * + * @param reg 目标寄存器地址。 + * + * @return 读取到的寄存器值。 + */ uint8_t ads1256_read_reg(uint8_t reg) { ads1256_wait_drdy(50); gpio_pin_set_dt(&cs_spec, 1); @@ -74,6 +95,11 @@ uint8_t ads1256_read_reg(uint8_t reg) { return rx_val; } +/** + * @brief 发送单字节命令到 ADS1256。 + * + * @param cmd 命令字。 + */ void ads1256_write_cmd(uint8_t cmd) { ads1256_wait_drdy(50); gpio_pin_set_dt(&cs_spec, 1); @@ -83,6 +109,11 @@ void ads1256_write_cmd(uint8_t cmd) { gpio_pin_set_dt(&cs_spec, 0); } +/** + * @brief 发送 SYNC + WAKEUP 命令，触发一次同步采样。 + * + * @return 无返回值。 + */ void ads1256_sync_wakeup(void) { uint8_t cmd_sync = CMD_SYNC; uint8_t cmd_wakeup = CMD_WAKEUP; @@ -98,6 +129,12 @@ void ads1256_sync_wakeup(void) { gpio_pin_set_dt(&cs_spec, 0); } +/** + * @brief 硬件复位 ADS1256（通过 RESET 引脚），等待复位后自校准完成。 + * + * @retval 0 芯片完成复位并重新进入可通信状态。 + * @retval -ETIMEDOUT 复位后等待 DRDY 超时，芯片未按预期响应。 + */ int ads1256_hwreset(void) { /* reset_spec 配了 GPIO_ACTIVE_LOW：逻辑 1 = 物理 LOW = 断言复位 */ gpio_pin_set_dt(&reset_spec, 1); @@ -109,6 +146,11 @@ int ads1256_hwreset(void) { return ads1256_wait_drdy(500); } +/** + * @brief 读取当前 24 位转换结果，符号扩展为 int32_t。 + * + * @return 当前 ADC 转换原始值。 + */ int32_t ads1256_read_data(void) { uint8_t cmd = CMD_RDATA; uint8_t rx_buf[3] = { 0 }; @@ -127,6 +169,14 @@ int32_t ads1256_read_data(void) { return -val; } +/** + * @brief 初始化 ADS1256：配置 GPIO/SPI，复位芯片，写入寄存器，自校准。 + * + * @retval 0 初始化成功，芯片参数已经写入并校验通过。 + * @retval -ENODEV SPI 控制器未就绪，无法访问 ADS1256。 + * @retval -ETIMEDOUT 复位或校准等待阶段未收到芯片响应。 + * @retval -EIO 寄存器回读校验失败，SPI 通信结果不可信。 + */ int ads1256_init(void) { /* GPIO */ gpio_pin_configure_dt(&cs_spec, GPIO_OUTPUT_INACTIVE); diff --git a/src/ble_transport.c b/src/ble_transport.c index 107426e..5bfbfb7 100644 --- a/src/ble_transport.c +++ b/src/ble_transport.c @@ -38,14 +38,26 @@ static float spotter_threshold; static bool spotter_enabled; static int64_t last_heartbeat_ms; -void ble_transport_adv_start(void) { - bt_le_adv_start(BT_LE_ADV_CONN_FAST_2, ad, ARRAY_SIZE(ad), sd, ARRAY_SIZE(sd)); -} - +/** + * @brief 延时广播任务处理函数，用于断链后重新进入可连接状态。 + * + * @param work Zephyr delayable work 入口参数，当前实现未直接使用。 + * + * @return 无返回值。 + */ static void adv_work_handler(struct k_work *work) { + ARG_UNUSED(work); ble_transport_adv_start(); } +/** + * @brief 蓝牙连接建立回调，接管当前连接并更新连接参数。 + * + * @param conn 新建立的连接对象。 + * @param err 连接建立阶段的状态码；非 0 表示本次连接失败。 + * + * @return 无返回值。 + */ static void connected(struct bt_conn *conn, uint8_t err) { if (err) return; @@ -58,7 +70,17 @@ static void connected(struct bt_conn *conn, uint8_t err) { bt_conn_le_param_update(conn, ¶m); } +/** + * @brief 蓝牙断开回调，释放连接引用并安排后续重启广播。 + * + * @param conn 已断开的连接对象。 + * @param reason 协议栈给出的断开原因码。 + * + * @return 无返回值。 + */ static void disconnected(struct bt_conn *conn, uint8_t reason) { + ARG_UNUSED(conn); + ARG_UNUSED(reason); if (current_conn) { bt_conn_unref(current_conn); current_conn = NULL; @@ -69,11 +91,28 @@ static void disconnected(struct bt_conn *conn, uint8_t reason) { k_work_schedule(&adv_work, K_MSEC(1000)); } +/** + * @brief NUS 通知状态回调，用于同步记录对端是否已开启通知。 + * + * @param status 当前 NUS 发送通道的可用状态。 + * + * @return 无返回值。 + */ static void nus_send_enabled(enum bt_nus_send_status status) { nus_notification_enabled = (status == BT_NUS_SEND_STATUS_ENABLED); } +/** + * @brief 处理来自中央设备的下行协议帧，并更新本地参数缓存。 + * + * @param conn 当前接收数据的蓝牙连接对象，当前实现未直接使用。 + * @param data 下行字节流起始地址。 + * @param len 本次收到的字节数。 + * + * @return 无返回值。 + */ static void nus_received_cb(struct bt_conn *conn, const uint8_t *data, uint16_t len) { + ARG_UNUSED(conn); if (len < 3 || data[0] != PROTO_SYNC0 || data[1] != PROTO_SYNC1) return; switch (data[2]) { @@ -130,6 +169,12 @@ static struct bt_nus_cb nus_cb = { }; BT_CONN_CB_DEFINE(conn_callbacks) = { .connected = connected, .disconnected = disconnected }; +/** + * @brief 初始化 BLE 协议栈、NUS 服务和内部状态。 + * + * @retval 0 BLE 协议栈和 NUS 服务初始化成功。 + * @retval 负值 Zephyr 蓝牙或 NUS 初始化阶段返回的具体错误码。 + */ int ble_transport_init(void) { k_work_init_delayable(&adv_work, adv_work_handler); @@ -151,6 +196,23 @@ int ble_transport_init(void) { return 0; } +/** + * @brief 启动 BLE 广播。 + * + * @return 无返回值。 + */ +void ble_transport_adv_start(void) { + bt_le_adv_start(BT_LE_ADV_CONN_FAST_2, ad, ARRAY_SIZE(ad), sd, ARRAY_SIZE(sd)); +} + +/** + * @brief 通过 NUS 发送二进制帧，自动重试最多 3 次。 + * + * @param data 待发送字节缓冲区。 + * @param len 字节数。 + * + * @return 无返回值。 + */ void ble_transport_send(const uint8_t *data, uint16_t len) { if (!nus_notification_enabled || !current_conn) return; @@ -161,30 +223,67 @@ void ble_transport_send(const uint8_t *data, uint16_t len) { } } +/** + * @brief 检查是否已连接且通知已使能。 + * + * @retval true 当前可以通过 NUS 对外发送数据。 + * @retval false 当前未连接或对端尚未开启通知。 + */ bool ble_transport_is_ready(void) { return nus_notification_enabled && (current_conn != NULL); } +/** + * @brief 获取最近一次下发的阻力刚度参数 K。 + * + * @return 当前缓存的阻力刚度参数 K。 + */ float ble_transport_get_resistance_K(void) { return resistance_K; } +/** + * @brief 获取最近一次下发的阻力阻尼参数 B。 + * + * @return 当前缓存的阻力阻尼参数 B。 + */ float ble_transport_get_resistance_B(void) { return resistance_B; } +/** + * @brief 获取最近一次下发的阻力时间常数 Tau。 + * + * @return 当前缓存的阻力时间常数 Tau。 + */ float ble_transport_get_resistance_Tau(void) { return resistance_Tau; } +/** + * @brief 获取最近一次下发的 Spotter 保护阈值。 + * + * @return 当前缓存的 Spotter 阈值。 + */ float ble_transport_get_spotter_threshold(void) { return spotter_threshold; } +/** + * @brief 获取最近一次下发的 Spotter 开关状态。 + * + * @retval true Spotter 保护当前处于启用状态。 + * @retval false Spotter 保护当前处于关闭状态。 + */ bool ble_transport_get_spotter_enabled(void) { return spotter_enabled; } +/** + * @brief 获取最近一次收到心跳包的系统时间戳。 + * + * @return 最近一次收到心跳包时的 uptime 毫秒值。 + */ int64_t ble_transport_get_last_heartbeat_ms(void) { return last_heartbeat_ms; } diff --git a/src/button.c b/src/button.c index 9407485..7b9f475 100644 --- a/src/button.c +++ b/src/button.c @@ -20,6 +20,13 @@ static struct k_work_delayable led_off_work; static struct k_work tare_work; static volatile int blink_remaining; +/** + * @brief 处理 LED 延时翻转，完成一次按键反馈闪烁序列。 + * + * @param work Zephyr delayable work 入口参数，当前实现未直接使用。 + * + * @return 无返回值。 + */ static void led_off_handler(struct k_work *work) { if (blink_remaining <= 0) return; @@ -31,6 +38,13 @@ static void led_off_handler(struct k_work *work) { } } +/** + * @brief 在工作队列上下文执行去皮，并启动 LED 闪烁反馈。 + * + * @param work Zephyr work 入口参数，当前实现未直接使用。 + * + * @return 无返回值。 + */ static void tare_work_handler(struct k_work *work) { LOG_INF("Button1 pressed → tare"); sensor_perform_tare(); @@ -41,10 +55,29 @@ static void tare_work_handler(struct k_work *work) { k_work_schedule(&led_off_work, K_MSEC(LED_FLASH_MS)); } +/** + * @brief 按键 GPIO 中断回调，只负责把去皮请求转交到工作队列。 + * + * @param dev 触发中断的 GPIO 设备，当前实现未直接使用。 + * @param cb GPIO 回调对象，当前实现未直接使用。 + * @param pins 本次触发的引脚位图，当前实现未直接使用。 + * + * @return 无返回值。 + */ static void btn1_isr(const struct device *dev, struct gpio_callback *cb, uint32_t pins) { + ARG_UNUSED(dev); + ARG_UNUSED(cb); + ARG_UNUSED(pins); k_work_submit(&tare_work); } +/** + * @brief 初始化按键模块：Button1 按下触发去皮 + LED1 闪烁反馈。 + * + * @retval 0 按键中断、去皮 work 和 LED 反馈均初始化成功。 + * @retval -ENODEV 按键或 LED 对应的 GPIO 设备未就绪。 + * @retval 负值 GPIO 配置、中断配置或回调注册阶段返回的具体错误码。 + */ int button_init(void) { if (!gpio_is_ready_dt(&btn1) || !gpio_is_ready_dt(&led1)) { LOG_ERR("GPIO device not ready"); diff --git a/src/main.c b/src/main.c index 8a28311..e3778e3 100644 --- a/src/main.c +++ b/src/main.c @@ -16,6 +16,12 @@ LOG_MODULE_REGISTER(main, LOG_LEVEL_INF); +/** + * @brief 初始化系统各模块并进入主线程驻留状态。 + * + * @retval 0 理论上的正常返回值；当前实现进入永久休眠后不会主动返回。 + * @retval -1 按键模块或传感器模块初始化失败。 + */ int main(void) { k_msleep(2000); LOG_INF("--- GML670 System (CoP Binary Protocol) ---"); diff --git a/src/sensor.c b/src/sensor.c index e2b1fe8..1d5af24 100644 --- a/src/sensor.c +++ b/src/sensor.c @@ -222,12 +222,12 @@ static void sensor_thread_fn(void *p1, void *p2, void *p3) { } /** - * @brief 初始化传感器采集链路并启动后台采集线程。 + * @brief 初始化压力传感器模块：ADS1256 硬件 + 初始去皮 + 创建采集线程。 * - * @param 无。 + * 调用后采集线程自动启动，以 50 Hz 循环采集、计算 CoP 并发送 BLE 数据。 * * @retval 0 初始化成功，传感器线程已经启动并完成一次初始去皮。 - * @retval 负值或其他错误码 ADS1256 初始化失败 + * @retval 负值 ADS1256 初始化阶段返回的具体错误码。 */ int sensor_init(void) { int err = ads1256_init(); @@ -253,9 +253,7 @@ int sensor_init(void) { } /** - * @brief 请求后台线程在下一帧边界执行去皮。 - * - * @param 无。 + * @brief 执行四路去皮（零点校准），可从任意线程调用。 * * @return 无返回值。 */ From 1a65bf0f224f08a9a509d5b64e5907c59ef3f4d5 Mon Sep 17 00:00:00 2001 From: pNexus Date: Fri, 15 May 2026 17:10:02 +0800 Subject: [PATCH 13/21] =?UTF-8?q?feat(sensor):=20=E5=A2=9E=E5=8A=A0=20CoP?= =?UTF-8?q?=20=E8=BF=9F=E6=BB=9E=E5=88=A4=E5=AE=9A=E4=B8=8E=20IIR=20?= =?UTF-8?q?=E4=BD=8E=E9=80=9A=E6=BB=A4=E6=B3=A2=EF=BC=8C=E4=BF=AE=E5=A4=8D?= =?UTF-8?q?=E7=A9=BA=E8=BD=BD=E5=B0=96=E5=88=BA=E5=92=8C=E8=B4=9F=E5=8A=9B?= =?UTF-8?q?=E5=AF=B9=E6=B6=88?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - 单通道负力钳零：压力传感器不可能产生负力，消除零漂通道对 total 的对消 - 迟滞阈值：进入 ≥5kg / 退出 <2kg，防止马达振动触发 VALID - IIR 低通：重载(≥10kg)直通零延迟，轻载 alpha=0.3 平滑 2-3 帧 - 物理常量统一到 calibration.h，sensor.c 和 calibration.c 共享 - 新增 calibration 模块（L1/L2 标定状态机 + NVS 持久化） --- inc/calibration.h | 117 +++++++++ src/calibration.c | 630 ++++++++++++++++++++++++++++++++++++++++++++++ src/sensor.c | 80 ++++-- 3 files changed, 804 insertions(+), 23 deletions(-) create mode 100644 inc/calibration.h create mode 100644 src/calibration.c diff --git a/inc/calibration.h b/inc/calibration.h new file mode 100644 index 0000000..14de0ef --- /dev/null +++ b/inc/calibration.h @@ -0,0 +1,117 @@ +#pragma once + +#include +#include + +/* ─── 板面几何 & 传感器物理常量 ─── */ + +#define BOARD_HALF_WIDTH_CM 42.5f /* 传感器到板中心 X 方向距离 (cm) */ +#define BOARD_HALF_LENGTH_CM 16.0f /* 传感器到板中心 Y 方向距离 (cm) */ + +/* + * ADC → kg 换算推导： + * GML670 50kg: 灵敏度 1.75 mV/V, 激励 5V → 满量程输出 8.75 mV + * ADS1256: PGA=64, VREF=2.5V → 满量程 ±78.125 mV + * 50kg 对应 ADC counts = 8.75 / 78.125 × 8388607 ≈ 939524 + * 1 LSB = 50.0 / 939524 ≈ 5.322e-5 kg + */ +#define SENSOR_EXCITATION_V 5.0f /* 传感器激励电压 (V) */ +#define SENSOR_SENSITIVITY_MVV 1.75f /* 传感器灵敏度 (mV/V) */ +#define SENSOR_RATED_LOAD_KG 50.0f /* 传感器满量程 (kg) */ +#define ADS1256_VREF_V 2.5f /* ADS1256 参考电压 (V) */ +#define ADS1256_PGA 64 /* ADS1256 增益倍数 */ + +/* 满量程时的 ADC 计数值 */ +#define ADC_COUNTS_AT_RATED \ + ((SENSOR_SENSITIVITY_MVV * SENSOR_EXCITATION_V) / (2.0f * ADS1256_VREF_V * 1000.0f / ADS1256_PGA) * 8388607.0f) +/* 1 LSB 对应的力 (kg/count) */ +#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) */ + +/* CoP IIR 低通：轻载平滑、重载直通 */ +#define COP_LPF_ALPHA_LIGHT 0.3f /* 轻载平滑系数（越小越平滑） */ +#define COP_HEAVY_FORCE 10.0f /* 超过此值视为重载，直通无延迟 (kg) */ + +/* ─── 标定常量 ─── */ + +#define CAL_NUM_CHANNELS 4 /* 差分通道数 */ +#define CAL_NUM_GRID_PTS 9 /* L2 网格标定点数 (3×3) */ + +/* ─── 标定运行时数据（sensor 线程只读） ─── */ +struct cal_runtime { + int32_t zero[CAL_NUM_CHANNELS]; + float gain[CAL_NUM_CHANNELS]; /* kg/count */ + bool l1_zero_valid; + bool l1_gain_valid; + bool l2_valid; + float grid_err_x[CAL_NUM_GRID_PTS]; /* CoP X 误差 (cm) */ + float grid_err_y[CAL_NUM_GRID_PTS]; /* CoP Y 误差 (cm) */ +}; + +/** + * @brief 初始化标定模块，从 NVS 加载持久化数据或使用默认值。 + * + * 必须在 settings_load() 之后、sensor 线程启动之前调用。 + * + * @retval 0 成功。 + */ +int cal_init(void); + +/** + * @brief 获取当前生效的标定数据指针。 + * + * sensor 线程在每帧开始时调用 cal_check_update() 后，通过本函数 + * 拿到稳定的数据指针用于当帧计算。 + * + * @return 指向内部 working copy 的只读指针。 + */ +const struct cal_runtime *cal_get_working(void); + +/** + * @brief 检查标定数据是否有更新，若有则刷新 working copy。 + * + * sensor 线程在每帧循环顶部调用。 + * + * @retval true 数据已刷新。 + * @retval false 无更新。 + */ +bool cal_check_update(void); + +/** + * @brief 入队一条来自 BLE 的标定命令。 + * + * 从 BLE 回调上下文调用，不做 ADC 操作，仅存储命令参数并设置 pending 标志。 + * + * @param subcmd 子命令码。 + * @param target 通道号 (0-3) 或网格点号 (0-8)，或 0xFF 表示全部。 + * @param param 浮点参数（如已知质量 kg），无参数时为 0。 + * + * @retval 0 命令已入队。 + * @retval -EBUSY 上一条命令尚未被 sensor 线程消费。 + * @retval -EINVAL 状态机拒绝该命令。 + */ +int cal_enqueue_command(uint8_t subcmd, uint8_t target, float param); + +/** + * @brief 在 sensor 线程中执行待处理的标定命令。 + * + * 若存在 pending 命令，执行 ADC 测量、更新状态机、通过 BLE 发送响应。 + * 调用者应在本函数返回 true 时跳过本帧的正常采集。 + * + * @retval true 执行了标定命令（本帧不做正常采集）。 + * @retval false 无待处理命令。 + */ +bool cal_execute_pending(void); + +/** + * @brief 对计算出的 CoP 坐标施加 L2 网格补偿。 + * + * 仅在 L2 标定有效时进行修正，否则不改变输入值。 + * + * @param[in,out] cop_x CoP X 坐标 (cm)。 + * @param[in,out] cop_y CoP Y 坐标 (cm)。 + */ +void cal_apply_l2_correction(float *cop_x, float *cop_y); diff --git a/src/calibration.c b/src/calibration.c new file mode 100644 index 0000000..c2dce73 --- /dev/null +++ b/src/calibration.c @@ -0,0 +1,630 @@ +#include "calibration.h" +#include "ads1256.h" +#include "comm_protocol.h" + +#include +#include +#include +#include +#include + +LOG_MODULE_REGISTER(calibration, LOG_LEVEL_INF); + +#define CAL_SETTINGS_ROOT "cal" +#define CAL_SETTINGS_VER CAL_SETTINGS_ROOT "/ver" +#define CAL_SETTINGS_ZERO CAL_SETTINGS_ROOT "/zero" +#define CAL_SETTINGS_GAIN CAL_SETTINGS_ROOT "/gain" +#define CAL_SETTINGS_GRID_X CAL_SETTINGS_ROOT "/grid_x" +#define CAL_SETTINGS_GRID_Y CAL_SETTINGS_ROOT "/grid_y" +#define CAL_VERSION_L1_VALID BIT(0) +#define CAL_VERSION_L2_VALID BIT(1) +#define CAL_FLAG_PENDING 0 +#define CAL_FLAG_DIRTY 0 +#define CAL_AVG_COUNT 17 +#define CAL_GRID_X_LEFT (-28.33f) +#define CAL_GRID_X_MID (0.0f) +#define CAL_GRID_X_RIGHT (28.33f) +#define CAL_GRID_Y_BOTTOM (-10.67f) +#define CAL_GRID_Y_MID (0.0f) +#define CAL_GRID_Y_TOP (10.67f) + +enum cal_state { + CAL_STATE_IDLE = 0, + CAL_STATE_L1_IN_PROGRESS, + CAL_STATE_L1_READY, + CAL_STATE_L2_IN_PROGRESS, + CAL_STATE_L2_READY, +}; + +struct cal_pending_cmd { + uint8_t subcmd; + uint8_t target; + float param; +}; + +static struct cal_runtime committed; +static struct cal_runtime working; +static struct cal_pending_cmd pending_cmd; +static atomic_t pending_flags; +static atomic_t cal_flags; +static enum cal_state state = CAL_STATE_IDLE; +static const uint8_t mux_channels[CAL_NUM_CHANNELS] = { 0x01, 0x23, 0x45, 0x67 }; + +/** + * @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 tmp = arr[j]; + arr[j] = arr[j + 1]; + arr[j + 1] = tmp; + } + } + } +} + +/** + * @brief 将一份运行时标定数据重置为默认值。 + * + * 默认值只保留“未标定但可运行”的最小语义：零点为 0，增益回落到 + * `ADC_TO_FORCE_SCALE`。这样擦除或首次上电后，系统仍能继续输出基础 CoP。 + * + * @param[out] runtime 待重置的标定数据。 + * + * @return 无返回值。 + */ +static void cal_reset_runtime(struct cal_runtime *runtime) { + memset(runtime, 0, sizeof(*runtime)); + for (int i = 0; i < CAL_NUM_CHANNELS; i++) { + runtime->gain[i] = ADC_TO_FORCE_SCALE; + } +} + +/** + * @brief 计算当前 committed 数据对应的版本位图。 + * + * @return 版本位图。 + */ +static uint32_t cal_make_version(void) { + uint32_t version = 0U; + + if (committed.l1_zero_valid && committed.l1_gain_valid) { + version |= CAL_VERSION_L1_VALID; + } + if (committed.l2_valid) { + version |= CAL_VERSION_L2_VALID; + } + + return version; +} + +/** + * @brief 发送标定响应帧。 + * + * `data[0]` 固定回显 `target`，`data[1]` 固定回显当前状态机状态，其余 6 字节 + * 由命令处理逻辑按上下文填充。这样上位机至少总能知道“哪条命令作用在谁身上， + * 执行后落在什么状态”，不需要依赖日志猜测固件内部阶段。 + * + * @param status 响应状态码。 + * @param subcmd 子命令码。 + * @param target 命令目标。 + * @param value4 4 字节上下文值。 + * @param extra2 2 字节补充值。 + * + * @return 无返回值。 + */ +static void cal_send_resp(uint8_t status, uint8_t subcmd, uint8_t target, uint32_t value4, uint16_t extra2) { + uint8_t data[8] = { 0 }; + + data[0] = target; + data[1] = (uint8_t)state; + memcpy(&data[2], &value4, sizeof(value4)); + memcpy(&data[6], &extra2, sizeof(extra2)); + comm_protocol_send_cal_resp(status, subcmd, data); +} + +/** + * @brief 读取指定通道的中值 ADC 原始计数。 + * + * 标定读数必须和正式采样走同一条 MUX/DRDY/中值链路，否则即便数学公式正确， + * 也会因为采样路径不一致而把系统误差带进标定结果。 + * + * @param ch 通道号，范围 0-3。 + * + * @return 中值 ADC 计数。 + */ +static int32_t cal_read_channel_median(uint8_t ch) { + int32_t samples[CAL_AVG_COUNT]; + + ads1256_write_reg(ADS1256_REG_MUX, mux_channels[ch]); + ads1256_sync_wakeup(); + + for (int i = 0; i < CAL_AVG_COUNT; i++) { + ads1256_wait_drdy(50); + samples[i] = ads1256_read_data(); + } + + sort_array(samples, CAL_AVG_COUNT); + return samples[CAL_AVG_COUNT / 2]; +} + +/** + * @brief 将网格点编号映射到 3x3 目标坐标。 + * + * 编号采用行优先顺序：`0..2` 为上排，`3..5` 为中排，`6..8` 为下排。 + * 这样坐标定义集中在一个函数里，后续若手机侧编号不同，只需要改这一处映射。 + * + * @param target 网格点编号。 + * @param[out] x 目标 X 坐标。 + * @param[out] y 目标 Y 坐标。 + * + * @return 无返回值。 + */ +static void cal_grid_target_to_xy(uint8_t target, float *x, float *y) { + uint8_t row = target / 3U; + uint8_t col = target % 3U; + + *x = (col == 0U) ? CAL_GRID_X_LEFT : ((col == 1U) ? CAL_GRID_X_MID : CAL_GRID_X_RIGHT); + *y = (row == 0U) ? CAL_GRID_Y_TOP : ((row == 1U) ? CAL_GRID_Y_MID : CAL_GRID_Y_BOTTOM); +} + +/** + * @brief 用当前 committed 的 L1 参数测一次 CoP。 + * + * L2 标定记录的是“当前测得的 CoP 与已知目标点之间的误差”，所以这里必须 + * 显式使用 committed 里的零点/增益，而不是 working 里旧版本的数据。 + * + * @param[out] cop_x 测得的 CoP X。 + * @param[out] cop_y 测得的 CoP Y。 + * + * @retval 0 测量成功。 + * @retval -ERANGE 总力过低，当前 CoP 无意义。 + */ +static int cal_measure_cop(float *cop_x, float *cop_y) { + static const float sensor_x[CAL_NUM_CHANNELS] = { + +BOARD_HALF_WIDTH_CM, + +BOARD_HALF_WIDTH_CM, + -BOARD_HALF_WIDTH_CM, + -BOARD_HALF_WIDTH_CM, + }; + static const float sensor_y[CAL_NUM_CHANNELS] = { + +BOARD_HALF_LENGTH_CM, + -BOARD_HALF_LENGTH_CM, + -BOARD_HALF_LENGTH_CM, + +BOARD_HALF_LENGTH_CM, + }; + float total = 0.0f; + float wx = 0.0f; + float wy = 0.0f; + + for (int i = 0; i < CAL_NUM_CHANNELS; i++) { + int32_t raw = cal_read_channel_median((uint8_t)i); + float force = (float)(raw - committed.zero[i]) * committed.gain[i]; + if (force < 0.0f) force = 0.0f; + + total += force; + wx += force * sensor_x[i]; + wy += force * sensor_y[i]; + } + + if (total < COP_FORCE_ENTER_THRESHOLD) { + *cop_x = 0.0f; + *cop_y = 0.0f; + return -ERANGE; + } + + *cop_x = wx / total; + *cop_y = wy / total; + return 0; +} + +/** + * @brief 将当前 committed 数据持久化到 NVS。 + * + * @retval 0 保存成功。 + * @retval 负值 settings 子系统返回的具体错误码。 + */ +static int cal_save_all(void) { + uint32_t version = cal_make_version(); + int err = settings_save_one(CAL_SETTINGS_VER, &version, sizeof(version)); + + if (err) return err; + err = settings_save_one(CAL_SETTINGS_ZERO, committed.zero, sizeof(committed.zero)); + if (err) return err; + err = settings_save_one(CAL_SETTINGS_GAIN, committed.gain, sizeof(committed.gain)); + if (err) return err; + err = settings_save_one(CAL_SETTINGS_GRID_X, committed.grid_err_x, sizeof(committed.grid_err_x)); + if (err) return err; + return settings_save_one(CAL_SETTINGS_GRID_Y, committed.grid_err_y, sizeof(committed.grid_err_y)); +} + +/** + * @brief 擦除所有标定键。 + * + * @retval 0 擦除成功。 + * @retval 负值 settings 子系统返回的具体错误码。 + */ +static int cal_delete_all(void) { + int err = settings_delete(CAL_SETTINGS_VER); + + if (err) return err; + err = settings_delete(CAL_SETTINGS_ZERO); + if (err) return err; + err = settings_delete(CAL_SETTINGS_GAIN); + if (err) return err; + err = settings_delete(CAL_SETTINGS_GRID_X); + if (err) return err; + return settings_delete(CAL_SETTINGS_GRID_Y); +} + +/** + * @brief settings 子树加载回调。 + * + * @param key `cal/` 后的子键名。 + * @param len 数据长度。 + * @param read_cb backend 读回调。 + * @param cb_arg backend 私有参数。 + * + * @retval 0 处理成功。 + * @retval 负值 读失败。 + */ +static int cal_settings_set(const char *key, size_t len, settings_read_cb read_cb, void *cb_arg) { + const char *next; + int rc; + + if (settings_name_steq(key, "ver", &next) && !next && len == sizeof(uint32_t)) { + uint32_t version = 0U; + rc = read_cb(cb_arg, &version, sizeof(version)); + if (rc >= 0) { + committed.l1_zero_valid = (version & CAL_VERSION_L1_VALID) != 0U; + committed.l1_gain_valid = (version & CAL_VERSION_L1_VALID) != 0U; + committed.l2_valid = (version & CAL_VERSION_L2_VALID) != 0U; + return 0; + } + return rc; + } + + if (settings_name_steq(key, "zero", &next) && !next && len == sizeof(committed.zero)) { + rc = read_cb(cb_arg, committed.zero, sizeof(committed.zero)); + return (rc < 0) ? rc : 0; + } + + if (settings_name_steq(key, "gain", &next) && !next && len == sizeof(committed.gain)) { + rc = read_cb(cb_arg, committed.gain, sizeof(committed.gain)); + return (rc < 0) ? rc : 0; + } + + if (settings_name_steq(key, "grid_x", &next) && !next && len == sizeof(committed.grid_err_x)) { + rc = read_cb(cb_arg, committed.grid_err_x, sizeof(committed.grid_err_x)); + return (rc < 0) ? rc : 0; + } + + if (settings_name_steq(key, "grid_y", &next) && !next && len == sizeof(committed.grid_err_y)) { + rc = read_cb(cb_arg, committed.grid_err_y, sizeof(committed.grid_err_y)); + return (rc < 0) ? rc : 0; + } + + return 0; +} + +SETTINGS_STATIC_HANDLER_DEFINE(calibration, CAL_SETTINGS_ROOT, NULL, cal_settings_set, NULL, NULL); + +/** + * @brief 初始化标定模块，从 NVS 加载持久化数据或使用默认值。 + * + * @retval 0 成功。 + */ +int cal_init(void) { + cal_reset_runtime(&committed); + cal_reset_runtime(&working); + atomic_clear(&pending_flags); + atomic_clear(&cal_flags); + state = CAL_STATE_IDLE; + + (void)settings_load_subtree(CAL_SETTINGS_ROOT); + + working = committed; + if (working.l2_valid) { + state = CAL_STATE_L2_READY; + } else if (working.l1_zero_valid && working.l1_gain_valid) { + state = CAL_STATE_L1_READY; + } + + LOG_INF("Calibration init: l1=%d l2=%d", working.l1_gain_valid, working.l2_valid); + return 0; +} + +/** + * @brief 获取当前生效的标定数据指针。 + * + * @return 指向内部 working copy 的只读指针。 + */ +const struct cal_runtime *cal_get_working(void) { + return &working; +} + +/** + * @brief 检查标定数据是否有更新，若有则刷新 working copy。 + * + * @retval true 数据已刷新。 + * @retval false 无更新。 + */ +bool cal_check_update(void) { + if (!atomic_test_and_clear_bit(&cal_flags, CAL_FLAG_DIRTY)) { + return false; + } + + working = committed; + return true; +} + +/** + * @brief 入队一条来自 BLE 的标定命令。 + * + * @param subcmd 子命令码。 + * @param target 通道号或网格点号。 + * @param param 浮点参数。 + * + * @retval 0 命令已入队。 + * @retval -EBUSY 上一条命令尚未被消费。 + * @retval -EINVAL 参数非法。 + */ +int cal_enqueue_command(uint8_t subcmd, uint8_t target, float param) { + switch (subcmd) { + case CAL_SUBCMD_TARE_CH: + case CAL_SUBCMD_GAIN_CH: + if (target >= CAL_NUM_CHANNELS) return -EINVAL; + break; + case CAL_SUBCMD_RECORD_GRID: + if (target >= CAL_NUM_GRID_PTS) return -EINVAL; + break; + default: + break; + } + + pending_cmd.subcmd = subcmd; + pending_cmd.target = target; + pending_cmd.param = param; + + if (atomic_test_and_set_bit(&pending_flags, CAL_FLAG_PENDING)) { + return -EBUSY; + } + + return 0; +} + +/** + * @brief 在 sensor 线程中执行待处理的标定命令。 + * + * @retval true 执行了标定命令。 + * @retval false 当前没有待处理命令。 + */ +bool cal_execute_pending(void) { + struct cal_pending_cmd cmd; + uint8_t status = CAL_STATUS_OK; + uint32_t value4 = 0U; + uint16_t extra2 = 0U; + + if (!atomic_test_and_clear_bit(&pending_flags, CAL_FLAG_PENDING)) { + return false; + } + + cmd = pending_cmd; + + switch (cmd.subcmd) { + case CAL_SUBCMD_START_L1: + committed = working; + committed.l1_zero_valid = false; + committed.l1_gain_valid = false; + committed.l2_valid = false; + memset(committed.grid_err_x, 0, sizeof(committed.grid_err_x)); + memset(committed.grid_err_y, 0, sizeof(committed.grid_err_y)); + state = CAL_STATE_L1_IN_PROGRESS; + break; + + case CAL_SUBCMD_TARE_CH: + if (state != CAL_STATE_L1_IN_PROGRESS) { + status = CAL_STATUS_ERR_STATE; + break; + } + committed.zero[cmd.target] = cal_read_channel_median(cmd.target); + memcpy(&value4, &committed.zero[cmd.target], sizeof(committed.zero[cmd.target])); + break; + + case CAL_SUBCMD_GAIN_CH: + if (state != CAL_STATE_L1_IN_PROGRESS) { + status = CAL_STATUS_ERR_STATE; + break; + } + if (cmd.param <= 0.0f) { + status = CAL_STATUS_ERR_PARAM; + break; + } + { + int32_t median = cal_read_channel_median(cmd.target); + int32_t delta = median - committed.zero[cmd.target]; + if (delta == 0) { + status = CAL_STATUS_ERR_PARAM; + break; + } + committed.gain[cmd.target] = cmd.param / (float)delta; + memcpy(&value4, &committed.gain[cmd.target], sizeof(committed.gain[cmd.target])); + } + break; + + case CAL_SUBCMD_COMMIT_L1: + if (state != CAL_STATE_L1_IN_PROGRESS) { + status = CAL_STATUS_ERR_STATE; + break; + } + committed.l1_zero_valid = true; + committed.l1_gain_valid = true; + committed.l2_valid = false; + if (cal_save_all() != 0) { + status = CAL_STATUS_ERR_NVS; + break; + } + state = CAL_STATE_L1_READY; + value4 = cal_make_version(); + atomic_set_bit(&cal_flags, CAL_FLAG_DIRTY); + break; + + case CAL_SUBCMD_ABORT: + committed = working; + state = working.l2_valid + ? CAL_STATE_L2_READY + : ((working.l1_zero_valid && working.l1_gain_valid) ? CAL_STATE_L1_READY : CAL_STATE_IDLE); + break; + + case CAL_SUBCMD_START_L2: + if (!(working.l1_zero_valid && working.l1_gain_valid)) { + status = CAL_STATUS_ERR_STATE; + break; + } + committed = working; + memset(committed.grid_err_x, 0, sizeof(committed.grid_err_x)); + memset(committed.grid_err_y, 0, sizeof(committed.grid_err_y)); + committed.l2_valid = false; + state = CAL_STATE_L2_IN_PROGRESS; + break; + + case CAL_SUBCMD_RECORD_GRID: + if (state != CAL_STATE_L2_IN_PROGRESS) { + status = CAL_STATUS_ERR_STATE; + break; + } + { + float measured_x; + float measured_y; + float target_x; + float target_y; + + if (cal_measure_cop(&measured_x, &measured_y) != 0) { + status = CAL_STATUS_ERR_PARAM; + break; + } + + cal_grid_target_to_xy(cmd.target, &target_x, &target_y); + committed.grid_err_x[cmd.target] = measured_x - target_x; + committed.grid_err_y[cmd.target] = measured_y - target_y; + memcpy(&value4, &committed.grid_err_x[cmd.target], sizeof(committed.grid_err_x[cmd.target])); + memcpy(&extra2, &committed.grid_err_y[cmd.target], sizeof(extra2)); + } + break; + + case CAL_SUBCMD_COMMIT_L2: + if (state != CAL_STATE_L2_IN_PROGRESS) { + status = CAL_STATUS_ERR_STATE; + break; + } + committed.l2_valid = true; + if (cal_save_all() != 0) { + committed.l2_valid = false; + status = CAL_STATUS_ERR_NVS; + break; + } + state = CAL_STATE_L2_READY; + value4 = cal_make_version(); + atomic_set_bit(&cal_flags, CAL_FLAG_DIRTY); + break; + + case CAL_SUBCMD_ERASE: + if (cal_delete_all() != 0) { + status = CAL_STATUS_ERR_NVS; + break; + } + cal_reset_runtime(&committed); + state = CAL_STATE_IDLE; + atomic_set_bit(&cal_flags, CAL_FLAG_DIRTY); + break; + + case CAL_SUBCMD_QUERY: + value4 = cal_make_version(); + extra2 = (uint16_t)((working.l1_zero_valid ? BIT(0) : 0U) | (working.l1_gain_valid ? BIT(1) : 0U) | + (working.l2_valid ? BIT(2) : 0U)); + break; + + default: + status = CAL_STATUS_ERR_PARAM; + break; + } + + cal_send_resp(status, cmd.subcmd, cmd.target, value4, extra2); + return true; +} + +/** + * @brief 对计算出的 CoP 坐标施加 L2 网格补偿。 + * + * @param[in,out] cop_x CoP X 坐标 (cm)。 + * @param[in,out] cop_y CoP Y 坐标 (cm)。 + * + * @return 无返回值。 + */ +void cal_apply_l2_correction(float *cop_x, float *cop_y) { + float x; + float y; + int ix; + int iy; + float x0; + float x1; + float y0; + float y1; + float tx; + float ty; + int idx00; + int idx10; + int idx01; + int idx11; + float ex0; + float ex1; + float ey0; + float ey1; + float err_x; + float err_y; + + if (!working.l2_valid || !cop_x || !cop_y) { + return; + } + + x = *cop_x; + y = *cop_y; + + if (x < CAL_GRID_X_LEFT) x = CAL_GRID_X_LEFT; + if (x > CAL_GRID_X_RIGHT) x = CAL_GRID_X_RIGHT; + if (y < CAL_GRID_Y_BOTTOM) y = CAL_GRID_Y_BOTTOM; + if (y > CAL_GRID_Y_TOP) y = CAL_GRID_Y_TOP; + + ix = (x <= CAL_GRID_X_MID) ? 0 : 1; + iy = (y <= CAL_GRID_Y_MID) ? 0 : 1; + + x0 = (ix == 0) ? CAL_GRID_X_LEFT : CAL_GRID_X_MID; + x1 = (ix == 0) ? CAL_GRID_X_MID : CAL_GRID_X_RIGHT; + y0 = (iy == 0) ? CAL_GRID_Y_BOTTOM : CAL_GRID_Y_MID; + y1 = (iy == 0) ? CAL_GRID_Y_MID : CAL_GRID_Y_TOP; + + tx = (x1 - x0) == 0.0f ? 0.0f : (x - x0) / (x1 - x0); + ty = (y1 - y0) == 0.0f ? 0.0f : (y - y0) / (y1 - y0); + + idx00 = (2 - iy) * 3 + ix; + idx10 = idx00 + 1; + idx01 = (1 - iy) * 3 + ix; + idx11 = idx01 + 1; + + ex0 = working.grid_err_x[idx00] + tx * (working.grid_err_x[idx10] - working.grid_err_x[idx00]); + ex1 = working.grid_err_x[idx01] + tx * (working.grid_err_x[idx11] - working.grid_err_x[idx01]); + ey0 = working.grid_err_y[idx00] + tx * (working.grid_err_y[idx10] - working.grid_err_y[idx00]); + ey1 = working.grid_err_y[idx01] + tx * (working.grid_err_y[idx11] - working.grid_err_y[idx01]); + err_x = ex0 + ty * (ex1 - ex0); + err_y = ey0 + ty * (ey1 - ey0); + + *cop_x = x - err_x; + *cop_y = y - err_y; +} diff --git a/src/sensor.c b/src/sensor.c index 1d5af24..6fd794d 100644 --- a/src/sensor.c +++ b/src/sensor.c @@ -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 #include @@ -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) { From 0979866a7cf9fb4cba809492d12e01bb28be197f Mon Sep 17 00:00:00 2001 From: pNexus Date: Fri, 15 May 2026 18:06:33 +0800 Subject: [PATCH 14/21] =?UTF-8?q?refactor(protocol):=20=E6=96=B0=E5=A2=9E?= =?UTF-8?q?=20comm=5Fprotocol=20=E5=8D=8F=E8=AE=AE=E6=94=B6=E5=8F=91?= =?UTF-8?q?=E8=A7=A3=E6=9E=90=E5=B1=82=EF=BC=8Cble=5Ftransport=20=E9=80=80?= =?UTF-8?q?=E5=8C=96=E4=B8=BA=E7=BA=AF=E4=BC=A0=E8=BE=93?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - 删除 protocol.h，帧定义和标定命令码统一迁入 comm_protocol.h - 新增 comm_protocol.c：上行组包(CoP/标定响应)、下行解析+msgq 分发 - ble_transport 剥离所有协议解析和参数 getter，仅保留字节收发+rx 回调注册 - main.c 初始化顺序：ble_transport → comm_protocol → adv_start → cal_init - 更新 README 架构图和项目结构表，补充标定流程章节 - 更新 BALANCE_BOARD_PROTOCOL.md 增加标定命令/响应帧格式文档 --- README.md | 85 ++++++++++-- doc/BALANCE_BOARD_PROTOCOL.md | 100 +++++++++++++- inc/ble_transport.h | 38 +++++- inc/comm_protocol.h | 242 ++++++++++++++++++++++++++++++++++ inc/protocol.h | 136 ------------------- src/ble_transport.c | 189 ++------------------------ src/comm_protocol.c | 158 ++++++++++++++++++++++ src/main.c | 8 ++ 8 files changed, 627 insertions(+), 329 deletions(-) create mode 100644 inc/comm_protocol.h delete mode 100644 inc/protocol.h create mode 100644 src/comm_protocol.c diff --git a/README.md b/README.md index 177e0c8..3d9e04d 100644 --- a/README.md +++ b/README.md @@ -73,16 +73,23 @@ src/ads1256.c | v - src/sensor.c + src/sensor.c (业务层) | +--> 去皮 / 17 次均值 / 死区抑制 / 重量换算 / CoP 解算 | - +--> CoP 二进制帧 --> src/ble_transport.c --> BLE NUS Notify + v + src/comm_protocol.c (协议层) | - +<-- Resistance / Spotter / Heartbeat <-- BLE NUS Write + +--> 上行：组包 CoP/标定响应帧，调用 ble_transport_send() + +--> 下行：解析收到的帧，推入 msgq 供业务层消费 + | + v + src/ble_transport.c (传输层) + | + +--> BLE NUS Notify / Write，纯字节收发 ``` -`src/main.c` 本身不承载采样逻辑，它只按顺序调用 `ble_transport_init()`、`ble_transport_adv_start()` 和 `sensor_init()`。 +`src/main.c` 本身不承载采样逻辑，它只按顺序调用 `ble_transport_init()`、`comm_protocol_init()`、`ble_transport_adv_start()` 和 `sensor_init()`。 ## 4. 功能需求与当前状态 @@ -121,6 +128,67 @@ | 阻力闭环控制 | 参数已接入协议层，控制逻辑未落地 | 🔲 | | Spotter 保护动作 | 参数已接入协议层，保护动作未落地 | 🔲 | +### 4.4 传感器标定 + +标定分两级，通过 BLE 下行标定命令帧触发，平衡板执行后回复标定响应帧。帧格式详见 [BALANCE_BOARD_PROTOCOL.md](./doc/BALANCE_BOARD_PROTOCOL.md)。 + +#### L1 标定（单通道零点 + 增益） + +```text +基站 平衡板 + │ │ + │── START_L1 ─────────────────>│ 进入 L1 模式 + │<──────────────── OK ─────────│ + │ │ + │── TARE_CH (ch=0) ──────────>│ 空载，记录零点 + │<──────────────── OK ─────────│ + │ ...对 ch=1,2,3 重复... │ + │ │ + │── GAIN_CH (ch=0, 10kg) ────>│ 加载砝码，计算增益 + │<──────────────── OK ─────────│ + │ ...对 ch=1,2,3 重复... │ + │ │ + │── COMMIT_L1 ────────────────>│ 写入 NVS + │<──────────────── OK ─────────│ +``` + +#### L2 标定（9 点网格空间修正） + +L2 需要 L1 已完成。在 9 个已知位置分别放置标定负载，记录 CoP 误差用于空间修正。 + +```text +基站 平衡板 + │ │ + │── START_L2 ─────────────────>│ 进入 L2 模式 + │<──────────────── OK ─────────│ + │ │ + │── RECORD_GRID (pt=0) ──────>│ 在网格点 0 放置已知负载 + │<──────────────── OK ─────────│ + │ ...对 pt=1..8 重复... │ + │ │ + │── COMMIT_L2 ────────────────>│ 写入 NVS + │<──────────────── OK ─────────│ +``` + +网格点编号（3×3，俯视）： + +```text + 6 ── 7 ── 8 + │ │ + 3 ── 4 ── 5 + │ │ + 0 ── 1 ── 2 +``` + +点 4 为板面中心。 + +#### 状态与持久化 + +- 标定数据通过 NVS 持久化，上电后自动加载 +- `ERASE` 命令可清除全部标定数据，回退到出厂默认 +- `QUERY` 命令返回当前各级标定的有效性标志 +- `ABORT` 可在任意阶段中止标定流程，已测量数据不写入 NVS + ## 5. BLE 协议规格 平衡板工作在 BLE `Peripheral` 角色，通过 Nordic UART Service 与基站交换二进制帧。 @@ -258,11 +326,12 @@ CoP 帧固定 17 字节，结构如下： | 路径 | 说明 | |------|------| -| `src/main.c` | 启动入口，负责初始化 BLE 和传感器子系统 | +| `src/main.c` | 启动入口，负责初始化 BLE、协议层和传感器子系统 | | `src/ads1256.c` | ADS1256 SPI 读写、复位、寄存器配置和校验 | -| `src/sensor.c` | 采样线程、tare、均值滤波、重量换算和 CoP 打包 | -| `src/ble_transport.c` | BLE 广播、NUS 收发、断线重广播和下行参数缓存 | -| `inc/protocol.h` | 二进制协议结构、重量换算系数、几何参数和阈值 | +| `src/sensor.c` | 采样线程、tare、均值滤波、重量换算和 CoP 计算 | +| `src/comm_protocol.c` | 协议收发解析层：上行组包、下行解析、msgq 分发 | +| `src/ble_transport.c` | BLE 广播、NUS 字节收发、断线重广播 | +| `inc/comm_protocol.h` | 帧格式定义、标定子命令码、下行消息队列类型和协议 API | | `app.overlay` | SPI1、CS、DRDY、RESET 的板级连线 | | `prj.conf` | BLE、日志和控制台相关配置 | | `BALANCE_BOARD_PROTOCOL.md` | 协议专项说明、字段表和示例帧 | diff --git a/doc/BALANCE_BOARD_PROTOCOL.md b/doc/BALANCE_BOARD_PROTOCOL.md index 333210e..2a01146 100644 --- a/doc/BALANCE_BOARD_PROTOCOL.md +++ b/doc/BALANCE_BOARD_PROTOCOL.md @@ -36,6 +36,8 @@ | `PROTO_TYPE_RESISTANCE` | `0x10` | 基站 -> 平衡板 | `16` | 按需 | 阻力参数帧 | | `PROTO_TYPE_SPOTTER` | `0x11` | 基站 -> 平衡板 | `9` | 按需 | 保护模式帧 | | `PROTO_TYPE_HEARTBEAT` | `0x20` | 基站 -> 平衡板 | `5` | ~1 Hz | 心跳帧 | +| `PROTO_TYPE_CAL_CMD` | `0x30` | 基站 -> 平衡板 | `10` | 按需 | 标定命令帧 | +| `PROTO_TYPE_CAL_RESP` | `0x31` | 平衡板 -> 基站 | `14` | 按需 | 标定响应帧 | ## 3. 上行数据包 @@ -194,7 +196,103 @@ AA 55 20 05 XX 含义：心跳包，计数 `5`，`XX` 为 CRC。 -## 5. 当前实现约束 +## 5. 标定协议 + +标定协议通过下行标定命令帧（`0x30`）和上行标定响应帧（`0x31`）完成传感器在线标定。标定分两级：L1 单通道零点/增益标定，L2 多点网格空间修正。 + +### 5.1 标定命令帧（下行） + +标定命令帧固定 10 字节，由基站发送给平衡板： + +| 字节偏移 | 字段 | 类型 | 值/范围 | 说明 | +|----------|------|------|---------|------| +| `0` | `sync0` | `uint8` | `0xAA` | 帧头第 1 字节 | +| `1` | `sync1` | `uint8` | `0x55` | 帧头第 2 字节 | +| `2` | `type` | `uint8` | `0x30` | 标定命令包类型 | +| `3` | `subcmd` | `uint8` | 见下表 | 子命令码 | +| `4` | `target` | `uint8` | `0-3` / `0-8` | 通道号或网格点号 | +| `5-8` | `param` | `float32 LE` | - | 参数（如已知质量 kg） | +| `9` | `crc` | `uint8` | - | CRC-8/MAXIM 校验 | + +#### `subcmd` 子命令码定义 + +| 子命令码 | 名称 | target 含义 | param 含义 | 说明 | +|----------|------|-------------|------------|------| +| `0x01` | `START_L1` | - | - | 进入 L1 标定模式 | +| `0x02` | `TARE_CH` | 通道号 (0-3) | - | 对指定通道执行零点标定 | +| `0x03` | `GAIN_CH` | 通道号 (0-3) | 已知质量 (kg) | 对指定通道执行增益标定 | +| `0x04` | `COMMIT_L1` | - | - | 提交 L1 标定数据到 NVS | +| `0x05` | `ABORT` | - | - | 中止当前标定流程 | +| `0x10` | `START_L2` | - | - | 进入 L2 网格标定模式 | +| `0x11` | `RECORD_GRID` | 网格点号 (0-8) | - | 记录当前网格点 CoP 误差 | +| `0x12` | `COMMIT_L2` | - | - | 提交 L2 标定数据到 NVS | +| `0x20` | `ERASE` | - | - | 擦除所有标定数据 | +| `0x21` | `QUERY` | - | - | 查询当前标定状态 | + +#### 示例 + +对通道 2 执行增益标定，已知质量 10.0 kg： + +```text +AA 55 30 03 02 00 00 20 41 XX +``` + +- `AA 55 30`：标定命令帧头 +- `03`：subcmd = `GAIN_CH` +- `02`：target = 通道 2 +- `00 00 20 41`：param = `10.0` kg（float32 LE） +- `XX`：CRC-8/MAXIM + +### 5.2 标定响应帧（上行） + +标定响应帧固定 14 字节，平衡板在执行标定命令后回复： + +| 字节偏移 | 字段 | 类型 | 值/范围 | 说明 | +|----------|------|------|---------|------| +| `0` | `sync0` | `uint8` | `0xAA` | 帧头第 1 字节 | +| `1` | `sync1` | `uint8` | `0x55` | 帧头第 2 字节 | +| `2` | `type` | `uint8` | `0x31` | 标定响应包类型 | +| `3` | `status` | `uint8` | 见下表 | 结果状态码 | +| `4` | `subcmd` | `uint8` | - | 对应的子命令回显 | +| `5-12` | `data` | `uint8[8]` | - | 响应数据（上下文相关） | +| `13` | `crc` | `uint8` | - | CRC-8/MAXIM 校验 | + +#### 状态码定义 + +| 状态码 | 名称 | 说明 | +|--------|------|------| +| `0x00` | `OK` | 命令执行成功 | +| `0x01` | `ERR_STATE` | 状态机不允许此操作 | +| `0x02` | `ERR_NVS` | NVS 读写失败 | +| `0x03` | `ERR_PARAM` | 参数非法 | + +#### data 字段含义 + +`data[8]` 内容取决于子命令： + +| 子命令 | data 内容 | 说明 | +|--------|-----------|------| +| `TARE_CH` | `data[0..3]` = 零点 ADC 值 (int32 LE) | 通道去皮后的零点偏移 | +| `GAIN_CH` | `data[0..3]` = 增益 (float32 LE) | kg/count 换算系数 | +| `QUERY` | `data[0]` = L1 zero valid, `data[1]` = L1 gain valid, `data[2]` = L2 valid | 各级标定有效性标志 | +| 其他 | 全零 | 无附加数据 | + +#### 示例 + +通道 2 增益标定成功，增益 = 5.322e-5 kg/count： + +```text +AA 55 31 00 03 XX XX XX XX 00 00 00 00 XX +``` + +- `AA 55 31`：标定响应帧头 +- `00`：status = `OK` +- `03`：subcmd = `GAIN_CH`（回显） +- 前 4 字节 data：增益值 float32 LE +- 后 4 字节 data：填 0 +- 末字节：CRC-8/MAXIM + +## 6. 当前实现约束 - CoP 帧固定 11 字节，可在最小 ATT MTU 23（NUS 有效负载 20 字节）下传输。 - 50 Hz CoP 上行流隐式充当平衡板到基站的活性检测，因此心跳仅为下行。 diff --git a/inc/ble_transport.h b/inc/ble_transport.h index c219b7d..697512d 100644 --- a/inc/ble_transport.h +++ b/inc/ble_transport.h @@ -2,15 +2,39 @@ #include #include +/** @brief BLE 原始数据接收回调类型。 */ +typedef void (*ble_rx_cb_t)(const uint8_t *data, uint16_t len); + +/** + * @brief 初始化 BLE 协议栈和 NUS 服务。 + * + * @retval 0 成功。 + * @retval 负值 初始化错误码。 + */ int ble_transport_init(void); + +/** @brief 启动 BLE 可连接广播。 */ void ble_transport_adv_start(void); + +/** + * @brief 通过 NUS 发送二进制数据，内部自动重试最多 3 次。 + * + * @param data 待发送字节缓冲区。 + * @param len 字节数。 + */ void ble_transport_send(const uint8_t *data, uint16_t len); + +/** + * @brief 检查 BLE 连接是否就绪（已连接且通知已使能）。 + * + * @retval true 可发送数据。 + * @retval false 未就绪。 + */ bool ble_transport_is_ready(void); -/* --- 下行参数 getter --- */ -float ble_transport_get_resistance_K(void); -float ble_transport_get_resistance_B(void); -float ble_transport_get_resistance_Tau(void); -float ble_transport_get_spotter_threshold(void); -bool ble_transport_get_spotter_enabled(void); -int64_t ble_transport_get_last_heartbeat_ms(void); +/** + * @brief 注册上层原始数据接收回调，NUS 收到数据时透传调用。 + * + * @param cb 回调函数指针。 + */ +void ble_transport_register_rx_cb(ble_rx_cb_t cb); diff --git a/inc/comm_protocol.h b/inc/comm_protocol.h new file mode 100644 index 0000000..dde1e9f --- /dev/null +++ b/inc/comm_protocol.h @@ -0,0 +1,242 @@ +#pragma once +#include +#include +#include +#include + +/* ═══════════════════════════════════════════════════════════════════ + * 帧常量 + * ═══════════════════════════════════════════════════════════════════ */ + +/* 双帧头比单字节魔数更容易在连续字节流里完成包起始判定 */ +#define PROTO_SYNC0 0xAAU +#define PROTO_SYNC1 0x55U +#define PROTO_TYPE_COP 0x01U /* 上行: 压力中心 (CoP) 帧 */ +#define PROTO_TYPE_ENCODER 0x02U /* 上行: 电机编码器帧（预留） */ +#define PROTO_TYPE_RESISTANCE 0x10U /* 下行: 阻力参数 (K, B, Tau) */ +#define PROTO_TYPE_SPOTTER 0x11U /* 下行: Spotter Mode 保护阈值 */ +#define PROTO_TYPE_HEARTBEAT 0x20U /* 下行: 心跳包 */ +#define PROTO_TYPE_CAL_CMD 0x30U /* 下行: 标定命令 */ +#define PROTO_TYPE_CAL_RESP 0x31U /* 上行: 标定响应 */ + +/* CoP flags 位定义 */ +#define COP_FLAG_FORCE_VALID 0x01U /* 总力 > 阈值，CoP 坐标有意义 */ + +/* ═══════════════════════════════════════════════════════════════════ + * 帧结构定义 + * ═══════════════════════════════════════════════════════════════════ */ + +/* ─── 上行: CoP 帧 (11 bytes, 50 Hz) ─── + * [AA 55 01 flags cop_x(2) cop_y(2) force(2) crc] + */ +struct cop_frame_t { + uint8_t sync0; + uint8_t sync1; + uint8_t type; + uint8_t flags; + int16_t cop_x; + int16_t cop_y; + int16_t force; + uint8_t crc; +} __attribute__((packed)); + +union cop_pkt_t { + struct cop_frame_t frame; + uint8_t bytes[sizeof(struct cop_frame_t)]; +}; + +/* ─── 上行: 电机编码器帧 (13 bytes, 预留) ─── + * [AA 55 02 flags cable_length(4) velocity(4) crc] + */ +struct encoder_frame_t { + uint8_t sync0; + uint8_t sync1; + uint8_t type; + uint8_t flags; + float cable_length; + float velocity; + uint8_t crc; +} __attribute__((packed)); + +union encoder_pkt_t { + struct encoder_frame_t frame; + uint8_t bytes[sizeof(struct encoder_frame_t)]; +}; + +/* ─── 下行: 阻力参数帧 (16 bytes) ─── + * [AA 55 10 K(4) B(4) Tau(4) crc] + */ +struct resistance_frame_t { + uint8_t sync0; + uint8_t sync1; + uint8_t type; + float K; + float B; + float Tau; + uint8_t crc; +} __attribute__((packed)); + +union resistance_pkt_t { + struct resistance_frame_t frame; + uint8_t bytes[sizeof(struct resistance_frame_t)]; +}; + +/* ─── 下行: Spotter Mode 帧 (9 bytes) ─── + * [AA 55 11 threshold(4) enable crc] + */ +struct spotter_frame_t { + uint8_t sync0; + uint8_t sync1; + uint8_t type; + float threshold; + uint8_t enable; + uint8_t crc; +} __attribute__((packed)); + +union spotter_pkt_t { + struct spotter_frame_t frame; + uint8_t bytes[sizeof(struct spotter_frame_t)]; +}; + +/* ─── 下行: 心跳帧 (5 bytes, ~1 Hz) ─── + * [AA 55 20 counter crc] + */ +struct heartbeat_frame_t { + uint8_t sync0; + uint8_t sync1; + uint8_t type; + uint8_t counter; + uint8_t crc; +} __attribute__((packed)); + +union heartbeat_pkt_t { + struct heartbeat_frame_t frame; + uint8_t bytes[sizeof(struct heartbeat_frame_t)]; +}; + +/* ═══════════════════════════════════════════════════════════════════ + * 标定子命令码 & 状态码 + * ═══════════════════════════════════════════════════════════════════ */ + +#define CAL_SUBCMD_START_L1 0x01U +#define CAL_SUBCMD_TARE_CH 0x02U +#define CAL_SUBCMD_GAIN_CH 0x03U +#define CAL_SUBCMD_COMMIT_L1 0x04U +#define CAL_SUBCMD_ABORT 0x05U +#define CAL_SUBCMD_START_L2 0x10U +#define CAL_SUBCMD_RECORD_GRID 0x11U +#define CAL_SUBCMD_COMMIT_L2 0x12U +#define CAL_SUBCMD_ERASE 0x20U +#define CAL_SUBCMD_QUERY 0x21U + +#define CAL_STATUS_OK 0x00U +#define CAL_STATUS_ERR_STATE 0x01U +#define CAL_STATUS_ERR_NVS 0x02U +#define CAL_STATUS_ERR_PARAM 0x03U + +/* ─── 下行: 标定命令帧 (10 bytes) ─── + * [AA 55 30 subcmd target param(4) crc] + */ +struct cal_cmd_frame_t { + uint8_t sync0; + uint8_t sync1; + uint8_t type; + uint8_t subcmd; + uint8_t target; + float param; + uint8_t crc; +} __attribute__((packed)); + +union cal_cmd_pkt_t { + struct cal_cmd_frame_t frame; + uint8_t bytes[sizeof(struct cal_cmd_frame_t)]; +}; + +/* ─── 上行: 标定响应帧 (14 bytes) ─── + * [AA 55 31 status subcmd data(8) crc] + */ +struct cal_resp_frame_t { + uint8_t sync0; + uint8_t sync1; + uint8_t type; + uint8_t status; + uint8_t subcmd; + uint8_t data[8]; + uint8_t crc; +} __attribute__((packed)); + +union cal_resp_pkt_t { + struct cal_resp_frame_t frame; + uint8_t bytes[sizeof(struct cal_resp_frame_t)]; +}; + +/* ═══════════════════════════════════════════════════════════════════ + * 下行消息队列 payload + * ═══════════════════════════════════════════════════════════════════ */ + +struct comm_msg_resistance { + float K, B, Tau; +}; + +struct comm_msg_spotter { + float threshold; + bool enable; +}; + +struct comm_msg_heartbeat { + uint8_t counter; +}; + +enum comm_msg_type { + COMM_MSG_RESISTANCE, + COMM_MSG_SPOTTER, + COMM_MSG_HEARTBEAT, +}; + +struct comm_msg { + enum comm_msg_type type; + union { + struct comm_msg_resistance resistance; + struct comm_msg_spotter spotter; + struct comm_msg_heartbeat heartbeat; + }; +}; + +/* ═══════════════════════════════════════════════════════════════════ + * 协议层 API + * ═══════════════════════════════════════════════════════════════════ */ + +/** + * @brief 初始化协议层，注册 BLE 传输层收数据回调。 + * + * 必须在 ble_transport_init() 之后调用。 + * + * @retval 0 成功。 + */ +int comm_protocol_init(void); + +/** + * @brief 组包并发送 CoP 上行帧。 + * + * @param flags COP_FLAG_* 位组合。 + * @param cop_x 压力中心 X (cm)。 + * @param cop_y 压力中心 Y (cm)。 + * @param force 总力 (kg)。 + */ +void comm_protocol_send_cop(uint8_t flags, int16_t cop_x, int16_t cop_y, int16_t force); + +/** + * @brief 组包并发送标定响应上行帧。 + * + * @param status 结果状态码 (CAL_STATUS_*)。 + * @param subcmd 对应的子命令回显。 + * @param data 8 字节响应数据。 + */ +void comm_protocol_send_cal_resp(uint8_t status, uint8_t subcmd, const uint8_t data[8]); + +/** + * @brief 获取下行消息队列指针，业务层通过 k_msgq_get() 消费。 + * + * @return 指向内部 msgq 的指针。 + */ +struct k_msgq *comm_protocol_get_msgq(void); diff --git a/inc/protocol.h b/inc/protocol.h deleted file mode 100644 index 1d9ceea..0000000 --- a/inc/protocol.h +++ /dev/null @@ -1,136 +0,0 @@ -#pragma once -#include -#include -#include - -/* ─── 帧常量 ─── */ -/* 双帧头比单字节魔数更容易在连续字节流里完成包起始判定。 */ -#define PROTO_SYNC0 0xAAU -#define PROTO_SYNC1 0x55U -#define PROTO_TYPE_COP 0x01U /* 上行: 压力中心 (CoP) 帧 */ -#define PROTO_TYPE_ENCODER 0x02U /* 上行: 电机编码器帧（预留） */ -#define PROTO_TYPE_RESISTANCE 0x10U /* 下行: 阻力参数 (K, B, Tau) */ -#define PROTO_TYPE_SPOTTER 0x11U /* 下行: Spotter Mode 保护阈值 */ -#define PROTO_TYPE_HEARTBEAT 0x20U /* 下行: 心跳包 */ - -/* CoP flags 位定义 */ -#define COP_FLAG_FORCE_VALID 0x01U /* 总力 > 阈值，CoP 坐标有意义 */ - -/* ─── 板面几何 & 传感器标定 ─── */ -/* 传感器到板面中心的半宽、半长 (cm)。 */ -#define BOARD_HALF_WIDTH_CM 42.5f -#define BOARD_HALF_LENGTH_CM 16.0f - -/* - * ADC → kg 换算推导： - * GML670 50kg: 灵敏度 1.75 mV/V, 激励 5V → 满量程输出 8.75 mV - * ADS1256: PGA=64, VREF=2.5V → 满量程 ±78.125 mV - * 50kg 对应 ADC counts = 8.75 / 78.125 × 8388607 ≈ 939524 - * 1 LSB = 50.0 / 939524 ≈ 5.322e-5 kg - * 如实际激励电压或 VREF 不同，按比例修正此系数。 - */ -#define SENSOR_EXCITATION_V 5.0f -#define SENSOR_SENSITIVITY_MVV 1.75f -#define SENSOR_RATED_LOAD_KG 50.0f -#define ADS1256_VREF_V 2.5f -#define ADS1256_PGA 64 - -#define ADC_COUNTS_AT_RATED \ - ((SENSOR_SENSITIVITY_MVV * SENSOR_EXCITATION_V) / (2.0f * ADS1256_VREF_V * 1000.0f / ADS1256_PGA) * 8388607.0f) -#define ADC_TO_FORCE_SCALE (SENSOR_RATED_LOAD_KG / ADC_COUNTS_AT_RATED) - -/* 总重低于此值时 CoP 无意义（除零保护），单位 kg */ -#define COP_MIN_FORCE_THRESHOLD 0.5f - -/* ─── 上行: CoP 帧 (11 bytes, 50 Hz) ─── - * - * [AA 55 01 flags cop_x(2) cop_y(2) force(2) crc] - */ -struct cop_frame_t { - uint8_t sync0; /* = PROTO_SYNC0 */ - uint8_t sync1; /* = PROTO_SYNC1 */ - uint8_t type; /* = PROTO_TYPE_COP */ - uint8_t flags; /* bit0: force_valid */ - int16_t cop_x; /* 压力中心 X (cm)，板面坐标系 */ - int16_t cop_y; /* 压力中心 Y (cm)，板面坐标系 */ - int16_t force; /* 总重量 (kg) */ - uint8_t crc; /* CRC-8/MAXIM, 校验 type~force */ -} __attribute__((packed)); - -union cop_pkt_t { - struct cop_frame_t frame; - uint8_t bytes[sizeof(struct cop_frame_t)]; -}; - -/* ─── 上行: 电机编码器帧 (13 bytes, 预留) ─── - * - * [AA 55 02 flags cable_length(4) velocity(4) crc] - */ -struct encoder_frame_t { - uint8_t sync0; /* = PROTO_SYNC0 */ - uint8_t sync1; /* = PROTO_SYNC1 */ - uint8_t type; /* = PROTO_TYPE_ENCODER */ - uint8_t flags; /* 预留，填 0 */ - float cable_length; /* 拉索长度 (mm) */ - float velocity; /* 拉索速度 (mm/s) */ - uint8_t crc; /* CRC-8/MAXIM */ -} __attribute__((packed)); - -union encoder_pkt_t { - struct encoder_frame_t frame; - uint8_t bytes[sizeof(struct encoder_frame_t)]; -}; - -/* ─── 下行: 阻力参数帧 (16 bytes) ─── - * - * [AA 55 10 K(4) B(4) Tau(4) crc] - */ -struct resistance_frame_t { - uint8_t sync0; /* = PROTO_SYNC0 */ - uint8_t sync1; /* = PROTO_SYNC1 */ - uint8_t type; /* = PROTO_TYPE_RESISTANCE */ - float K; /* 刚度 (N/m) */ - float B; /* 阻尼 (Ns/m) */ - float Tau; /* 时间常数 (s) */ - uint8_t crc; /* CRC-8/MAXIM */ -} __attribute__((packed)); - -union resistance_pkt_t { - struct resistance_frame_t frame; - uint8_t bytes[sizeof(struct resistance_frame_t)]; -}; - -/* ─── 下行: Spotter Mode 帧 (9 bytes) ─── - * - * [AA 55 11 threshold(4) enable crc] - */ -struct spotter_frame_t { - uint8_t sync0; /* = PROTO_SYNC0 */ - uint8_t sync1; /* = PROTO_SYNC1 */ - uint8_t type; /* = PROTO_TYPE_SPOTTER */ - float threshold; /* 保护力阈值 (N) */ - uint8_t enable; /* 0 = 关闭, 1 = 启用 */ - uint8_t crc; /* CRC-8/MAXIM */ -} __attribute__((packed)); - -union spotter_pkt_t { - struct spotter_frame_t frame; - uint8_t bytes[sizeof(struct spotter_frame_t)]; -}; - -/* ─── 下行: 心跳帧 (5 bytes, ~1 Hz) ─── - * - * [AA 55 20 counter crc] - */ -struct heartbeat_frame_t { - uint8_t sync0; /* = PROTO_SYNC0 */ - uint8_t sync1; /* = PROTO_SYNC1 */ - uint8_t type; /* = PROTO_TYPE_HEARTBEAT */ - uint8_t counter; /* 滚动计数，用于活性检测 */ - uint8_t crc; /* CRC-8/MAXIM */ -} __attribute__((packed)); - -union heartbeat_pkt_t { - struct heartbeat_frame_t frame; - uint8_t bytes[sizeof(struct heartbeat_frame_t)]; -}; diff --git a/src/ble_transport.c b/src/ble_transport.c index 5bfbfb7..bc22dbc 100644 --- a/src/ble_transport.c +++ b/src/ble_transport.c @@ -1,8 +1,6 @@ #include "ble_transport.h" -#include "protocol.h" #include -#include #include #include #include @@ -15,12 +13,10 @@ LOG_MODULE_REGISTER(ble_transport, LOG_LEVEL_INF); #define BT_UUID_NUS_VAL BT_UUID_128_ENCODE(0x6e400001, 0xb5a3, 0xf393, 0xe0a9, 0xe50e24dcca9e) /* --- 广播数据 --- */ -/* 广播包只放 NUS UUID，让中央设备按服务快速筛到本设备 */ static const struct bt_data ad[] = { BT_DATA_BYTES(BT_DATA_FLAGS, (BT_LE_AD_GENERAL | BT_LE_AD_NO_BREDR)), BT_DATA_BYTES(BT_DATA_UUID128_ALL, BT_UUID_NUS_VAL), }; -/* 扫描响应单独带设备名 */ static const struct bt_data sd[] = { BT_DATA(BT_DATA_NAME_COMPLETE, CONFIG_BT_DEVICE_NAME, sizeof(CONFIG_BT_DEVICE_NAME) - 1), }; @@ -30,34 +26,16 @@ static struct bt_conn *current_conn; static volatile bool nus_notification_enabled; static struct k_work_delayable adv_work; -/* --- 下行参数 --- */ -static float resistance_K; -static float resistance_B; -static float resistance_Tau; -static float spotter_threshold; -static bool spotter_enabled; -static int64_t last_heartbeat_ms; +/* 上层收数据回调，必须在 ble_transport_adv_start() 前完成注册 */ +static ble_rx_cb_t rx_cb; -/** - * @brief 延时广播任务处理函数，用于断链后重新进入可连接状态。 - * - * @param work Zephyr delayable work 入口参数，当前实现未直接使用。 - * - * @return 无返回值。 - */ +/** @brief 延时广播任务，断链后重新进入可连接状态。 */ static void adv_work_handler(struct k_work *work) { ARG_UNUSED(work); ble_transport_adv_start(); } -/** - * @brief 蓝牙连接建立回调，接管当前连接并更新连接参数。 - * - * @param conn 新建立的连接对象。 - * @param err 连接建立阶段的状态码；非 0 表示本次连接失败。 - * - * @return 无返回值。 - */ +/** @brief 连接建立回调，接管连接引用并收紧连接参数。 */ static void connected(struct bt_conn *conn, uint8_t err) { if (err) return; @@ -65,19 +43,11 @@ static void connected(struct bt_conn *conn, uint8_t err) { k_work_cancel_delayable(&adv_work); nus_notification_enabled = false; - /* 收紧连接参数，降低数据发送抖动 */ struct bt_le_conn_param param = { .interval_min = 6, .interval_max = 12, .latency = 0, .timeout = 400 }; bt_conn_le_param_update(conn, ¶m); } -/** - * @brief 蓝牙断开回调，释放连接引用并安排后续重启广播。 - * - * @param conn 已断开的连接对象。 - * @param reason 协议栈给出的断开原因码。 - * - * @return 无返回值。 - */ +/** @brief 断开回调，释放连接引用并安排延时重广播。 */ static void disconnected(struct bt_conn *conn, uint8_t reason) { ARG_UNUSED(conn); ARG_UNUSED(reason); @@ -86,80 +56,21 @@ static void disconnected(struct bt_conn *conn, uint8_t reason) { current_conn = NULL; } nus_notification_enabled = false; - - /* 延迟 1 秒再重启广播，给协议栈留状态回收时间 */ k_work_schedule(&adv_work, K_MSEC(1000)); } -/** - * @brief NUS 通知状态回调，用于同步记录对端是否已开启通知。 - * - * @param status 当前 NUS 发送通道的可用状态。 - * - * @return 无返回值。 - */ +/** @brief NUS 通知使能状态回调。 */ static void nus_send_enabled(enum bt_nus_send_status status) { nus_notification_enabled = (status == BT_NUS_SEND_STATUS_ENABLED); } /** - * @brief 处理来自中央设备的下行协议帧，并更新本地参数缓存。 - * - * @param conn 当前接收数据的蓝牙连接对象，当前实现未直接使用。 - * @param data 下行字节流起始地址。 - * @param len 本次收到的字节数。 - * - * @return 无返回值。 + * @brief NUS 收数据回调，直接转发给上层注册的处理函数。 */ static void nus_received_cb(struct bt_conn *conn, const uint8_t *data, uint16_t len) { ARG_UNUSED(conn); - if (len < 3 || data[0] != PROTO_SYNC0 || data[1] != PROTO_SYNC1) return; - - switch (data[2]) { - case PROTO_TYPE_RESISTANCE: - if (len >= sizeof(struct resistance_frame_t)) { - uint8_t crc = crc8(data + 2, sizeof(struct resistance_frame_t) - 3, 0x31, 0x00, true); - if (data[sizeof(struct resistance_frame_t) - 1] != crc) { - LOG_WRN("Resistance frame CRC mismatch"); - break; - } - union resistance_pkt_t pkt; - memcpy(pkt.bytes, data, sizeof(struct resistance_frame_t)); - resistance_K = pkt.frame.K; - resistance_B = pkt.frame.B; - resistance_Tau = pkt.frame.Tau; - LOG_INF( - "RX Resistance: K=%.1f B=%.1f Tau=%.2f", (double)resistance_K, (double)resistance_B, - (double)resistance_Tau); - } - break; - case PROTO_TYPE_SPOTTER: - if (len >= sizeof(struct spotter_frame_t)) { - uint8_t crc = crc8(data + 2, sizeof(struct spotter_frame_t) - 3, 0x31, 0x00, true); - if (data[sizeof(struct spotter_frame_t) - 1] != crc) { - LOG_WRN("Spotter frame CRC mismatch"); - break; - } - union spotter_pkt_t pkt; - memcpy(pkt.bytes, data, sizeof(struct spotter_frame_t)); - spotter_threshold = pkt.frame.threshold; - spotter_enabled = pkt.frame.enable != 0; - LOG_INF("RX Spotter: threshold=%.1f enable=%d", (double)spotter_threshold, spotter_enabled); - } - break; - case PROTO_TYPE_HEARTBEAT: - if (len >= sizeof(struct heartbeat_frame_t)) { - uint8_t crc = crc8(data + 2, sizeof(struct heartbeat_frame_t) - 3, 0x31, 0x00, true); - if (data[sizeof(struct heartbeat_frame_t) - 1] != crc) { - LOG_WRN("Heartbeat frame CRC mismatch"); - break; - } - last_heartbeat_ms = k_uptime_get(); - LOG_DBG("RX Heartbeat"); - } - break; - default: - break; + if (rx_cb) { + rx_cb(data, len); } } @@ -169,12 +80,6 @@ static struct bt_nus_cb nus_cb = { }; BT_CONN_CB_DEFINE(conn_callbacks) = { .connected = connected, .disconnected = disconnected }; -/** - * @brief 初始化 BLE 协议栈、NUS 服务和内部状态。 - * - * @retval 0 BLE 协议栈和 NUS 服务初始化成功。 - * @retval 负值 Zephyr 蓝牙或 NUS 初始化阶段返回的具体错误码。 - */ int ble_transport_init(void) { k_work_init_delayable(&adv_work, adv_work_handler); @@ -196,94 +101,24 @@ int ble_transport_init(void) { return 0; } -/** - * @brief 启动 BLE 广播。 - * - * @return 无返回值。 - */ void ble_transport_adv_start(void) { bt_le_adv_start(BT_LE_ADV_CONN_FAST_2, ad, ARRAY_SIZE(ad), sd, ARRAY_SIZE(sd)); } -/** - * @brief 通过 NUS 发送二进制帧，自动重试最多 3 次。 - * - * @param data 待发送字节缓冲区。 - * @param len 字节数。 - * - * @return 无返回值。 - */ void ble_transport_send(const uint8_t *data, uint16_t len) { if (!nus_notification_enabled || !current_conn) return; - /* NUS 偶发忙时短间隔重试减少丢包 */ for (int i = 0; i < 3; i++) { if (bt_nus_send(current_conn, data, len) == 0) return; k_usleep(500); } + LOG_WRN("NUS send failed after retries (len=%u)", len); } -/** - * @brief 检查是否已连接且通知已使能。 - * - * @retval true 当前可以通过 NUS 对外发送数据。 - * @retval false 当前未连接或对端尚未开启通知。 - */ bool ble_transport_is_ready(void) { return nus_notification_enabled && (current_conn != NULL); } -/** - * @brief 获取最近一次下发的阻力刚度参数 K。 - * - * @return 当前缓存的阻力刚度参数 K。 - */ -float ble_transport_get_resistance_K(void) { - return resistance_K; -} - -/** - * @brief 获取最近一次下发的阻力阻尼参数 B。 - * - * @return 当前缓存的阻力阻尼参数 B。 - */ -float ble_transport_get_resistance_B(void) { - return resistance_B; -} - -/** - * @brief 获取最近一次下发的阻力时间常数 Tau。 - * - * @return 当前缓存的阻力时间常数 Tau。 - */ -float ble_transport_get_resistance_Tau(void) { - return resistance_Tau; -} - -/** - * @brief 获取最近一次下发的 Spotter 保护阈值。 - * - * @return 当前缓存的 Spotter 阈值。 - */ -float ble_transport_get_spotter_threshold(void) { - return spotter_threshold; -} - -/** - * @brief 获取最近一次下发的 Spotter 开关状态。 - * - * @retval true Spotter 保护当前处于启用状态。 - * @retval false Spotter 保护当前处于关闭状态。 - */ -bool ble_transport_get_spotter_enabled(void) { - return spotter_enabled; -} - -/** - * @brief 获取最近一次收到心跳包的系统时间戳。 - * - * @return 最近一次收到心跳包时的 uptime 毫秒值。 - */ -int64_t ble_transport_get_last_heartbeat_ms(void) { - return last_heartbeat_ms; +void ble_transport_register_rx_cb(ble_rx_cb_t cb) { + rx_cb = cb; } diff --git a/src/comm_protocol.c b/src/comm_protocol.c new file mode 100644 index 0000000..da30861 --- /dev/null +++ b/src/comm_protocol.c @@ -0,0 +1,158 @@ +#include "comm_protocol.h" +#include "ble_transport.h" +#include "calibration.h" + +#include +#include +#include + +LOG_MODULE_REGISTER(comm_protocol, LOG_LEVEL_INF); + +#define MSGQ_DEPTH 8 + +K_MSGQ_DEFINE(rx_msgq, sizeof(struct comm_msg), MSGQ_DEPTH, 4); + +/** + * @brief 计算帧 CRC-8/MAXIM，校验范围为 type 字段到 payload 末尾。 + * + * @param frame_bytes 完整帧的字节起始地址。 + * @param frame_size 帧总字节数（含 sync + crc）。 + * + * @return CRC-8 值。 + */ +static inline uint8_t frame_crc(const uint8_t *frame_bytes, size_t frame_size) { + /* 跳过 sync0+sync1，校验到倒数第二字节（crc 字段之前） */ + return crc8(frame_bytes + 2, frame_size - 3, 0x31, 0x00, true); +} + +/** + * @brief BLE 收数据回调，解析下行帧并推入消息队列。 + * + * @param data 原始字节流。 + * @param len 字节数。 + */ +static void protocol_rx_handler(const uint8_t *data, uint16_t len) { + if (len < 3 || data[0] != PROTO_SYNC0 || data[1] != PROTO_SYNC1) return; + + struct comm_msg msg; + + switch (data[2]) { + case PROTO_TYPE_RESISTANCE: + if (len < sizeof(struct resistance_frame_t)) return; + if (data[sizeof(struct resistance_frame_t) - 1] != frame_crc(data, sizeof(struct resistance_frame_t))) { + LOG_WRN("Resistance frame CRC mismatch"); + return; + } + union resistance_pkt_t rpkt; + memcpy(rpkt.bytes, data, sizeof(struct resistance_frame_t)); + msg.type = COMM_MSG_RESISTANCE; + msg.resistance.K = rpkt.frame.K; + msg.resistance.B = rpkt.frame.B; + msg.resistance.Tau = rpkt.frame.Tau; + LOG_INF( + "RX Resistance: K=%.1f B=%.1f Tau=%.2f", (double)msg.resistance.K, (double)msg.resistance.B, + (double)msg.resistance.Tau); + break; + + case PROTO_TYPE_SPOTTER: + if (len < sizeof(struct spotter_frame_t)) return; + if (data[sizeof(struct spotter_frame_t) - 1] != frame_crc(data, sizeof(struct spotter_frame_t))) { + LOG_WRN("Spotter frame CRC mismatch"); + return; + } + union spotter_pkt_t spkt; + memcpy(spkt.bytes, data, sizeof(struct spotter_frame_t)); + msg.type = COMM_MSG_SPOTTER; + msg.spotter.threshold = spkt.frame.threshold; + msg.spotter.enable = spkt.frame.enable != 0; + LOG_INF("RX Spotter: threshold=%.1f enable=%d", (double)msg.spotter.threshold, msg.spotter.enable); + break; + + case PROTO_TYPE_HEARTBEAT: + if (len < sizeof(struct heartbeat_frame_t)) return; + if (data[sizeof(struct heartbeat_frame_t) - 1] != frame_crc(data, sizeof(struct heartbeat_frame_t))) { + LOG_WRN("Heartbeat frame CRC mismatch"); + return; + } + msg.type = COMM_MSG_HEARTBEAT; + msg.heartbeat.counter = data[3]; + LOG_DBG("RX Heartbeat"); + break; + + /* 标定命令不走 msgq：cal 模块自带入队/pending 机制， + * sensor 线程通过 cal_execute_pending() 消费，无需额外中转 */ + case PROTO_TYPE_CAL_CMD: + if (len < sizeof(struct cal_cmd_frame_t)) return; + if (data[sizeof(struct cal_cmd_frame_t) - 1] != frame_crc(data, sizeof(struct cal_cmd_frame_t))) { + LOG_WRN("Cal cmd frame CRC mismatch"); + return; + } + { + union cal_cmd_pkt_t cpkt; + memcpy(cpkt.bytes, data, sizeof(struct cal_cmd_frame_t)); + + int err = cal_enqueue_command(cpkt.frame.subcmd, cpkt.frame.target, cpkt.frame.param); + if (err == 0) { + LOG_INF( + "RX CalCmd: sub=0x%02x target=%u param=%.2f", cpkt.frame.subcmd, cpkt.frame.target, + (double)cpkt.frame.param); + } else if (err == -EBUSY) { + uint8_t resp[8] = { cpkt.frame.target, 0 }; + comm_protocol_send_cal_resp(CAL_STATUS_ERR_STATE, cpkt.frame.subcmd, resp); + LOG_WRN("Cal cmd busy (sub=0x%02x)", cpkt.frame.subcmd); + } else if (err == -EINVAL) { + uint8_t resp[8] = { cpkt.frame.target, 0 }; + comm_protocol_send_cal_resp(CAL_STATUS_ERR_PARAM, cpkt.frame.subcmd, resp); + LOG_WRN("Cal cmd invalid (sub=0x%02x)", cpkt.frame.subcmd); + } else { + uint8_t resp[8] = { cpkt.frame.target, 0 }; + comm_protocol_send_cal_resp(CAL_STATUS_ERR_STATE, cpkt.frame.subcmd, resp); + LOG_ERR("Cal cmd unexpected err=%d (sub=0x%02x)", err, cpkt.frame.subcmd); + } + } + return; + + default: + LOG_DBG("Unknown frame type: 0x%02x", data[2]); + return; + } + + if (k_msgq_put(&rx_msgq, &msg, K_NO_WAIT)) { + LOG_WRN("rx_msgq full, dropped type=%d", msg.type); + } +} + +int comm_protocol_init(void) { + ble_transport_register_rx_cb(protocol_rx_handler); + LOG_INF("comm_protocol initialized"); + return 0; +} + +void comm_protocol_send_cop(uint8_t flags, int16_t cop_x, int16_t cop_y, int16_t force) { + union cop_pkt_t pkt; + pkt.frame.sync0 = PROTO_SYNC0; + pkt.frame.sync1 = PROTO_SYNC1; + pkt.frame.type = PROTO_TYPE_COP; + pkt.frame.flags = flags; + pkt.frame.cop_x = cop_x; + pkt.frame.cop_y = cop_y; + pkt.frame.force = force; + pkt.frame.crc = frame_crc(pkt.bytes, sizeof(pkt.bytes)); + ble_transport_send(pkt.bytes, sizeof(pkt.bytes)); +} + +void comm_protocol_send_cal_resp(uint8_t status, uint8_t subcmd, const uint8_t data[8]) { + union cal_resp_pkt_t pkt; + pkt.frame.sync0 = PROTO_SYNC0; + pkt.frame.sync1 = PROTO_SYNC1; + pkt.frame.type = PROTO_TYPE_CAL_RESP; + pkt.frame.status = status; + pkt.frame.subcmd = subcmd; + memcpy(pkt.frame.data, data, 8); + pkt.frame.crc = frame_crc(pkt.bytes, sizeof(pkt.bytes)); + ble_transport_send(pkt.bytes, sizeof(pkt.bytes)); +} + +struct k_msgq *comm_protocol_get_msgq(void) { + return &rx_msgq; +} diff --git a/src/main.c b/src/main.c index e3778e3..ccc14f2 100644 --- a/src/main.c +++ b/src/main.c @@ -9,6 +9,8 @@ #include "ble_transport.h" #include "button.h" +#include "calibration.h" +#include "comm_protocol.h" #include "sensor.h" #include @@ -27,8 +29,14 @@ int main(void) { LOG_INF("--- GML670 System (CoP Binary Protocol) ---"); ble_transport_init(); + comm_protocol_init(); ble_transport_adv_start(); + if (cal_init()) { + LOG_ERR("Failed to initialize calibration"); + return -1; + } + if (button_init()) { LOG_ERR("Failed to initialize buttons"); return -1; From 06b1f642136aea25e16435dd5df666d018c4c410 Mon Sep 17 00:00:00 2001 From: pNexus Date: Fri, 15 May 2026 21:34:50 +0800 Subject: [PATCH 15/21] =?UTF-8?q?feat(config):=20=E6=B7=BB=E5=8A=A0=20hci?= =?UTF-8?q?=5Fipc=20=E7=BD=91=E7=BB=9C=E6=A0=B8=E9=85=8D=E7=BD=AE=EF=BC=8C?= =?UTF-8?q?=E4=BC=98=E5=8C=96=20DLE=20=E5=92=8C=20ACL=20=E7=BC=93=E5=86=B2?= =?UTF-8?q?=E5=8C=BA=E8=AE=BE=E7=BD=AE?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- prj.conf | 30 +++++++++++++++++++----------- src/ble_transport.c | 12 +++++++++++- sysbuild.cmake | 6 ++++++ sysbuild.conf | 2 ++ sysbuild/hci_ipc.conf | 9 +++++++++ 5 files changed, 47 insertions(+), 12 deletions(-) create mode 100644 sysbuild.cmake create mode 100644 sysbuild.conf create mode 100644 sysbuild/hci_ipc.conf diff --git a/prj.conf b/prj.conf index caf22e6..f026f4c 100644 --- a/prj.conf +++ b/prj.conf @@ -5,23 +5,28 @@ CONFIG_BT_DEVICE_NAME="GML670_System" CONFIG_BT_MAX_CONN=1 CONFIG_BT_NUS=y -# 借鉴 Kiwii: 禁用 NUS 安全限制，提高连接成功率 -CONFIG_BT_NUS_SECURITY_ENABLED=n +# DLE + PHY +CONFIG_BT_BUF_ACL_RX_SIZE=251 +CONFIG_BT_BUF_ACL_TX_SIZE=251 +CONFIG_BT_DATA_LEN_UPDATE=y +CONFIG_BT_USER_DATA_LEN_UPDATE=y +CONFIG_BT_AUTO_DATA_LEN_UPDATE=y +CONFIG_BT_L2CAP_TX_MTU=247 +CONFIG_BT_PHY_UPDATE=y +CONFIG_BT_USER_PHY_UPDATE=y +CONFIG_BT_AUTO_PHY_UPDATE=y -# SMP 配对与绑定 -CONFIG_BT_SMP=y -CONFIG_BT_SETTINGS=y -CONFIG_BT_KEYS_OVERWRITE_OLDEST=y +# 缓冲区 +CONFIG_BT_BUF_ACL_TX_COUNT=3 +CONFIG_BT_L2CAP_TX_BUF_COUNT=6 + +# 存储 CONFIG_FLASH=y CONFIG_FLASH_MAP=y CONFIG_NVS=y CONFIG_SETTINGS=y CONFIG_SETTINGS_NVS=y -# 借鉴 Kiwii: 优化缓冲区 (匹配硬件) -CONFIG_BT_BUF_ACL_TX_COUNT=3 -CONFIG_BT_L2CAP_TX_BUF_COUNT=3 - # 硬件驱动 CONFIG_SPI=y CONFIG_GPIO=y @@ -34,8 +39,11 @@ CONFIG_SERIAL=n CONFIG_LOG=y CONFIG_USE_SEGGER_RTT=y CONFIG_LOG_BACKEND_RTT=y +CONFIG_LOG_BACKEND_RTT_MODE_DROP=y CONFIG_LOG_BACKEND_UART=n -CONFIG_LOG_MODE_IMMEDIATE=y +CONFIG_LOG_MODE_DEFERRED=y +CONFIG_LOG_BUFFER_SIZE=4096 +CONFIG_SEGGER_RTT_BUFFER_SIZE_UP=4096 CONFIG_MAIN_STACK_SIZE=4096 CONFIG_CRC=y diff --git a/src/ble_transport.c b/src/ble_transport.c index bc22dbc..e8c96cb 100644 --- a/src/ble_transport.c +++ b/src/ble_transport.c @@ -62,6 +62,7 @@ static void disconnected(struct bt_conn *conn, uint8_t reason) { /** @brief NUS 通知使能状态回调。 */ static void nus_send_enabled(enum bt_nus_send_status status) { nus_notification_enabled = (status == BT_NUS_SEND_STATUS_ENABLED); + LOG_INF("NUS send_enabled: status=%d, enabled=%d", status, nus_notification_enabled); } /** @@ -106,10 +107,19 @@ void ble_transport_adv_start(void) { } void ble_transport_send(const uint8_t *data, uint16_t len) { + static bool first_send_logged = false; + if (!nus_notification_enabled || !current_conn) return; for (int i = 0; i < 3; i++) { - if (bt_nus_send(current_conn, data, len) == 0) return; + int err = bt_nus_send(current_conn, data, len); + if (err == 0) { + if (!first_send_logged) { + LOG_INF("First NUS send OK (len=%u)", len); + first_send_logged = true; + } + return; + } k_usleep(500); } LOG_WRN("NUS send failed after retries (len=%u)", len); diff --git a/sysbuild.cmake b/sysbuild.cmake new file mode 100644 index 0000000..3448781 --- /dev/null +++ b/sysbuild.cmake @@ -0,0 +1,6 @@ +# 用绝对路径将 hci_ipc overlay 传给网络核， +# 避免 sysbuild 在 SDK 源码目录下解析相对路径而找不到文件。 +set(hci_ipc_EXTRA_CONF_FILE + ${CMAKE_CURRENT_LIST_DIR}/sysbuild/hci_ipc.conf + CACHE INTERNAL "" FORCE +) diff --git a/sysbuild.conf b/sysbuild.conf new file mode 100644 index 0000000..75e7998 --- /dev/null +++ b/sysbuild.conf @@ -0,0 +1,2 @@ +# 让 sysbuild 自动构建 hci_ipc 网络核固件，west flash 一次刷两个核 +SB_CONFIG_NETCORE_HCI_IPC=y diff --git a/sysbuild/hci_ipc.conf b/sysbuild/hci_ipc.conf new file mode 100644 index 0000000..9a7a215 --- /dev/null +++ b/sysbuild/hci_ipc.conf @@ -0,0 +1,9 @@ +# 网络核 hci_ipc overlay: 开启 DLE 251B，解除链路层分片瓶颈 + +# DLE 依赖链: DATA_LEN_UPDATE -> BT_CTLR_DATA_LENGTH -> DATA_LENGTH_MAX +CONFIG_BT_DATA_LEN_UPDATE=y +CONFIG_BT_CTLR_DATA_LENGTH_MAX=251 + +# 控制器 ACL 缓冲区必须能容纳 251B 的 LL PDU +CONFIG_BT_BUF_ACL_RX_SIZE=251 +CONFIG_BT_BUF_ACL_TX_SIZE=251 From d95cce5cf2fee25a3b616b187ac5d4cb2b06d48d Mon Sep 17 00:00:00 2001 From: pNexus Date: Tue, 19 May 2026 14:50:04 +0800 Subject: [PATCH 16/21] =?UTF-8?q?feat(ads1256):=20=E6=B7=BB=E5=8A=A0=20EMI?= =?UTF-8?q?=20=E9=98=B2=E6=8A=A4=E4=B8=8E=E5=AF=84=E5=AD=98=E5=99=A8?= =?UTF-8?q?=E8=87=AA=E6=81=A2=E5=A4=8D=E6=9C=BA=E5=88=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit CS 引脚加内部上拉防止干扰误触发；新增 ads1256_recover() 在检测到 寄存器被改写时重写关键配置并验证恢复。 --- inc/ads1256.h | 1 + src/ads1256.c | 30 +++++++++++++++++++++++++++++- 2 files changed, 30 insertions(+), 1 deletion(-) diff --git a/inc/ads1256.h b/inc/ads1256.h index 12d0131..fbdd91a 100644 --- a/inc/ads1256.h +++ b/inc/ads1256.h @@ -12,3 +12,4 @@ void ads1256_sync_wakeup(void); int ads1256_hwreset(void); int32_t ads1256_read_data(void); void ads1256_write_cmd(uint8_t cmd); +int ads1256_recover(int max_retries); diff --git a/src/ads1256.c b/src/ads1256.c index b5cd474..59c876f 100644 --- a/src/ads1256.c +++ b/src/ads1256.c @@ -179,7 +179,8 @@ int32_t ads1256_read_data(void) { */ int ads1256_init(void) { /* GPIO */ - gpio_pin_configure_dt(&cs_spec, GPIO_OUTPUT_INACTIVE); + /* CS 加内部上拉：EMI 干扰时维持高电平，阻止 ADS1256 误收命令 */ + gpio_pin_configure_dt(&cs_spec, GPIO_OUTPUT_INACTIVE | GPIO_PULL_UP); gpio_pin_configure_dt(&reset_spec, GPIO_OUTPUT_INACTIVE); gpio_pin_configure_dt(&drdy_spec, GPIO_INPUT); @@ -248,3 +249,30 @@ int ads1256_init(void) { LOG_INF("ADS1256 initialized (BUFEN=1, PGA=64, 3750SPS)"); return 0; } + +/** + * @brief EMI 恢复：重写关键寄存器并验证，用于马达干扰后自恢复。 + * + * @param max_retries 最大重试次数，每次间隔 20ms。 + * + * @retval 0 恢复成功。 + * @retval -EIO 达到最大重试次数仍未恢复。 + */ +int ads1256_recover(int max_retries) { + for (int i = 0; i < max_retries; i++) { + ads1256_write_reg(REG_STATUS, 0x06); + ads1256_write_reg(REG_ADCON, 0x07); + ads1256_write_reg(REG_DRATE, 0xC0); + ads1256_sync_wakeup(); + k_msleep(20); + + uint8_t adcon = ads1256_read_reg(REG_ADCON); + uint8_t drate = ads1256_read_reg(REG_DRATE); + if (adcon == 0x07 && drate == 0xC0) { + LOG_WRN("ADS1256 recovered after %d retries", i + 1); + return 0; + } + } + LOG_ERR("ADS1256 recovery failed after %d retries", max_retries); + return -EIO; +} From a2d6ef8893c9a07f27382d27e29452a18ea2f334 Mon Sep 17 00:00:00 2001 From: pNexus Date: Tue, 19 May 2026 14:50:10 +0800 Subject: [PATCH 17/21] =?UTF-8?q?refactor(sensor):=20=E9=87=8D=E6=9E=84?= =?UTF-8?q?=E9=87=87=E6=A0=B7=E7=AD=96=E7=95=A5=EF=BC=8CButterworth=2020Hz?= =?UTF-8?q?=20=E6=BB=A4=E6=B3=A2=20+=2050Hz=20=E8=BE=93=E5=87=BA?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - 采样次数 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 检测：采集异常短时触发寄存器恢复 --- inc/calibration.h | 19 ++++++---- src/sensor.c | 88 +++++++++++++++++++++++++++-------------------- 2 files changed, 64 insertions(+), 43 deletions(-) diff --git a/inc/calibration.h b/inc/calibration.h index 14de0ef..9e4d1c7 100644 --- a/inc/calibration.h +++ b/inc/calibration.h @@ -5,7 +5,7 @@ /* ─── 板面几何 & 传感器物理常量 ─── */ -#define BOARD_HALF_WIDTH_CM 42.5f /* 传感器到板中心 X 方向距离 (cm) */ +#define BOARD_HALF_WIDTH_CM 42.5f /* 传感器到板中心 X 方向距离 (cm) */ #define BOARD_HALF_LENGTH_CM 16.0f /* 传感器到板中心 Y 方向距离 (cm) */ /* @@ -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 /* ─── 标定常量 ─── */ diff --git a/src/sensor.c b/src/sensor.c index 6fd794d..9015aac 100644 --- a/src/sensor.c +++ b/src/sensor.c @@ -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,26 +163,27 @@ 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_x = 0; - *out_y = 0; + *out_force = 0; + *out_x = 0; + *out_y = 0; return false; } force_valid_state = true; } 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) { @@ -214,34 +230,32 @@ 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 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); } - 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); } } } From 38a70ccb8e7cf36d130c254fc73a0a905d1cdf26 Mon Sep 17 00:00:00 2001 From: pNexus Date: Tue, 19 May 2026 14:50:16 +0800 Subject: [PATCH 18/21] =?UTF-8?q?chore(ble):=20=E5=A2=9E=E5=BC=BA=E8=BF=9E?= =?UTF-8?q?=E6=8E=A5=E4=B8=8E=E5=8F=91=E9=80=81=E6=97=A5=E5=BF=97=EF=BC=8C?= =?UTF-8?q?=E8=A1=A5=E5=85=85=20README=20TODO?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - 连接/断开事件打印详细信息 - 发送统计计数（成功/失败） - README 补充 L1 标定、BR 通道排查、马达频率确认待办 --- README.md | 6 ++++++ src/ble_transport.c | 18 ++++++++++++++---- 2 files changed, 20 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index 3d9e04d..52dae32 100644 --- a/README.md +++ b/README.md @@ -369,3 +369,9 @@ Heartbeat 接收路径已经接入，但默认 `DBG` 级别下才单独打印。 - 传感器线程没有额外 `k_msleep()` 节流，实际帧率由 ADS1256 采样和 BLE 发送耗时共同决定 - README 中的频率描述按当前 `3750 SPS + 4 通道 * 17 次均值` 这套实现估算，目标运行频率约 `50 Hz` - 当前实现默认四角传感器是固定几何布局，半宽和半长都是 `100 mm` + +### 10.3 TODO + +- [ ] L1 增益标定：当前使用理论推导值 `ADC_TO_FORCE_SCALE`，实测 10 kg 砝码显示 9 kg（约 10% 误差），需用已知砝码通过标定命令校准每通道增益 +- [ ] BR 通道硬件排查：静止无马达状态下 BR 通道有 ±4600 counts 异常抖动（其余通道仅 ±500），疑似接触不良或安装松动 +- [ ] 马达振动频率确认：当前 IIR 截止 1.2 Hz，若实际振动低于帧率一半（23.5 Hz）存在混叠，可能需要在 ADC 级做抗混叠或调整采样策略 diff --git a/src/ble_transport.c b/src/ble_transport.c index e8c96cb..c6ce300 100644 --- a/src/ble_transport.c +++ b/src/ble_transport.c @@ -37,11 +37,15 @@ static void adv_work_handler(struct k_work *work) { /** @brief 连接建立回调，接管连接引用并收紧连接参数。 */ static void connected(struct bt_conn *conn, uint8_t err) { - if (err) return; + if (err) { + LOG_WRN("Connection failed (err %u)", err); + return; + } current_conn = bt_conn_ref(conn); k_work_cancel_delayable(&adv_work); nus_notification_enabled = false; + LOG_INF("BLE connected"); struct bt_le_conn_param param = { .interval_min = 6, .interval_max = 12, .latency = 0, .timeout = 400 }; bt_conn_le_param_update(conn, ¶m); @@ -50,7 +54,7 @@ static void connected(struct bt_conn *conn, uint8_t err) { /** @brief 断开回调，释放连接引用并安排延时重广播。 */ static void disconnected(struct bt_conn *conn, uint8_t reason) { ARG_UNUSED(conn); - ARG_UNUSED(reason); + LOG_WRN("BLE disconnected (reason 0x%02x)", reason); if (current_conn) { bt_conn_unref(current_conn); current_conn = NULL; @@ -108,23 +112,29 @@ void ble_transport_adv_start(void) { void ble_transport_send(const uint8_t *data, uint16_t len) { static bool first_send_logged = false; + static uint32_t send_ok_count; + static uint32_t send_fail_count; if (!nus_notification_enabled || !current_conn) return; for (int i = 0; i < 3; i++) { int err = bt_nus_send(current_conn, data, len); if (err == 0) { + send_ok_count++; if (!first_send_logged) { LOG_INF("First NUS send OK (len=%u)", len); first_send_logged = true; } return; } + if (i == 0) { + LOG_DBG("NUS send retry (err=%d)", err); + } k_usleep(500); } - LOG_WRN("NUS send failed after retries (len=%u)", len); + send_fail_count++; + LOG_WRN("NUS send failed x3 (len=%u, ok=%u, fail=%u)", len, send_ok_count, send_fail_count); } - bool ble_transport_is_ready(void) { return nus_notification_enabled && (current_conn != NULL); } From 633459caeef1c8a51e93e554ceeb8b0e51b6d330 Mon Sep 17 00:00:00 2001 From: pNexus Date: Wed, 20 May 2026 21:02:40 +0800 Subject: [PATCH 19/21] =?UTF-8?q?refactor(protocol):=20=E7=A7=BB=E9=99=A4?= =?UTF-8?q?=E6=9C=AA=E4=BD=BF=E7=94=A8=E7=9A=84=E6=B6=88=E6=81=AF=E9=98=9F?= =?UTF-8?q?=E5=88=97=E8=8E=B7=E5=8F=96=E5=87=BD=E6=95=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- inc/comm_protocol.h | 7 ------- src/comm_protocol.c | 4 ---- 2 files changed, 11 deletions(-) diff --git a/inc/comm_protocol.h b/inc/comm_protocol.h index dde1e9f..3d3643d 100644 --- a/inc/comm_protocol.h +++ b/inc/comm_protocol.h @@ -233,10 +233,3 @@ void comm_protocol_send_cop(uint8_t flags, int16_t cop_x, int16_t cop_y, int16_t * @param data 8 字节响应数据。 */ void comm_protocol_send_cal_resp(uint8_t status, uint8_t subcmd, const uint8_t data[8]); - -/** - * @brief 获取下行消息队列指针，业务层通过 k_msgq_get() 消费。 - * - * @return 指向内部 msgq 的指针。 - */ -struct k_msgq *comm_protocol_get_msgq(void); diff --git a/src/comm_protocol.c b/src/comm_protocol.c index da30861..4820bb3 100644 --- a/src/comm_protocol.c +++ b/src/comm_protocol.c @@ -152,7 +152,3 @@ void comm_protocol_send_cal_resp(uint8_t status, uint8_t subcmd, const uint8_t d pkt.frame.crc = frame_crc(pkt.bytes, sizeof(pkt.bytes)); ble_transport_send(pkt.bytes, sizeof(pkt.bytes)); } - -struct k_msgq *comm_protocol_get_msgq(void) { - return &rx_msgq; -} From 8fb033c3be3f36f20cc0c9fa786ad8880d5bf042 Mon Sep 17 00:00:00 2001 From: pNexus Date: Wed, 20 May 2026 21:12:34 +0800 Subject: [PATCH 20/21] =?UTF-8?q?feat(docs):=20=E6=B7=BB=E5=8A=A0=E5=8A=9B?= =?UTF-8?q?=E9=87=8F=E7=94=B5=E6=9C=BA=E8=AF=B4=E6=98=8E=E6=96=87=E6=A1=A3?= =?UTF-8?q?=EF=BC=8C=E5=8C=85=E5=90=AB=E8=BF=9E=E6=8E=A5=E6=B5=81=E7=A8=8B?= =?UTF-8?q?=E5=92=8C=E5=8A=9B=E6=8E=A7=E5=8F=82=E6=95=B0=E8=AE=BE=E7=BD=AE?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- doc/communication.md | 191 +++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 191 insertions(+) create mode 100644 doc/communication.md diff --git a/doc/communication.md b/doc/communication.md new file mode 100644 index 0000000..7cef201 --- /dev/null +++ b/doc/communication.md @@ -0,0 +1,191 @@ +# BLE 接入说明 + +本文档只说明客户实现“连接蓝牙”和“设置力控参数”所需的最小协议。 + +## 1. BLE 连接 + +设备使用 Nordic UART Service 风格的 BLE 服务。 + +| 用途 | UUID | 方向 | +| --- | --- | --- | +| Service | `6e400001-b5a3-f393-e0a9-e50e24dcca9e` | - | +| TX characteristic | `6e400002-b5a3-f393-e0a9-e50e24dcca9e` | 客户 App 写入设备 | +| RX characteristic | `6e400003-b5a3-f393-e0a9-e50e24dcca9e` | 设备 notify 给客户 App | + +连接流程： + +1. 扫描 BLE 设备，设备名通常包含 `YRobot`。 +2. 连接目标设备。 +3. 对 RX characteristic 开启 notify。 +4. 向 TX characteristic 写入力控参数帧。 + +单包最大长度建议不超过 `250 bytes`。 + +## 2. 左右侧设备 + +力控参数帧的 `key` 需要区分左右侧。 + +| 侧别 | 写入 key | +| --- | --- | +| 左侧 | `0x6F` | +| 右侧 | `0xAF` | +| 不区分左右侧 | `0x2F` | + +现有工具通过设备名判断左右侧： + +- 右侧：设备名包含 `ZDR`, `ZDB`, `ZCR`, `ZCB`, `ARR`, `ARB`, `ASR`, `ASB` +- 左侧：设备名包含 `ZDL`, `ZCL`, `ARL`, `ASL` + +如果设备名无法判断，客户 App 需要让用户手动选择左/右侧。 + +## 3. 力控参数帧格式 + +设置力控参数使用 JSON 字符串，通过 TX characteristic 写入。 + +帧格式： + +```text +[0] key +[1] length +[2] crc +[3] command +[4...] utf8(json) +``` + +字段说明： + +| 字段 | 说明 | +| --- | --- | +| `key` | 左侧 `0x6F`，右侧 `0xAF` | +| `length` | 整帧长度，即 `json_payload_len + 4` | +| `crc` | 固定填 `0x00` | +| `command` | 设置参数固定填 `0x01` | +| `json` | UTF-8 编码的 JSON 参数 | + +也就是： + +```text +payload = [0x01] + UTF8(json_string) +frame = [key, len(payload) + 3, 0x00] + payload +``` + +## 4. 设置力控模式和参数 + +### 力控参数表 + +所有 key 都定义在 `TuningParams::RegisterForceModeCtrl()` 中。 + +| JSON key | 默认值 | 单位 | 说明 | +| --- | ---: | --- | --- | +| `fm_mode` | `0.0` | - | 力控模式选择，四舍五入后映射到模式枚举 | +| `fm_mset` | `0.1` | kg | 用户设定重量 | +| `fm_kin` | `0.0` | - | 惯性比例系数，虚拟质量 = `fm_kin * fm_mset` | +| `fm_bfr` | `0.0` | N/(m/s) | 线性摩擦/阻尼系数 | +| `fm_kecc` | `0.1` | - | 离心倍率，离心力 = `fm_kecc * 向心力` | +| `fm_vth` | `0.3` | m/s | 离心/向心平滑切换速度阈值 | +| `fm_cdrv` | `0.1` | N/(m/s)^2 | 粘滞模式拉出方向平方阻尼系数 | +| `fm_crec` | `0.1` | N/(m/s) | 粘滞模式回收方向线性阻尼系数 | +| `fm_k` | `0.0` | N/m | 弹性刚度 | +| `fm_x0` | `0.0` | m | 弹性零点位置 | +| `fm_vmax` | `1.0` | m/s | 等速模式最大速度 | +| `fm_gwall` | `0.0` | N/(m/s) | 等速速度墙增益 | +| `fm_sp_v` | `0.1` | m/s | Spotter 触发速度阈值 | +| `fm_sp_t` | `5.0` | s | Spotter 触发时间阈值 | +| `fm_sp_d` | `5.0` | s | Spotter 减重衰减时长 | +| `fm_sp_home` | `0.0` | m | Spotter 参考初始位置 | +| `fm_sp_rng` | `-0.10` | m | Spotter 负重判定位置阈值 | +| `fm_sp_rec` | `1.0` | s | Spotter 恢复时长；当前实现中会被同步为 `fm_sp_t` | +| `fm_rad` | `0.2` | m | 滑轮半径，输出扭矩 = 输出力 * `fm_rad` | + +### 力控模式 + +`fm_mode` 会先四舍五入，再限幅到 `0...6`。 + +| `fm_mode` | 模式 | 主要参数 | 说明 | +| ---: | --- | --- | --- | +| `0` | None | - | 输出力为 0 | +| `1` | FreeWeight | `fm_mset`, `fm_kin`, `fm_bfr`, `fm_rad` | 自由重量/惯性补偿 | +| `2` | EccentricOverload | `fm_mset`, `fm_kin`, `fm_bfr`, `fm_kecc`, `fm_vth`, Spotter 参数 | 离心超负荷 | +| `3` | Viscous | `fm_cdrv`, `fm_crec`, `fm_rad` | 粘滞/流体阻尼 | +| `4` | Elastic | `fm_k`, `fm_x0`, `fm_rad` | 弹性/弹簧 | +| `5` | IsokineticSpotting | `fm_mset`, `fm_vmax`, `fm_gwall`, Spotter 参数 | 等速速度墙 | +| `6` | Spotter | `fm_mset`, `fm_kin`, `fm_bfr`, Spotter 参数 | 保护模式 | + +## 5. 示例 + +设置右侧设备为离心超负荷模式： + +```json +{ + "fm_mode": 2, + "fm_mset": 10.0, + "fm_vth": 0.3, + "fm_kecc": 1.5, + "fm_kin": 1.0, + "fm_bfr": 0.05, + "fm_rad": 0.04 +} +``` + +对应写入帧： + +```text +key = 0xAF +command = 0x01 +json_string = '{"fm_mode":2,"fm_mset":10.0,"fm_vth":0.3,"fm_kecc":1.5,"fm_kin":1.0,"fm_bfr":0.05,"fm_rad":0.04}' + +payload = [0x01] + UTF8(json_string) +frame = [0xAF, len(payload) + 3, 0x00] + payload +``` + +写入 TX characteristic UUID： + +```text +6e400002-b5a3-f393-e0a9-e50e24dcca9e +``` + +## 6. 查询当前力控参数 + +查询参数同样写 TX characteristic。 + +查询当前模式参数： + +```text +frame = [key, 0x04, 0x00, 0x02] +``` + +查询指定模式参数，例如查询 `fm_mode = 2`： + +```text +payload = [0x02] + UTF8('{"fm_mode":2}') +frame = [key, len(payload) + 3, 0x00] + payload +``` + +设备会通过 RX characteristic notify 返回 JSON 参数。返回可能分包，需要按包序号拼接： + +```text +[0] key +[1] length +[2] crc +[3] type = 0x02 +[4] packet_index +[5] packet_count +[6...] utf8(json_fragment) +``` + +当 `packet_index == packet_count` 时，说明最后一包已收到，可以拼接所有 `json_fragment` 后解析 JSON。 + +## 7. 心跳包 +当连接上设备之后，需要每隔2s发送心跳包给设备，维持和设备的连接 +```text +[0] key +[1] length +[2] crc +[3] command = 0x00 +``` +## 8. 注意事项 + +- 写入前必须先连接设备并开启 RX notify。 +- 写入 key 必须和设备侧别一致：左侧 `0x6F`，右侧 `0xAF`。 +- `crc` 当前固定填 `0x00`。 +- JSON 使用 UTF-8 编码。 From 11673a70f2fbc7ea4afa1008b2b69dab5eff4adc Mon Sep 17 00:00:00 2001 From: pNexus Date: Thu, 21 May 2026 10:07:02 +0800 Subject: [PATCH 21/21] =?UTF-8?q?feat(doc):=20=E6=B7=BB=E5=8A=A0=E5=8A=9B?= =?UTF-8?q?=E9=87=8F=E7=94=B5=E6=9C=BA=E6=8E=A5=E5=85=A5=E8=AF=B4=E6=98=8E?= =?UTF-8?q?=E6=96=87=E6=A1=A3=EF=BC=8C=E5=8C=85=E5=90=AB=E8=BF=9E=E6=8E=A5?= =?UTF-8?q?=E6=B5=81=E7=A8=8B=E5=92=8C=E5=8A=9B=E6=8E=A7=E5=8F=82=E6=95=B0?= =?UTF-8?q?=E8=AE=BE=E7=BD=AE?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- doc/strength_motor_ble.md | 191 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 191 insertions(+) create mode 100644 doc/strength_motor_ble.md diff --git a/doc/strength_motor_ble.md b/doc/strength_motor_ble.md new file mode 100644 index 0000000..7cef201 --- /dev/null +++ b/doc/strength_motor_ble.md @@ -0,0 +1,191 @@ +# BLE 接入说明 + +本文档只说明客户实现“连接蓝牙”和“设置力控参数”所需的最小协议。 + +## 1. BLE 连接 + +设备使用 Nordic UART Service 风格的 BLE 服务。 + +| 用途 | UUID | 方向 | +| --- | --- | --- | +| Service | `6e400001-b5a3-f393-e0a9-e50e24dcca9e` | - | +| TX characteristic | `6e400002-b5a3-f393-e0a9-e50e24dcca9e` | 客户 App 写入设备 | +| RX characteristic | `6e400003-b5a3-f393-e0a9-e50e24dcca9e` | 设备 notify 给客户 App | + +连接流程： + +1. 扫描 BLE 设备，设备名通常包含 `YRobot`。 +2. 连接目标设备。 +3. 对 RX characteristic 开启 notify。 +4. 向 TX characteristic 写入力控参数帧。 + +单包最大长度建议不超过 `250 bytes`。 + +## 2. 左右侧设备 + +力控参数帧的 `key` 需要区分左右侧。 + +| 侧别 | 写入 key | +| --- | --- | +| 左侧 | `0x6F` | +| 右侧 | `0xAF` | +| 不区分左右侧 | `0x2F` | + +现有工具通过设备名判断左右侧： + +- 右侧：设备名包含 `ZDR`, `ZDB`, `ZCR`, `ZCB`, `ARR`, `ARB`, `ASR`, `ASB` +- 左侧：设备名包含 `ZDL`, `ZCL`, `ARL`, `ASL` + +如果设备名无法判断，客户 App 需要让用户手动选择左/右侧。 + +## 3. 力控参数帧格式 + +设置力控参数使用 JSON 字符串，通过 TX characteristic 写入。 + +帧格式： + +```text +[0] key +[1] length +[2] crc +[3] command +[4...] utf8(json) +``` + +字段说明： + +| 字段 | 说明 | +| --- | --- | +| `key` | 左侧 `0x6F`，右侧 `0xAF` | +| `length` | 整帧长度，即 `json_payload_len + 4` | +| `crc` | 固定填 `0x00` | +| `command` | 设置参数固定填 `0x01` | +| `json` | UTF-8 编码的 JSON 参数 | + +也就是： + +```text +payload = [0x01] + UTF8(json_string) +frame = [key, len(payload) + 3, 0x00] + payload +``` + +## 4. 设置力控模式和参数 + +### 力控参数表 + +所有 key 都定义在 `TuningParams::RegisterForceModeCtrl()` 中。 + +| JSON key | 默认值 | 单位 | 说明 | +| --- | ---: | --- | --- | +| `fm_mode` | `0.0` | - | 力控模式选择，四舍五入后映射到模式枚举 | +| `fm_mset` | `0.1` | kg | 用户设定重量 | +| `fm_kin` | `0.0` | - | 惯性比例系数，虚拟质量 = `fm_kin * fm_mset` | +| `fm_bfr` | `0.0` | N/(m/s) | 线性摩擦/阻尼系数 | +| `fm_kecc` | `0.1` | - | 离心倍率，离心力 = `fm_kecc * 向心力` | +| `fm_vth` | `0.3` | m/s | 离心/向心平滑切换速度阈值 | +| `fm_cdrv` | `0.1` | N/(m/s)^2 | 粘滞模式拉出方向平方阻尼系数 | +| `fm_crec` | `0.1` | N/(m/s) | 粘滞模式回收方向线性阻尼系数 | +| `fm_k` | `0.0` | N/m | 弹性刚度 | +| `fm_x0` | `0.0` | m | 弹性零点位置 | +| `fm_vmax` | `1.0` | m/s | 等速模式最大速度 | +| `fm_gwall` | `0.0` | N/(m/s) | 等速速度墙增益 | +| `fm_sp_v` | `0.1` | m/s | Spotter 触发速度阈值 | +| `fm_sp_t` | `5.0` | s | Spotter 触发时间阈值 | +| `fm_sp_d` | `5.0` | s | Spotter 减重衰减时长 | +| `fm_sp_home` | `0.0` | m | Spotter 参考初始位置 | +| `fm_sp_rng` | `-0.10` | m | Spotter 负重判定位置阈值 | +| `fm_sp_rec` | `1.0` | s | Spotter 恢复时长；当前实现中会被同步为 `fm_sp_t` | +| `fm_rad` | `0.2` | m | 滑轮半径，输出扭矩 = 输出力 * `fm_rad` | + +### 力控模式 + +`fm_mode` 会先四舍五入，再限幅到 `0...6`。 + +| `fm_mode` | 模式 | 主要参数 | 说明 | +| ---: | --- | --- | --- | +| `0` | None | - | 输出力为 0 | +| `1` | FreeWeight | `fm_mset`, `fm_kin`, `fm_bfr`, `fm_rad` | 自由重量/惯性补偿 | +| `2` | EccentricOverload | `fm_mset`, `fm_kin`, `fm_bfr`, `fm_kecc`, `fm_vth`, Spotter 参数 | 离心超负荷 | +| `3` | Viscous | `fm_cdrv`, `fm_crec`, `fm_rad` | 粘滞/流体阻尼 | +| `4` | Elastic | `fm_k`, `fm_x0`, `fm_rad` | 弹性/弹簧 | +| `5` | IsokineticSpotting | `fm_mset`, `fm_vmax`, `fm_gwall`, Spotter 参数 | 等速速度墙 | +| `6` | Spotter | `fm_mset`, `fm_kin`, `fm_bfr`, Spotter 参数 | 保护模式 | + +## 5. 示例 + +设置右侧设备为离心超负荷模式： + +```json +{ + "fm_mode": 2, + "fm_mset": 10.0, + "fm_vth": 0.3, + "fm_kecc": 1.5, + "fm_kin": 1.0, + "fm_bfr": 0.05, + "fm_rad": 0.04 +} +``` + +对应写入帧： + +```text +key = 0xAF +command = 0x01 +json_string = '{"fm_mode":2,"fm_mset":10.0,"fm_vth":0.3,"fm_kecc":1.5,"fm_kin":1.0,"fm_bfr":0.05,"fm_rad":0.04}' + +payload = [0x01] + UTF8(json_string) +frame = [0xAF, len(payload) + 3, 0x00] + payload +``` + +写入 TX characteristic UUID： + +```text +6e400002-b5a3-f393-e0a9-e50e24dcca9e +``` + +## 6. 查询当前力控参数 + +查询参数同样写 TX characteristic。 + +查询当前模式参数： + +```text +frame = [key, 0x04, 0x00, 0x02] +``` + +查询指定模式参数，例如查询 `fm_mode = 2`： + +```text +payload = [0x02] + UTF8('{"fm_mode":2}') +frame = [key, len(payload) + 3, 0x00] + payload +``` + +设备会通过 RX characteristic notify 返回 JSON 参数。返回可能分包，需要按包序号拼接： + +```text +[0] key +[1] length +[2] crc +[3] type = 0x02 +[4] packet_index +[5] packet_count +[6...] utf8(json_fragment) +``` + +当 `packet_index == packet_count` 时，说明最后一包已收到，可以拼接所有 `json_fragment` 后解析 JSON。 + +## 7. 心跳包 +当连接上设备之后，需要每隔2s发送心跳包给设备，维持和设备的连接 +```text +[0] key +[1] length +[2] crc +[3] command = 0x00 +``` +## 8. 注意事项 + +- 写入前必须先连接设备并开启 RX notify。 +- 写入 key 必须和设备侧别一致：左侧 `0x6F`，右侧 `0xAF`。 +- `crc` 当前固定填 `0x00`。 +- JSON 使用 UTF-8 编码。