192 lines
5.3 KiB
Markdown
192 lines
5.3 KiB
Markdown
# 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 编码。
|