From 0979866a7cf9fb4cba809492d12e01bb28be197f Mon Sep 17 00:00:00 2001 From: pNexus Date: Fri, 15 May 2026 18:06:33 +0800 Subject: [PATCH] =?UTF-8?q?refactor(protocol):=20=E6=96=B0=E5=A2=9E=20comm?= =?UTF-8?q?=5Fprotocol=20=E5=8D=8F=E8=AE=AE=E6=94=B6=E5=8F=91=E8=A7=A3?= =?UTF-8?q?=E6=9E=90=E5=B1=82=EF=BC=8Cble=5Ftransport=20=E9=80=80=E5=8C=96?= =?UTF-8?q?=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;