...
存储顺序 | MSB +7 | +0 LSB | ||||||
---|---|---|---|---|---|---|---|---|
Payload[0]: | Data[0] | |||||||
Payload[1]: | Data[1] | |||||||
... | ||||||||
Payload[n-1]: | Data[n-1] |
该字段记录了数据包发送方希望携带的有效信息负载。其数据长度由Len/LenH字段表示。如果发送方没有任何需要传输的信息时,该字段将被省略。
...
数据类型 | u8 | u8 | u8 | u8[n] | u8 |
---|---|---|---|---|---|
Standard: | 0x10 | Len[7..0] | Resp | Payload[n] | CHKSUM |
数据类型 | u8 | u16 | u8 | u8[n] | u8 |
Long Frame: | 0x50 | Len[15..0] | Resp | Payload[n] | CHKSUM |
由Slave发送的应答数据包满足上述2种格式。其中,CMD字段被用于对Slave先前收到的请求命令处理情况的回应。其取值必须为如下的几种:
保留Resp(CMD) | 描述 |
---|---|
0x00 | 强制同步帧的应答包 |
0x01 | ECHO帧的应答包 |
0x02 | <OK> 表示上一个请求命令被正确执行或者从协议栈角度没有遇到问题。 |
0x03 | <Error> 表示上一个请求命令执行过程出现问题 |
0xFF | <Invalid> 表示上一个请求命令接收的不完整、或者数据包本身结构不正确 |
...
错误码 | 描述 |
---|---|
0x40 | 请求命令包校验和不匹配 |
0x20 | 请求命令包长度超过了当前Slave协议栈缓冲区承受范围 |
0x10 | Slave协议栈信道没有完成同步 |
0x8000 | 请求命令包中的CMD字段不被Slave支持 |
0x8001 | 请求命令在执行中遇到格式类错误,例如请求参数不符合规范 |
0x8002 | 请求命令在执行中遇到操作类错误,例如执行过程遇到问题而无法继续 |
SR001 SLAMWARE Control Bus Protocol
SLAMWARE Core 协议命令
SLAMWARE Core Ctrl Bus通讯协议
SLAMWARE Core Ctrl Bus通讯协议使用Inter-chip通讯协议的扩展模式。
它的数据包类型域为标识SLAMWARE CORE CTRL BUS扩展模式的0xF8。在此扩展模式下,数据负载的格式为请求Core Ctrl Bus扩展模式的0xF8。在此扩展模式下,数据负载的格式为请求/应答通讯模式,应答数据包格式与标准Inter-chip通讯协议一致。根据数据包长度的类型分为如下两种格式:
格式一:
...
协议栈数据 | SLAMWARE Core Ctrl Bus扩展模式 | 协议栈数据 | |||
---|---|---|---|---|---|
同步标志字节 (0x500x10) | 数据包长度 u8 | 0xF8 | 请求命令 u8 | 数据负载 ... | 校验字节 u8 |
SLAMWARE Core和外部系统之间的Ctrl Bus通讯协议设计为单向通讯模式,始终由SLAMWARE Core发起数据包请求。
外部系统需要及时回应SLAMWARE Core的请求命令,否则SLAMWARE Core将认为外部系统连线中断,可能停止正常工作。
请求命令与应答必须是成对出现的,每个应答总对应于一个请求命令。SLAMWARE Core在发送一个请求命令后,总会在接收到外部系统发送应答数据包或者通讯超时后,才会进行下一个请求命令的发送。
SLAMWARE Core也可以接受外部系统的控制命令,但由于单向通讯的模式,外部系统只能在SLAMWARE Core定期发送外部系统控制命令查询请求的时候才可以发送控制命令。但外部系统可以通过#CMD信号通知SLAMWARE Core尽快发送外部系统控制命令查询请求。
SR001 SLAMWARE Control Bus Protocol
SLAMWARE CORE 请求命令
SLAMWARE Core室内定位导航模块会发送以下请求命令,外部系统可以根据需要响应请求命令,否则SLAMWARE Core Ctrl Bus协议栈会做默认响应。标*的命令为外部系统必须响应的请求命令。否则SLAMWARE Core将不能正常工作。
...
请求数据包 | ||||||
---|---|---|---|---|---|---|
命令代码 | 负载数据 | |||||
0x10 | 协议版本(u8) | |||||
应答数据包 | ||||||
应答代码 | 负载数据 | |||||
<OK> | 型号(u8[12]) | 固件版本(u16) | 硬件版本(u16) | 序列(u32[3]) |
该命令数据域描述如下:
数据域 | 类型 | 描述 |
---|---|---|
协议版本 | u8 | 协议的版本信息,版本信息不匹配时外部系统应应答错误 |
型号 | u8[12] | 外部系统的型号,最多12个字符 |
固件版本 | u16 | 外部系统的固件版本 |
硬件版本 | u16 | 外部系统的硬件版本 |
序列号 | u32[3] | 外部系统的序列号,最多12个字节 |
SR001 SLAMWARE Control Bus Protocol
外部系统参数获取请求(GET_BASE_CONF)
...
请求数据包 | ||||||||||||
---|---|---|---|---|---|---|---|---|---|---|---|---|
命令代码 | 负载数据 | |||||||||||
0x20 | 无 | |||||||||||
应答数据包 | ||||||||||||
应答代码 | 负载数据 | |||||||||||
<OK> | 尺寸类别(u8) | 尺寸半径(u32) | 轮组类别(u8) | 距离传感器数量(u8) | 距离传感器数量(pose距离传感器安装位置(pose[8]) | |||||||
触碰传感器数量(u8) | 触碰传感器安装位置(pose[8]) |
该命令数据域描述如下:
数据域 | 类型 | 描述 |
---|---|---|
尺寸类型 | u8 | 外部系统的尺寸类型,目前只支持圆形和方形 |
尺寸半径 | u32 | 外部系统的尺寸半径,以mm为单位,Q8定点小数 |
轮组类型 | u8 | 外部系统的轮组类型,目前只支持两轮差动轮组 |
距离传感器数量 | u8 | 外部系统的距离传感器数量,最多支持8组 |
距离传感器安装位置 | pos[8] | 外部系统的距离传感器安装位置 |
触碰传感器数量 | u8 | 外部系统的触碰传感器数量,最多支持8组 |
触碰传感器安装位置 | pos[8] | 外部系统的触碰传感器安装数据 |
...
数据域 | 类型 | 描述 |
---|---|---|
X轴向距离 | s32 | 传感器中心距离外部系统中心的X轴向距离,Q8定点小数 |
Y轴向距离 | s32 | 传感器中心距离外部系统中心的Y轴向距离,Q8定点小数 |
Z轴向距离 | s32 | 传感器中心距离外部系统中心的Z轴向距离,Q8定点小数 |
角度 | u32 | 传感器中心跟外部系统中心点的逆时针夹角,Q8定点小数 |
SR001 SLAMWARE Control Bus Protocol
外部系统配置获取请求(GET_BINARY_CONF)
...
请求数据包 | ||||||||||||
---|---|---|---|---|---|---|---|---|---|---|---|---|
命令代码 | 负载数据 | |||||||||||
0x30 | 无 | |||||||||||
应答数据包 | ||||||||||||
应答代码 | 负载数据 | |||||||||||
<OK> | 电池电量百分比(u8) | 电池充电状态(u8) |
该命令数据域描述如下:
数据域 | 类型 | 描述 |
---|---|---|
电池电量百分比 | u8 | 外部系统的电池电量百分比,0-100 |
电池充电状态 | u8 | 外部系统的电池状态,该变量为一个位图。 |
SR001 SLAMWARE Control Bus Protocol
外部系统轮组状态获取请求(GET_BASE_MOTOR_DATA)
...
请求数据包 | ||||||||||||
---|---|---|---|---|---|---|---|---|---|---|---|---|
命令代码 | 负载数据 | |||||||||||
0x31 | 无 | |||||||||||
应答数据包 | ||||||||||||
应答代码 | 负载数据 | |||||||||||
<OK> | 左轮电机累计里程(s32) | 右轮电机累计里程(s32) |
该命令数据域描述如下:
数据域 | 类型 | 描述 |
---|---|---|
左轮电机累计里程 | s32 | 外部系统的左轮电机累计运动里程,单位为mm。 |
右轮电机累计里程 | s32 | 外部系统的右轮电机累计运动里程,单位为mm。 |
...
请求数据包 | ||||||
---|---|---|---|---|---|---|
命令代码 | 负载数据 | |||||
0x32 | 无 | |||||
应答数据包 | ||||||
应答代码 | 负载数据 | |||||
<OK> | 距离传感器数据(u32[16]) |
该命令数据域描述如下:
数据域 | 类型 | 描述 |
---|---|---|
距离传感器数据 | u32[16] | 外部系统的距离传感器数据,结果为距离传感器到障碍物的距离信息,单位为mm,Q16定点小数,最多支持16组距离传感器。 |
SR001 SLAMWARE Control Bus Protocol
外部系统触碰传感器数据获取请求(GET_BASE_BUMPER_DATA)
...
请求数据包 | ||||||
---|---|---|---|---|---|---|
命令代码 | 负载数据 | |||||
0x33 | 无 | |||||
应答数据包 | ||||||
应答代码 | 负载数据 | |||||
<OK> | 触碰传感器数据(u8) |
该命令数据域描述如下:
数据域 | 类型 | 描述 |
---|---|---|
触碰传感器数据 | u8 | 表示外部系统的触碰传感器数据。自最低位开始,每个bit代表一个碰撞传感器的状态,顺序与配置中的顺序相同。指定位为1时,表示该碰撞传感器未触发,指定位为0时,表示该碰撞传感器触发。最多支持8组触碰传感器 |
...
数据域 | 类型 | 描述 |
---|---|---|
数据类型 | u8 | SLAMWARE Core想要获取的自动回充的数据类型,当前支持的类型只有一种: |
0x34命令代码请求数据包中数据域定义如下:
数据域
类型
信标数量 | u8 | 外部系统充电座上的信标的数量。当前只支持3个信标时,0号信标指的是左信标,1号信标指的是主信标,2号信标指的是右信标. |
接收头 | u8 | 外部系统机器人上接收头的数量。当前只支持3个接收头,0号接收头指的是左接收头,1号接收头指的是主接收头,2号接收头指的是右接收头 |
接收头 | u8[n] | 每个接收头对应一个字节,每个字节代表这个接收头能接收到的信标。 |
SR001 SLAMWARE Control Bus Protocol
外部系统运动控制请求(SET_BASE_MOTOR)
SLAMWARE Core室内定位导航模块通过发送运动控制请求控制外部系统运动。外部系统需要根据请求数据包的内容准确控制系统运动才能使SLAMWARE Core正常工作。
请求数据包 | |||||||||||
---|---|---|---|---|---|---|---|---|---|---|---|
命令代码 | 负载数据 | ||||||||||
0x40 | 运动速度(s32[4]) | ||||||||||
应答数据包 | |||||||||||
应答代码 | 负载数据 | ||||||||||
<OK> | 无 |
该命令数据域描述如下:
数据域 | 类型 | 描述 |
---|---|---|
运动速度 | s32[4] | 外部系统的轮子运动速度,单位为mm/s |
...
请求数据包 | ||||||||||||
---|---|---|---|---|---|---|---|---|---|---|---|---|
命令代码 | 负载数据 | |||||||||||
0x41 | X轴向速度量(s32) | Y轴向速度量(s32) | 角速度量(s32) | |||||||||
应答数据包 | ||||||||||||
应答代码 | 负载数据 | |||||||||||
<OK> | X轴向位移(s32) | Y轴向位移(s32) | 角度位移(s32) |
该命令数据域描述如下:
数据域 | 类型 | 描述 |
---|---|---|
X轴向速度量 | s32 | X轴向速度,Q16定点小数 |
Y轴向速度量 | s32 | Y轴向速度,Q16定点小数 |
角速度量 | s32 | 逆时针角速度,Q16定点小数 |
X轴向位移 | s32 | 外部系统整体相对于上一次应答此请求时的X轴向位移,单位为mm,Q16定点小数。 |
Y轴向位移 | s32 | 外部系统整体相对于上一次应答此请求时的Y轴向位移,单位为mm,Q16定点小数。 |
角度位移 | s32 | 外部系统整体相对于上一次应答此请求时的逆时针角度位移,单位为度,Q16定点小数。 |
...
变量定义:
变量名称
定义
单位
dl
左轮位移,机器人向前为正
m
dr
右轮位移,机器人向前为正
m
dx
机器人正方向位移,机器人前方为正
m
dy
机器人侧向位移,机器人左侧为正
m
dyaw
机器人角位移,逆时针为正
rad
- 计算公式:
dyaw=(dr-dl)/2R
dx=cos(dyaw)×(dl+dr)/2
dy=sin(dyaw)×(dl+dr)/2
Note title 注意 R是机器人轮距半径,单位为m,下文同。
示例代码
Code Block language text title base_set_velocity_request linenumbers true float d_yaw = (d_dist_r_mm_f - d_dist_l_mm_f)/2.0f/robot_radius_mm; float displacement = (d_dist_l_mm_f + d_dist_r_mm_f)/2.0f; float dx = cos(d_yaw)*displacement; float dy = sin(d_yaw)*displacement; ans_pkt->base_dx_mm_q16 = (_32)(dx*(1<<16)) ans_pkt->base_dy_mm_q16 = (_32)(dy*(1<<16)) ans_pkt->base_dtheta_degree_q16 = (_32)(d_yaw/M_PIF*180*(1<<16))
根据速度量计算左右轮线速度的方法:
变量定义
变量名称
定义
单位
vx
机器人正向线速度,前进方向为正
m/s
vy
机器人侧向线速度,向左为正, 对于二轮差动机器人,该值为0
m/s
omega
机器人角速度,逆时针为正
rad/s
vl
机器人左轮线速度,机器人向前为正
m/s
vr
机器人右轮线速度,机器人向前为正
m/s
- 计算公式(以二轮差动模型为例)
vl=vx-omega*R
vr=vx+omega*R 示例代码
Code Block language cpp title base_set_velocity_request linenumbers true base_set_velocity_request_t *req = (base_set_velocity_request_t*)request->payload; float speed_l_mm = (float)req->velocity_x_q16 * 1000.0 / (1 << 16); float speed_r_mm = speed_l_mm; float line_speed_mm = (float)req->angular_velocity_q16 / (1 << 16) * robot_radius_mm; speed_l_mm -= line_speed_mm; speed_r_mm += line_speed_mm;
外部系统命令查询请求(POLL_BASE_CMD)
...
请求数据包 | |||||||||||
---|---|---|---|---|---|---|---|---|---|---|---|
命令代码 | 负载数据 | ||||||||||
0x50 | 无 | ||||||||||
应答数据包 | |||||||||||
应答代码 | 负载数据 | ||||||||||
<OK> | 外部系统命令代码(u8) |
该命令数据域描述如下:
数据域 | 类型 | 描述 |
---|---|---|
外部系统命令代码 | u8 | 外部系统的命令代码,只能是SLAMWARE CORE支持的命令代码。详情请见下表。Core支持的命令代码。详情请见下表。 |
SLAMWARE Core支持的命令代码如下:
外部系统命令代码 | 命令名称 | 描述 |
---|---|---|
0x51 | GET_INFO | 获取SLAMWARE CORE系统信息,包含软硬件版本,网络配置。 |
0x52 | RESET_WIFI | 重置SLAMWARE CORE无线网络 |
0x53 | FW_UPGRADING | 更新SLAMWARE CORE固件 |
0x80 | START_SWEEP | 开始清扫(仅针对扫地机版本) |
0x81 | STOP_SWEEP | 结束清扫(仅针对扫地机版本) |
0x82 | SPOT_SWEEP | 定点清扫(仅针对扫地机版本) |
0x90 | GET_HEALTH | 获取健康状况 |
0xA0 | MOVE_FORWARD | 向前 |
0xA1 | MOVE_BACKWARD | 向 后 |
0xA2 | TURN_LEFT | 向左 |
0xA3 | TURN_RIGHT | 向右 |
0xAF | CANCEL_ACTION | 取消上一步操作 |
0xB0 | GET_AUXILIARY_ANCHOR | 获取辅助定位传感器 |
SR001 SLAMWARE Control Bus Protocol
外部系统命令回应请求(POLL_BASE_ANS_CMD)
...
请求数据包 | |||||||||||
---|---|---|---|---|---|---|---|---|---|---|---|
命令代码 | 负载数据 | ||||||||||
0x5F | 无 | ||||||||||
应答数据包 | |||||||||||
应答代码 | 负载数据 | ||||||||||
<OK> | 应答数据(u8) |
该命令数据域描述如下:
数据域 | 类型 | 描述 |
---|---|---|
应答数据 | u8 | 外部系统命令的应答数据,与SLAMWARE CORE命令代码相关 |
...
请求数据包 | |||||||||||
---|---|---|---|---|---|---|---|---|---|---|---|
命令代码 | 负载数据 | ||||||||||
0x60 | 系统事件代码(u8) | ||||||||||
应答数据包 | |||||||||||
应答代码 | 负载数据 | ||||||||||
<OK> | 无 |
该命令数据域描述如下:
数据域 | 类型 | 描述 |
---|---|---|
系统事件代码 | u8 | SLAMWARE CORE的系统事件代码,详见下表 |
...
系统事件代码 | 命令名称 | 描述 |
---|---|---|
0x61 | LIDAR_CONN_FAIL | LIDAR连接失败 |
0x62 | LIDAR_RAMPUP_FAIL | LIDAR启动失败 |
0x63 | SYSTEM_UP_OK | 系统启动成功 |
0x64 | FIRMWARE_UPDATE | 系统固件升级 |
0x65 | CORE_DISCONNECT | 系统断开连接 |
0x66 | FIRMWARE_UPDATE_OK | 固件升级成功 |
0x80 | START_SWEEP | 通知外部系统清扫开始(仅针对扫地机版本) |
0x81 | END_SWEEP | 通知外部系统清扫结束(仅针对扫地机版本) |
SR001 SLAMWARE Control Bus Protocol
外部系统健康管理命令请求(HEALTH_MGMT)
...
HEALTH_GET_HEALTH
请求数据包 | ||||||||||||
---|---|---|---|---|---|---|---|---|---|---|---|---|
命令代码 | 负载数据 | |||||||||||
0x90 | 0x100x01 | |||||||||||
应答数据包 | ||||||||||||
应答代码 | 负载数据 | |||||||||||
<OK> | Health_flag(u8) | Error_count(u8) |
该命令数据域描述如下:
数据域 | 类型 | 描述 |
---|---|---|
Health_flag | u8 | 表示健康信息按照严重程度的分类,具体定义请参考下表 |
Error_count | u8 | 表示错误信息的数目 |
Health flag定义如下:
Error Health Flag | Bit | 描述 |
---|---|---|
保留 | [7:3] | 保留 |
FATAL | [2] | 表示是否有FATAL。 0 = Has no FATAL 1 = Has FATAL |
ERROR | [1] | 表示是否有ERROR。 0 = Has no ERROR 1 = Has ERROR |
WARN | [0] | 表示是否有WARN。 0 = Has no WARN 1 = Has WARN |
...
请求数据包 | ||||||||||||
---|---|---|---|---|---|---|---|---|---|---|---|---|
命令代码 | 负载数据 | |||||||||||
0x90 | 0x02 | error id(u8) | ||||||||||
应答数据包 | ||||||||||||
应答代码 | 负载数据 | |||||||||||
<OK> | error_code(u32) | error_message(u8[32]) |
该命令数据域描述如下:
数据域 | 类型 | 描述 |
---|---|---|
error_id | u8 | 表示第几个error,是一个小于error count的整数. |
error_code | u32 | 具体请参考下表。 |
error_message | u8[32] | 表示具体错误信息的字符串. |
Error code定义如下:
Error Code | Bit | 描述 |
---|---|---|
错误等级 | [31:24] | 表示错误的严重程度。 |
错误组件 | [23:16] | 表示发生错误的具体位置或组件。 |
具体信息 | [15:8] | 表示具体的错误信息。 |
具体组件 | [7:0] | 表示具体某个组件有错误。 |
...
请求数据包 | ||||||||||||
---|---|---|---|---|---|---|---|---|---|---|---|---|
命令代码 | 负载数据 | |||||||||||
0x90 | 0x03 | error code(u32) | ||||||||||
应答数据包 | ||||||||||||
应答代码 | 负载数据 | |||||||||||
<OK> | 无 |
SR001 SLAMWARE Control Bus Protocol
外部系统辅助定位数据获取请求(GET_AUXILIARY_ANCHOR)
SLAMWARE CORE室内定位导航模块会不定期的向外部系统发送获取辅助定位数据相关的命令请求。
请求数据包 | ||||||||||||
---|---|---|---|---|---|---|---|---|---|---|---|---|
命令代码 | 负载数据 | |||||||||||
0x35 | 无 | |||||||||||
应答数据包 | ||||||||||||
应答代码 | 负载数据 | |||||||||||
<OK> | Flag(u8) | Anchors(u8[*]) |
该命令数据域描述如下:
数据域 | 类型 | 描述 |
---|---|---|
Flag | u8 | 具体请参考后附表格说明。 |
Anchors | u8[*] | 表示多个AnchorInfo的数据域。具体格式请参考后附表格说明。 |
Flag字段定义如下:
Flag | Bit | 描述 |
---|---|---|
辅助传感器类型 | [7:5] | 表示辅助传感器的类型。 |
辅助传感器标准差域 | [4] | 表示辅助数据是否支持标准差测量。 |
辅助传感器锚数量 | [15:8] | 表示辅助传感器锚数量。 |
AnchorInfo数据格式如下:
AnchorInfo[0] | 第0个AnchorInfo | 注:n 为辅助传感器数量。 |
AnchorInfo[1] | 第1个AnchorInfo | |
... | ||
AnchorInfo[n] | 第n个AnchorInfo |
AnchorInfo数据域定义如下:
AnchorInfo数据域 | 类型 | 描述 |
---|---|---|
id | u16 | 表示Anchor的id。 |
distance | u16 | 表示当前Anchor的距离单位为毫米。 |
Max error | u8 | 表示距离的标准差。单位为毫米。(如果传感器不支持标准差测量,则没有此数据域。) |
SR001 SLAMWARE Control Bus Protocol