协议规范

端到端

分为PC端和设备端

  • PC端运行调试程序
  • 设备端实现自己的业务

设备端向PC端发送数据，称之为PUSH
设备端获取PC端数据，称之为PULL

PUSH 协议规范

帧头 命令位 数据长度 数据位 校验位 帧尾
字节数 1 1 1 n 1 1
默认值 0x7a 待定 待定 待定 待定 0x7b

命令位: 表示命令类型
数据位的n值，由数据长度位的值决定
校验位：(命令位 + 数据长度位 + 数据位)的结果取高位

PULL 协议规范

帧头 命令位 数据长度 数据位 校验位 帧尾
字节数 1 1 1 n 1 1
默认值 0x7b 待定 待定 待定 待定 0x7a

命令位: 表示命令类型
数据位的n值，由数据长度位的值决定
校验位：(命令位 + 数据长度位 + 数据位)的结果取高位

已实现协议

命令 方向 说明 示例
0x00 接收 速度 0x7a 0x00 11 2 2 2 1 0x7b
0x01 接收 直立环PID 0x7a 0x01 17 4 4 4 1 0x7b
0x02 接收 速度环PID 0x7a 0x02 17 4 4 4 1 0x7b
0x03 接收 转向环PID 0x7a 0x03 17 4 4 4 1 0x7b

PUSH协议

通道消息

帧头 命令位 数据长度 数据位 校验位 帧尾
字节数 1 1 1 4 …… 1 1
默认值 0x7a 0x01 待定 计算获得 0x7b
  • 数据位长度为4的倍数
  • 每4位表示一个通道
  • 每4位表示一个float类型数据

速度消息

帧头 命令位 数据长度 数据位 校验位 帧尾
x y z ax ay az gx gy gz v
字节数 1 1 1 4 4 4 4 4 4 4 4 4 4 1 1
默认值 0x7a 0x01 待定 待定 0x7b
  • x：4个字节，float类型。x轴速度
  • y: 4个字节，float类型。y轴速度
  • z: 4个字节，float类型。z轴速度
  • ax：4个字节，float类型。x轴加速度
  • ay: 4个字节，float类型。y轴加速度
  • az: 4个字节，float类型。z轴加速度
  • gx：4个字节，float类型。x轴角速度
  • gy: 4个字节，float类型。y轴角速度
  • gz: 4个字节，float类型。z轴角速度
  • v: 4个字节，float类型。电压值

PULL协议

配置PID

帧头 命令位 数据长度 数据位 校验位 帧尾
类 P i d
字节数 1 1 1 1 4 4 4 1 1
默认值 0x7b 0x01 待定 待定 0x7a

数据位包含了

  • id: 第几组pid
  • p：4个字节，float类型
  • i: 4个字节，float类型
  • d: 4个字节，float类型

设置速度

帧头 命令位 数据长度 数据位 校验位 帧尾
x y z
字节数 1 1 1 4 4 4 1 1
默认值 0x7b 0x02 待定 待定 0x7a

数据位包含了

  • x：4个字节，float类型。x轴速度
  • y: 4个字节，float类型。y轴速度
  • z: 4个字节，float类型。z轴速度

库使用说明

库文件

  1. #ifndef __PID_H__
  2. #define __PID_H__
  6. void PID_init();
  7. void PID_push(float *chns, int chn_cnt);
  8. void PID_pull(unsigned char dat);
  9. void PID_parse();
  11. extern void PID_on_config_pid(unsigned char id, float kp, float ki, float kd);
  14. #endif
  1. #include "PID.h"
  2. #include "Bluetooth.h"
  4. //////////// PUSH ///////////////
  5. #define TX_FRAME_HEAD    0x7a
  6. #define TX_FRAME_END    0x7b
  7. #define TX_CMD_CHN        0x01
  9. //////////// PULL //////////////
  10. #define RX_FRAME_HEAD    0x7b
  11. #define RX_FRAME_END    0x7a
  12. #define RX_CMD_PID        0x01
  14. #define MAX_BUF        64
  15. static unsigned char buf[MAX_BUF];
  16. static unsigned char buf_idx = 0;
  17. static unsigned char buf_len = 0;
  19. //////////////////////////// 根据实际情况修改以下代码调用方式////////////////
  20. static void PID_send(unsigned char dat) {
  21.     BT_send(dat);
  22. }
  23. ////////////////////////////////////////////////////////////////////////////
  25. //////////////////////////// 数据转换：不同平台可能存在大小端问题 /////////
  26. ////////////////////// 当前是大端，修改顺序即可改为小端 ///////////////////
  27. typedef union {
  28.     float f;
  29.     unsigned char b[4];
  30. } FloatBytes;
  32. static void floatToBytes(float value, unsigned char *bytes) {
  33.     FloatBytes fb;
  34.     fb.f = value;
  35.     bytes[0] = fb.b[3];
  36.     bytes[1] = fb.b[2];
  37.     bytes[2] = fb.b[1];
  38.     bytes[3] = fb.b[0];
  39. }
  41. static float bytesToFloat(unsigned char *bytes) {
  42.     FloatBytes fb;
  43.     fb.b[0] = bytes[3];
  44.     fb.b[1] = bytes[2];
  45.     fb.b[2] = bytes[1];
  46.     fb.b[3] = bytes[0];
  47.     return fb.f;
  48. }
  49. ///////////////////////////////////////////////////////////////
  51. void PID_init() {
  53. }
  55. void PID_push(float *chns, int chn_cnt) {
  56.     //帧头  |   命令  |   数据长度   |   数据  |   校验码 | 帧尾
  57.     //1字节 |   1字节 |   1字节      |  n字节  |  1字节   | 1字节
  58.     //命令: 请求类型的标识
  59.     //数据长度: 表示后面 数据 的字节个数
  60.     //校验码:  命令 + 数据长度 + 数据, 取高位
  61.     unsigned char i;
  62.     float chn;
  63.     unsigned char tmp[4];
  64.     unsigned short sum = 0;
  66.     PID_send(TX_FRAME_HEAD);
  67.     PID_send(TX_CMD_CHN);
  68.     PID_send(chn_cnt * 4);
  69.     sum += TX_CMD_CHN;
  70.     sum += chn_cnt * 4;
  71.     for(i = 0; i < chn_cnt; i++) {
  72.         floatToBytes(chns[i], tmp);
  74.         PID_send(tmp[0]);
  75.         sum += tmp[0];
  76.         PID_send(tmp[1]);
  77.         sum += tmp[1];
  78.         PID_send(tmp[2]);
  79.         sum += tmp[2];
  80.         PID_send(tmp[3]);
  81.         sum += tmp[3];
  82.     }
  83.     PID_send((sum >> 8) & 0xFF);
  84.     PID_send(TX_FRAME_END);
  85. }
  87. static void parse_config_pid() {
  88.         float kp, ki, kd;
  89.         kp = bytesToFloat(&buf[(buf_idx + 4) % MAX_BUF]);
  90.         ki = bytesToFloat(&buf[(buf_idx + 8) % MAX_BUF]);
  91.         kd = bytesToFloat(&buf[(buf_idx + 12) % MAX_BUF]);
  92.         PID_on_config_pid(buf[buf[(buf_idx + 3) % MAX_BUF], kp, ki, kd);
  93. }
  95. //////////////////// 如果要去增加其他命令的解析，可以修改这个函数 ////////////
  96. static void on_cmd() {
  97.         unsigned char cmd;
  99.         cmd = buf[(buf_idx + 1) % MAX_BUF];
  100.         if(cmd == RX_CMD_PID) {
  101.                 parse_config_pid();
  102.         }
  103. }
  104. //////////////////////////////////////////////////////////////////////////////
  106. void PID_parse() {
  107.     unsigned char dat_len, i;
  108.     unsigned short sum = 0;
  109.     //////////////// check cmd
  110.     if(buf_len < 3) return;
  112.     // 消息组成
  113.     // 帧头  |   命令  |   数据长度    |   数据  |   校验码 | 帧尾
  114.     // 1字节 |   1字节 |   1字节       |  n字节  |  1字节   | 1字节
  115.     // 命令: 请求类型的标识
  116.     // 数据长度: 表示后面 数据 的字节个数
  117.     // 校验码:  命令 + 数据长度 + 数据, 取高位
  119.     // check frame head
  120.     if(buf[buf_idx] != RX_FRAME_HEAD) {
  121.         // drop one
  122.         buf_idx++;
  123.         buf_idx %= MAX_BUF;
  124.                 if(buf_len != 0) buf_len--;
  126.         PID_parse();
  127.         return;
  128.     }
  130.     // check dat len
  131.     dat_len = buf[(buf_idx + 2) % MAX_BUF];
  132.     // check cmd total len
  133.     if(buf_len < dat_len + 5) return;
  135.     // check sum
  136.     sum += buf[(buf_idx + 1) % MAX_BUF];
  137.     sum += buf[(buf_idx + 2) % MAX_BUF];
  138.     for(i = 0; i < dat_len; i++) {
  139.             sum += buf[(buf_idx + 3 + i) % MAX_BUF];
  140.     }
  142.         if(((sum >> 8) & 0xFF) != buf[(buf_idx + dat_len + 3) % MAX_BUF]) {
  143.                 // drop
  144.                 buf_idx++;
  145.         buf_idx %= MAX_BUF;
  146.         if(buf_len != 0) buf_len--;
  148.         PID_parse();
  149.         return;
  150.         }
  151.         // check frame end
  152.         if(buf[(buf_idx + dat_len + 4) % MAX_BUF] != RX_FRAME_END) {
  153.                 // drop
  154.                 buf_idx++;
  155.         buf_idx %= MAX_BUF;
  156.         if(buf_len != 0) buf_len--;
  158.         PID_parse();
  159.         return;
  160.         }
  161.         // parse success
  162.         /**
  163.         if(buf[(buf_idx + 1) % MAX_BUF] == RX_CMD_PID) {
  164.                 // config pid
  165.                 parse_config_pid();
  166.         }
  167.         **/
  168.         on_cmd();
  170.         // drop
  171.         buf_idx += dat_len + 5;
  172.     buf_idx %= MAX_BUF;
  173.         if(buf_len >= dat_len + 5) {
  174.                 buf_len -= dat_len + 5;
  175.         } else {
  176.                 buf_len = 0;
  177.         }
  179.         PID_parse();
  180. }
  182. void PID_pull(unsigned char dat) {
  183.         unsigned char idx = buf_idx + buf_len;
  184.         idx %= MAX_BUF;
  185.     // config dat
  186.     buf[idx] = dat;
  187.     buf_len ++;
  189.         if(buf_len > MAX_BUF) {
  190.             buf_idx = 0;
  191.             buf_len = 0;
  192.         }
  193. }

使用步骤

  1. 拷贝都文件和c文件到项目种

  2. 修改c文件中PID__send函数实现

    1. #include "Bluetooth.h"
    2. ....
    3. static void PID_send(unsigned char dat) {
    4.  BT_send(dat);
    5. }

    修改为自己的实现，以上是蓝牙的实现，发送的是单个byte

  3. 调用PID_init()初始化

  4. 在需要的地方调用PID_push

    1. void PID_push(float *chns, int chn_cnt);
    • 第一个参数是float数组
    • 第二个是数组长度
    • 一个元素对应一个GUI显示通道

      高级配置

命令 方向 说明 示例
0x00 接收 速度 0x7a 0x00 11 2 2 2 1 0x7b
0x01 接收 直立环PID 0x7a 0x01 17 4 4 4 1 0x7b
0x02 接收 速度环PID 0x7a 0x02 17 4 4 4 1 0x7b
0x03 接收 转向换 0x7a 0x03 17 4 4 4 1 0x7b

协议文档

  1.     /*
  2.         接收的数据：
  3.         帧头    标志位 长度  X速度    Y速度  Z速度  校验位  帧尾
  4.         0x7a   0x00  11      2     2      2      1    0x7b   11位
  7.         帧头  标志位 长度    kp  ki  kd   校验位1 帧尾     直立环
  8.           0x7a   0x01  17     4   4  4       1     0x7b 
  10.         帧头  标志位 长度    kp  ki  kd   校验位1 帧尾     速度环    
  11.           0x7a   0x02  17     4   4  4       1     0x7b 
  13.         帧头  标志位 长度    kp  ki  kd   校验位1 帧尾     转向换
  14.           0x7a   0x03  17     4   4  4       1     0x7b
  16.         浮点数据,放大100倍,转为整数发送
  18.         0x7a 0x00 0x00 0x11 0x11 0x11 0x11 0x12 0x13 0x12 0x7b
  20.         发出去的数据：
  21.         帧头 停止位  X速度 Y速度 Z速度 X加速度 Y加速度 Z加速度 X角速度 Y角速度 Z角速度 电压 校验位 帧尾
  22.         0x7b    1     2     2    2     2         2     2      2       2       2      2    1   0x7a
  24.         short 是2个字节，范围-32768~32767
  25.     */