diff --git a/BALANCE_BOARD_PROTOCOL.md b/BALANCE_BOARD_PROTOCOL.md deleted file mode 100644 index 616ede5..0000000 --- a/BALANCE_BOARD_PROTOCOL.md +++ /dev/null @@ -1,198 +0,0 @@ -# 平衡板协议说明 - -本文说明当前固件里的数据包格式和 BLE 通讯方式。 - -## 1. BLE 通讯方式 - -平衡板工作在 BLE `Peripheral` 角色，通过 Nordic UART Service（NUS）与基站通信。 - -- 上行方向：平衡板 -> 基站，使用 NUS `Notify` -- 下行方向：基站 -> 平衡板，使用 NUS `Write` -- 广播内容：包含设备名和 NUS 128-bit UUID - -### 1.1 NUS UUID - -| 项目 | UUID | 方向 | 说明 | -|------|------|------|------| -| Service | `6E400001-B5A3-F393-E0A9-E50E24DCCA9E` | - | NUS 主服务 | -| TX Characteristic | `6E400003-B5A3-F393-E0A9-E50E24DCCA9E` | 平衡板 -> 基站 | Notify，上报 CoP 和编码器数据 | -| RX Characteristic | `6E400002-B5A3-F393-E0A9-E50E24DCCA9E` | 基站 -> 平衡板 | Write，下发阻力参数和控制命令 | - -### 1.2 当前连接行为 - -- 连接建立后，平衡板会请求 `7.5-15 ms` 连接区间（`min=6, max=12`）。 -- 所有数据通过 NUS 二进制帧发送，不走文本协议。 - -## 2. 数据包总览 - -当前协议使用双帧头 `0xAA 0x55` 作为包起始标记。所有多字节字段按 Little Endian 编码。 - -| 类型名 | 值 | 方向 | 长度 | 频率 | 说明 | -|--------|----|------|------|------|------| -| `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 | 心跳帧 | - -## 3. 上行数据包 - -### 3.1 压力中心帧 (CoP) - -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) | - -#### flags 位定义 - -| 位 | 名称 | 说明 | -|----|------|------| -| bit0 | `COP_FLAG_FORCE_VALID` | `1` 表示总力超过阈值，CoP 坐标有意义 | -| bit1-7 | 预留 | 填 `0` | - -#### CoP 解算公式 - -传感器布局（俯视）： - -```text - S4 (FL) -------- S1 (FR) - | center | - S3 (BL) -------- S2 (BR) -``` - -传感器坐标（单位：mm，板面中心为原点）： - -| 传感器 | 位置 | X | Y | -|--------|------|---|---| -| S1 | 右前 (FR) | `+200` | `+200` | -| S2 | 右后 (BR) | `+200` | `-200` | -| S3 | 左后 (BL) | `-200` | `-200` | -| S4 | 左前 (FL) | `-200` | `+200` | - -计算公式： - -```text -F_total = F0 + F1 + F2 + F3 - -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`。 - -#### 示例 - -下面是一个 CoP 帧示例（CoP 有效）： - -```text -AA 55 01 2A 01 00 00 F0 41 00 00 48 42 00 80 BB 44 -``` - -含义如下： - -- `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） - -### 3.2 电机编码器帧（预留） - -编码器帧固定 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) | - -## 4. 下行数据包 - -### 4.1 阻力参数帧 - -阻力参数帧固定 15 字节，用于基站向平衡板下发电机阻力控制参数。 - -| 字节偏移 | 字段 | 类型 | 值/范围 | 说明 | -|----------|------|------|---------|------| -| `0` | `sync0` | `uint8` | `0xAA` | 帧头第 1 字节 | -| `1` | `sync1` | `uint8` | `0x55` | 帧头第 2 字节 | -| `2` | `type` | `uint8` | `0x10` | 阻力参数包类型 | -| `3-6` | `K` | `float32 LE` | - | 刚度 (N/m) | -| `7-10` | `B` | `float32 LE` | - | 阻尼 (Ns/m) | -| `11-14` | `Tau` | `float32 LE` | - | 时间常数 (s) | - -#### 示例 - -```text -AA 55 10 00 00 C8 42 00 00 48 41 CD CC 4C 3E -``` - -含义如下： - -- `AA 55 10`：阻力参数帧头 -- `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 - -### 4.2 Spotter Mode 帧 - -Spotter 帧固定 8 字节，用于启用或关闭保护模式。 - -| 字节偏移 | 字段 | 类型 | 值/范围 | 说明 | -|----------|------|------|---------|------| -| `0` | `sync0` | `uint8` | `0xAA` | 帧头第 1 字节 | -| `1` | `sync1` | `uint8` | `0x55` | 帧头第 2 字节 | -| `2` | `type` | `uint8` | `0x11` | Spotter 包类型 | -| `3-6` | `threshold` | `float32 LE` | - | 保护力阈值 (N) | -| `7` | `enable` | `uint8` | `0` 或 `1` | `0` = 关闭, `1` = 启用 | - -#### 示例 - -启用 Spotter Mode，阈值 500 N： - -```text -AA 55 11 00 00 FA 43 01 -``` - -### 4.3 心跳帧 - -心跳帧固定 4 字节，基站以约 1 Hz 频率发送，用于活性检测。 - -| 字节偏移 | 字段 | 类型 | 值/范围 | 说明 | -|----------|------|------|---------|------| -| `0` | `sync0` | `uint8` | `0xAA` | 帧头第 1 字节 | -| `1` | `sync1` | `uint8` | `0x55` | 帧头第 2 字节 | -| `2` | `type` | `uint8` | `0x20` | 心跳包类型 | -| `3` | `counter` | `uint8` | `0-255` | 滚动计数 | - -#### 示例 - -```text -AA 55 20 05 -``` - -含义：心跳包，计数 `5`。 - -## 5. 当前实现约束 - -- 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`（后续可能调整）。 -- `ADC_TO_FORCE_SCALE` 当前为占位值 `1.0f`，待 10 kg 砝码标定后修正。 -- ARM Cortex-M33 为小端架构，`float32` 在 packed struct 中天然符合 Little Endian 要求。 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 ) diff --git a/README.md b/README.md index 7408e69..52dae32 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)。 --- @@ -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` | 协议专项说明、字段表和示例帧 | @@ -300,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/doc/BALANCE_BOARD_PROTOCOL.md b/doc/BALANCE_BOARD_PROTOCOL.md new file mode 100644 index 0000000..2a01146 --- /dev/null +++ b/doc/BALANCE_BOARD_PROTOCOL.md @@ -0,0 +1,302 @@ +# 平衡板协议说明 + +本文说明当前固件里的数据包格式和 BLE 通讯方式。 + +## 1. BLE 通讯方式 + +平衡板工作在 BLE `Peripheral` 角色，通过 Nordic UART Service（NUS）与基站通信。 + +- 上行方向：平衡板 -> 基站，使用 NUS `Notify` +- 下行方向：基站 -> 平衡板，使用 NUS `Write` +- 广播内容：包含设备名和 NUS 128-bit UUID + +### 1.1 NUS UUID + +| 项目 | UUID | 方向 | 说明 | +|------|------|------|------| +| Service | `6E400001-B5A3-F393-E0A9-E50E24DCCA9E` | - | NUS 主服务 | +| TX Characteristic | `6E400003-B5A3-F393-E0A9-E50E24DCCA9E` | 平衡板 -> 基站 | Notify，上报 CoP 和编码器数据 | +| RX Characteristic | `6E400002-B5A3-F393-E0A9-E50E24DCCA9E` | 基站 -> 平衡板 | Write，下发阻力参数和控制命令 | + +### 1.2 当前连接行为 + +- 连接建立后，平衡板会请求 `7.5-15 ms` 连接区间（`min=6, max=12`）。 +- 所有数据通过 NUS 二进制帧发送，不走文本协议。 + +## 2. 数据包总览 + +当前协议使用双帧头 `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` | 平衡板 -> 基站 | `11` | 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 | 心跳帧 | +| `PROTO_TYPE_CAL_CMD` | `0x30` | 基站 -> 平衡板 | `10` | 按需 | 标定命令帧 | +| `PROTO_TYPE_CAL_RESP` | `0x31` | 平衡板 -> 基站 | `14` | 按需 | 标定响应帧 | + +## 3. 上行数据包 + +### 3.1 压力中心帧 (CoP) + +CoP 帧固定 11 字节，结构如下： + +| 字节偏移 | 字段 | 类型 | 值/范围 | 说明 | +|----------|------|------|---------|------| +| `0` | `sync0` | `uint8` | `0xAA` | 帧头第 1 字节 | +| `1` | `sync1` | `uint8` | `0x55` | 帧头第 2 字节 | +| `2` | `type` | `uint8` | `0x01` | CoP 包类型 | +| `3` | `flags` | `uint8` | 见下表 | 状态标志位 | +| `4-5` | `cop_x` | `int16 LE` | - | 压力中心 X 坐标 (cm) | +| `6-7` | `cop_y` | `int16 LE` | - | 压力中心 Y 坐标 (cm) | +| `8-9` | `force` | `int16 LE` | - | 总重量 (kg) | +| `10` | `crc` | `uint8` | - | CRC-8/MAXIM 校验 | + +#### flags 位定义 + +| 位 | 名称 | 说明 | +|----|------|------| +| bit0 | `COP_FLAG_FORCE_VALID` | `1` 表示总力超过阈值，CoP 坐标有意义 | +| bit1-7 | 预留 | 填 `0` | + +#### CoP 解算公式 + +传感器布局（俯视）： + +```text + S4 (FL) -------- S1 (FR) + | center | + S3 (BL) -------- S2 (BR) +``` + +传感器坐标（单位：cm，板面中心为原点）： + +| 传感器 | 位置 | X | Y | +|--------|------|---|---| +| S1 | 右前 (FR) | `+10` | `+10` | +| S2 | 右后 (BR) | `+10` | `-10` | +| S3 | 左后 (BL) | `-10` | `-10` | +| S4 | 左前 (FL) | `-10` | `+10` | + +计算公式： + +```text +F_total = F0 + F1 + F2 + F3 + +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`。 + +#### 示例 + +下面是一个 CoP 帧示例（CoP 有效，cop_x=3 cm, cop_y=5 cm, force=75 kg）： + +```text +AA 55 01 01 03 00 05 00 4B 00 XX +``` + +含义如下： + +- `AA 55`：双帧头 +- `01`：CoP 包 +- `01`：flags = `force_valid` +- `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 电机编码器帧（预留） + +编码器帧固定 13 字节，结构如下。该帧目前仅在协议中预留定义，固件暂不实现。 + +| 字节偏移 | 字段 | 类型 | 值/范围 | 说明 | +|----------|------|------|---------|------| +| `0` | `sync0` | `uint8` | `0xAA` | 帧头第 1 字节 | +| `1` | `sync1` | `uint8` | `0x55` | 帧头第 2 字节 | +| `2` | `type` | `uint8` | `0x02` | 编码器包类型 | +| `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. 下行数据包 + +### 4.1 阻力参数帧 + +阻力参数帧固定 16 字节，用于基站向平衡板下发电机阻力控制参数。 + +| 字节偏移 | 字段 | 类型 | 值/范围 | 说明 | +|----------|------|------|---------|------| +| `0` | `sync0` | `uint8` | `0xAA` | 帧头第 1 字节 | +| `1` | `sync1` | `uint8` | `0x55` | 帧头第 2 字节 | +| `2` | `type` | `uint8` | `0x10` | 阻力参数包类型 | +| `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 XX +``` + +含义如下： + +- `AA 55 10`：阻力参数帧头 +- `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 帧固定 9 字节，用于启用或关闭保护模式。 + +| 字节偏移 | 字段 | 类型 | 值/范围 | 说明 | +|----------|------|------|---------|------| +| `0` | `sync0` | `uint8` | `0xAA` | 帧头第 1 字节 | +| `1` | `sync1` | `uint8` | `0x55` | 帧头第 2 字节 | +| `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 XX +``` + +### 4.3 心跳帧 + +心跳帧固定 5 字节，基站以约 1 Hz 频率发送，用于活性检测。 + +| 字节偏移 | 字段 | 类型 | 值/范围 | 说明 | +|----------|------|------|---------|------| +| `0` | `sync0` | `uint8` | `0xAA` | 帧头第 1 字节 | +| `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 XX +``` + +含义：心跳包，计数 `5`，`XX` 为 CRC。 + +## 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 上行流隐式充当平衡板到基站的活性检测，因此心跳仅为下行。 +- 传感器通道到物理位置的映射：MUX `{0x01, 0x23, 0x45, 0x67}` → FL/FR/BR/BL。 +- `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/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 编码。 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 编码。 diff --git a/inc/ads1256.h b/inc/ads1256.h index 4b37b53..fbdd91a 100644 --- a/inc/ads1256.h +++ b/inc/ads1256.h @@ -4,50 +4,12 @@ /* sensor.c 切换通道时需要此寄存器地址 */ #define ADS1256_REG_MUX 0x01 -/** - * @brief 初始化 ADS1256：配置 GPIO/SPI，复位芯片，写入寄存器，自校准。 - * @return 0 成功，负 errno 失败。 - */ int ads1256_init(void); - -/** - * @brief 等待 DRDY 拉低（转换结果可读），带超时保护。 - * @param timeout_ms 超时时间（毫秒）。 - */ -void ads1256_wait_drdy(uint16_t timeout_ms); - -/** - * @brief 向 ADS1256 写单个寄存器。 - * @param reg 目标寄存器地址。 - * @param val 要写入的值。 - */ +int ads1256_wait_drdy(uint16_t timeout_ms); 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 引脚），等待复位后自校准完成。 - */ -void ads1256_hwreset(void); - -/** - * @brief 读取当前 24 位转换结果，符号扩展为 int32_t。 - * @return ADC 原始值。 - */ +int ads1256_hwreset(void); int32_t ads1256_read_data(void); - -/** - * @brief 发送单字节命令到 ADS1256。 - * @param cmd 命令字。 - */ void ads1256_write_cmd(uint8_t cmd); +int ads1256_recover(int max_retries); diff --git a/inc/ble_transport.h b/inc/ble_transport.h index 4ba1450..697512d 100644 --- a/inc/ble_transport.h +++ b/inc/ble_transport.h @@ -2,33 +2,39 @@ #include #include +/** @brief BLE 原始数据接收回调类型。 */ +typedef void (*ble_rx_cb_t)(const uint8_t *data, uint16_t len); + /** - * @brief 初始化 BLE 协议栈、NUS 服务和内部状态。 - * @return 0 成功，负 errno 失败。 + * @brief 初始化 BLE 协议栈和 NUS 服务。 + * + * @retval 0 成功。 + * @retval 负值 初始化错误码。 */ int ble_transport_init(void); -/** - * @brief 启动 BLE 广播。 - */ +/** @brief 启动 BLE 可连接广播。 */ void ble_transport_adv_start(void); /** - * @brief 通过 NUS 发送二进制帧，自动重试最多 3 次。未连接或通知未开启时静默丢弃。 + * @brief 通过 NUS 发送二进制数据，内部自动重试最多 3 次。 + * * @param data 待发送字节缓冲区。 - * @param len 字节数。 + * @param len 字节数。 */ void ble_transport_send(const uint8_t *data, uint16_t len); /** - * @brief 检查是否已连接且通知已使能。 + * @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/button.h b/inc/button.h new file mode 100644 index 0000000..f3389f2 --- /dev/null +++ b/inc/button.h @@ -0,0 +1,3 @@ +#pragma once + +int button_init(void); diff --git a/inc/calibration.h b/inc/calibration.h new file mode 100644 index 0000000..9e4d1c7 --- /dev/null +++ b/inc/calibration.h @@ -0,0 +1,124 @@ +#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 3.0f /* 总力超过此值才判定有人 (kg) */ +#define COP_FORCE_EXIT_THRESHOLD 1.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 + +/* ─── 标定常量 ─── */ + +#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/inc/comm_protocol.h b/inc/comm_protocol.h new file mode 100644 index 0000000..3d3643d --- /dev/null +++ b/inc/comm_protocol.h @@ -0,0 +1,235 @@ +#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]); diff --git a/inc/protocol.h b/inc/protocol.h deleted file mode 100644 index 67ce18a..0000000 --- a/inc/protocol.h +++ /dev/null @@ -1,132 +0,0 @@ -#pragma once -#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 坐标有意义 */ - -/* ─── 板面几何 & 传感器标定 ─── */ -/* 传感器到板面中心的半宽、半长 (mm)。 */ -#define BOARD_HALF_WIDTH_MM 100.0f -#define BOARD_HALF_LENGTH_MM 100.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 帧 (17 bytes, 50 Hz) ─── - * - * [AA 55 01 seq flags cop_x(4) cop_y(4) force(4)] - */ -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)，板面坐标系 */ - float force; /* 总重量 (kg) */ -} __attribute__((packed)); - -union cop_pkt_t { - struct cop_frame_t frame; - uint8_t bytes[sizeof(struct cop_frame_t)]; -}; - -/* ─── 上行: 电机编码器帧 (13 bytes, 预留) ─── - * - * [AA 55 02 seq flags cable_length(4) velocity(4)] - */ -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) */ -} __attribute__((packed)); - -union encoder_pkt_t { - struct encoder_frame_t frame; - uint8_t bytes[sizeof(struct encoder_frame_t)]; -}; - -/* ─── 下行: 阻力参数帧 (15 bytes) ─── - * - * [AA 55 10 K(4) B(4) Tau(4)] - */ -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) */ -} __attribute__((packed)); - -union resistance_pkt_t { - struct resistance_frame_t frame; - uint8_t bytes[sizeof(struct resistance_frame_t)]; -}; - -/* ─── 下行: Spotter Mode 帧 (8 bytes) ─── - * - * [AA 55 11 threshold(4) enable] - */ -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 = 启用 */ -} __attribute__((packed)); - -union spotter_pkt_t { - struct spotter_frame_t frame; - uint8_t bytes[sizeof(struct spotter_frame_t)]; -}; - -/* ─── 下行: 心跳帧 (4 bytes, ~1 Hz) ─── - * - * [AA 55 20 counter] - */ -struct heartbeat_frame_t { - uint8_t sync0; /* = PROTO_SYNC0 */ - uint8_t sync1; /* = PROTO_SYNC1 */ - uint8_t type; /* = PROTO_TYPE_HEARTBEAT */ - uint8_t counter; /* 滚动计数，用于活性检测 */ -} __attribute__((packed)); - -union heartbeat_pkt_t { - struct heartbeat_frame_t frame; - uint8_t bytes[sizeof(struct heartbeat_frame_t)]; -}; 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/prj.conf b/prj.conf index 1959d0a..f026f4c 100644 --- a/prj.conf +++ b/prj.conf @@ -5,12 +5,27 @@ 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 -# 借鉴 Kiwii: 优化缓冲区 (匹配硬件) +# 缓冲区 CONFIG_BT_BUF_ACL_TX_COUNT=3 -CONFIG_BT_L2CAP_TX_BUF_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 # 硬件驱动 CONFIG_SPI=y @@ -24,6 +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/ads1256.c b/src/ads1256.c index ed4d430..59c876f 100644 --- a/src/ads1256.c +++ b/src/ads1256.c @@ -26,25 +26,39 @@ 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) { +/** + * @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 有效（物理拉低） */ - /* 用 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); if (k_uptime_get() >= deadline) { LOG_WRN("DRDY timeout (%u ms)", timeout_ms); - break; + return -ETIMEDOUT; } } + 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,7 +129,13 @@ void ads1256_sync_wakeup(void) { gpio_pin_set_dt(&cs_spec, 0); } -void ads1256_hwreset(void) { +/** + * @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); /* t16: 最小复位脉宽 4 × tCLKIN ≈ 0.5µs，取 1ms 余量 */ @@ -106,9 +143,14 @@ void ads1256_hwreset(void) { gpio_pin_set_dt(&reset_spec, 0); /* 复位释放后芯片执行自校准，等待完成 */ k_msleep(10); - ads1256_wait_drdy(500); + 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 }; @@ -124,12 +166,21 @@ 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; } +/** + * @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); + /* 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); @@ -144,7 +195,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); @@ -181,10 +236,43 @@ 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; } 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; +} diff --git a/src/ble_transport.c b/src/ble_transport.c index c425ca5..c6ce300 100644 --- a/src/ble_transport.c +++ b/src/ble_transport.c @@ -1,25 +1,22 @@ #include "ble_transport.h" -#include "protocol.h" #include -#include #include #include #include #include #include +#include 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), }; @@ -29,82 +26,56 @@ 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; - -void ble_transport_adv_start(void) { - bt_le_adv_start(BT_LE_ADV_CONN_FAST_2, ad, ARRAY_SIZE(ad), sd, ARRAY_SIZE(sd)); -} +/* 上层收数据回调，必须在 ble_transport_adv_start() 前完成注册 */ +static ble_rx_cb_t rx_cb; +/** @brief 延时广播任务，断链后重新进入可连接状态。 */ static void adv_work_handler(struct k_work *work) { + ARG_UNUSED(work); ble_transport_adv_start(); } +/** @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); } +/** @brief 断开回调，释放连接引用并安排延时重广播。 */ static void disconnected(struct bt_conn *conn, uint8_t reason) { + ARG_UNUSED(conn); + LOG_WRN("BLE disconnected (reason 0x%02x)", reason); if (current_conn) { bt_conn_unref(current_conn); current_conn = NULL; } nus_notification_enabled = false; - - /* 延迟 1 秒再重启广播，给协议栈留状态回收时间 */ k_work_schedule(&adv_work, K_MSEC(1000)); } +/** @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); } +/** + * @brief NUS 收数据回调，直接转发给上层注册的处理函数。 + */ static void nus_received_cb(struct bt_conn *conn, const uint8_t *data, uint16_t len) { - 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)) { - 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)) { - 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)) { - last_heartbeat_ms = k_uptime_get(); - LOG_DBG("RX Heartbeat"); - } - break; - default: - break; + ARG_UNUSED(conn); + if (rx_cb) { + rx_cb(data, len); } } @@ -123,6 +94,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); @@ -133,40 +106,39 @@ int ble_transport_init(void) { return 0; } -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); - } +void ble_transport_adv_start(void) { + bt_le_adv_start(BT_LE_ADV_CONN_FAST_2, ad, ARRAY_SIZE(ad), sd, ARRAY_SIZE(sd)); } +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); + } + 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); } -float ble_transport_get_resistance_K(void) { - return resistance_K; -} - -float ble_transport_get_resistance_B(void) { - return resistance_B; -} - -float ble_transport_get_resistance_Tau(void) { - return resistance_Tau; -} - -float ble_transport_get_spotter_threshold(void) { - return spotter_threshold; -} - -bool ble_transport_get_spotter_enabled(void) { - return spotter_enabled; -} - -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/button.c b/src/button.c new file mode 100644 index 0000000..7b9f475 --- /dev/null +++ b/src/button.c @@ -0,0 +1,108 @@ +#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; + +/** + * @brief 处理 LED 延时翻转，完成一次按键反馈闪烁序列。 + * + * @param work Zephyr delayable work 入口参数，当前实现未直接使用。 + * + * @return 无返回值。 + */ +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)); + } +} + +/** + * @brief 在工作队列上下文执行去皮，并启动 LED 闪烁反馈。 + * + * @param work Zephyr work 入口参数，当前实现未直接使用。 + * + * @return 无返回值。 + */ +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)); +} + +/** + * @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"); + 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/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/comm_protocol.c b/src/comm_protocol.c new file mode 100644 index 0000000..4820bb3 --- /dev/null +++ b/src/comm_protocol.c @@ -0,0 +1,154 @@ +#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)); +} diff --git a/src/main.c b/src/main.c index df2d624..ccc14f2 100644 --- a/src/main.c +++ b/src/main.c @@ -8,6 +8,9 @@ */ #include "ble_transport.h" +#include "button.h" +#include "calibration.h" +#include "comm_protocol.h" #include "sensor.h" #include @@ -15,14 +18,36 @@ 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) ---"); ble_transport_init(); + comm_protocol_init(); ble_transport_adv_start(); - if (sensor_init()) return 0; + if (cal_init()) { + LOG_ERR("Failed to initialize calibration"); + return -1; + } + + 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); return 0; } diff --git a/src/sensor.c b/src/sensor.c index 8132566..9015aac 100644 --- a/src/sensor.c +++ b/src/sensor.c @@ -1,7 +1,8 @@ #include "sensor.h" #include "ads1256.h" #include "ble_transport.h" -#include "protocol.h" +#include "calibration.h" +#include "comm_protocol.h" #include #include @@ -11,12 +12,12 @@ 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) + * 每通道每帧采 9 次取中值。3750 SPS 下： + * 9 次各 0.27ms = 2.4ms/通道 + * 4 通道 = 9.7ms，帧率 ~100Hz + * 提高帧率将奈奎斯特频率抬至 ~50Hz，避免低速马达振动混叠 */ -#define AVG_COUNT 17 +#define AVG_COUNT 9 #define DEADZONE_THRESHOLD 250 /* MUX 通道配置：4 路差分 */ @@ -24,21 +25,24 @@ 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 */ }; /* --- 内部状态 --- */ 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; /* --- 线程 --- */ @@ -47,6 +51,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 +73,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]; @@ -78,61 +99,117 @@ 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)); } +/** + * @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(); + 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(); } - 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; + + /* 二阶 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; } } -static bool compute_cop(float *out_x, float *out_y, float *out_force) { +/** + * @brief 根据四路受力结果计算压力中心（CoP）和总力。 + * + * @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(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(); 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 = total; - - if (total < COP_MIN_FORCE_THRESHOLD) { - *out_x = 0.0f; - *out_y = 0.0f; - return false; + /* 迟滞判定：避免阈值附近反复切换 */ + if (!force_valid_state) { + if (total < COP_FORCE_ENTER_THRESHOLD) { + *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]; 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; } +/** + * @brief 传感器后台线程主循环，负责采集、去皮处理、CoP 计算和蓝牙发送。 + * + * @param p1 Zephyr 线程入口预留参数，当前未使用； + * @param p2 Zephyr 线程入口预留参数，当前未使用； + * @param p3 Zephyr 线程入口预留参数，当前未使用； + * + * @return 无返回值。 + */ static void sensor_thread_fn(void *p1, void *p2, void *p3) { (void)p1; (void)p2; (void)p3; - uint8_t cop_seq = 0; uint8_t debug_log_divider = 0; + uint8_t send_divider = 0; int64_t frame_ts = k_uptime_get(); while (1) { @@ -140,6 +217,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(); @@ -148,46 +230,63 @@ 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 + 打包 */ - float cop_x, cop_y, force; - bool valid = compute_cop(&cop_x, &cop_y, &force); + /* EMI 自恢复：acq 异常短说明寄存器被干扰改写 */ + if (acq < 10) { + LOG_ERR("EMI detected (acq=%lld ms)", (long long)acq); + ads1256_recover(10); + continue; + } - union cop_pkt_t pkt; - pkt.frame.sync0 = PROTO_SYNC0; - pkt.frame.sync1 = PROTO_SYNC1; - pkt.frame.type = PROTO_TYPE_COP; - pkt.frame.seq = cop_seq++; - pkt.frame.flags = valid ? COP_FLAG_FORCE_VALID : 0; - pkt.frame.cop_x = cop_x; - pkt.frame.cop_y = cop_y; - pkt.frame.force = force; - - /* 发送 */ - ble_transport_send(pkt.bytes, sizeof(pkt.bytes)); - int64_t t2 = k_uptime_get(); + /* CoP 计算 + 发送 */ + int16_t cop_x, cop_y, force; + 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); + 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); + } /* 调试日志（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\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 frame=%lld ms", (long long)(t1 - t0), (long long)frame_ms); } } } +/** + * @brief 初始化压力传感器模块：ADS1256 硬件 + 初始去皮 + 创建采集线程。 + * + * 调用后采集线程自动启动，以 50 Hz 循环采集、计算 CoP 并发送 BLE 数据。 + * + * @retval 0 初始化成功，传感器线程已经启动并完成一次初始去皮。 + * @retval 负值 ADS1256 初始化阶段返回的具体错误码。 + */ int sensor_init(void) { int err = ads1256_init(); if (err) return err; + /* SELFCAL 后 ADC 模拟链路需几个转换周期才能完全建立，空读排空 pipeline */ + for (int i = 0; i < 4; i++) { + ads1256_wait_drdy(50); + (void)ads1256_read_data(); + } + memset(sensor_offsets, 0, sizeof(sensor_offsets)); memset(filtered, 0, sizeof(filtered)); @@ -201,6 +300,11 @@ int sensor_init(void) { return 0; } +/** + * @brief 执行四路去皮（零点校准），可从任意线程调用。 + * + * @return 无返回值。 + */ void sensor_perform_tare(void) { atomic_set(&tare_requested, 1); } 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