学习目标

  1. 熟悉协议的定义
  2. 掌握协议生成
  3. 掌握协议解析
  4. 熟悉消息队列处理协议
  5. 熟悉消息队列处理业务

    学习内容

    协议的定义

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

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

以控制开发板上PID配置为例，定义出控制协议如下

帧头 命令位 数据长度 数据位 校验位 帧尾
idx P I D
字节数 1 1 1 1 4 4 4 1 1
默认值 0x7a 0x01 待定 待定 0x7b
  • idx：1个字节，int类型， 表示配置哪一组PID
  • P: 4个字节，float类型。P值
  • I: 4个字节，float类型。I值
  • D: 4个字节，float类型。D值

协议生成

通过上位机生成协议，然后通过串口发送给下位机。

类型转换

python中，构建的协议最终是bytes，但是有意义的数据类型是int、float等等类型，因此存在转换。

  1. def int2byte(v: int) -> bytes:
  2.     return struct.pack('B', v)
  5. def float2bytes(v: float) -> bytes:
  6.     return struct.pack('f', v)
  9. def bytes2float(v: bytes) -> float:
  10.     return struct.unpack('f', v)[0]

协议构建

协议构建就是拼装出规定数据：

帧头 命令位 数据长度 数据位 校验位 帧尾
字节数 1 1 1 n 1 1
默认值 0x7a 待定 待定 待定 待定 0x7b 
  1. def checksum(cmd: int, data: bytes) -> bytes:
  2.     sum = cmd
  3.     sum += len(data)
  4.     for d in data:
  5.         sum += d
  6.     # 取高位
  7.     sum &= 0xFF00
  8.     sum >>= 8
  9.     return int2byte(sum)
  12. def make_request(cmd: int, data:bytes) -> bytes:
  13.     #   帧头  |   命令  |   数据长度    |   数据  |   校验码 | 帧尾
  14.     #   1字节 |   1字节 |   1字节      |  n字节  |  1字节   | 1字节
  15.     # 命令: 请求类型的标识
  16.     # 数据长度: 表示后面 数据 的字节个数
  17.     # 校验码:  命令 + 数据长度 + 数据, 取高位
  18.     result = b''
  19.     result += int2byte(FRAME_HEAD)
  20.     result += int2byte(cmd)
  21.     result += int2byte(len(data))
  22.     result += data
  23.     result += checksum(cmd, data)
  24.     result += int2byte(FRAME_END)
  25.     return result

具体指令构建

具体指令构建是基于标准协议基础，处理具体逻辑的协议，为自定义：

帧头 命令位 数据长度 数据位 校验位 帧尾
idx P I D
字节数 1 1 1 1 4 4 4 1 1
默认值 0x7a 0x01 待定 待定 0x7b 
  1. def make_action_request(idx: int, p: float, i: float, d: float):
  2.     data = b''
  3.     data += int2byte(idx)
  4.     data += float2bytes(p)
  5.     data += float2bytes(i)
  6.     data += float2bytes(d)
  7.     return make_request(0x01, data)
  10. def print_request(request: bytes):
  11.     print()
  12.     for r in request:
  13.         print("{:02x} ".format(r), end='')
  14.     print()

串口调试

通过串口发送数据到设备，查看协议的接收

  1. import serial
  2. from protocol import *
  4. from threading import Thread
  7. def do_recv():
  8.     print('do recv')
  9.     while True:
  10.         buffer = ser.read(1)
  11.         print(buffer)
  14. if __name__ == '__main__':
  15.     ser = serial.Serial(baudrate=115200, port="COM15")
  17.     Thread(target=do_recv).start()
  19.     request = make_action_request(0, 10, 0.1, 0.3)
  20.     print_request(request)
  21.     ser.write(request)
  23.    # ser.close()

协议解析

通过下位机解析数据

环形缓冲

074.png
环形缓冲，也被称为环形队列，是一种常用的数据结构，用于实现在固定大小的缓冲区中高效地插入和删除数据。
环形缓冲区的特点是它具有固定的容量，并且在达到最大容量时，新的数据可以覆盖最旧的数据。这使得环形缓冲区适用于需要保持最新数据并丢弃旧数据的场景，如实时数据流处理、音频/视频流缓冲等。
环形缓冲区通常使用一个固定大小的数组来实现。该数组有两个指针，一个指向缓冲区的读取位置，另一个指向缓冲区的写入位置。读取位置用于获取缓冲区中的数据，写入位置用于添加新的数据。
主要操作为往环形数组中添加和移除数据。
添加如图所示:
075.png
添加数据其实，就是拨动结束的指针，然后长度加一

移除如图所示：
76.png
添加数据其实，就是拨动开始的指针，然后长度减一

环形缓冲实现:

  1. #define RING_SIZE    512
  2. uint8_t ring[RING_SIZE];
  3. uint32_t ring_head = 0;
  4. uint32_t ring_end = 0;
  5. uint32_t ring_len = 0;
  8. void ring_put(uint8_t data) {
  9.     if(ring_len > RING_SIZE) {
  10.         // ring is full. error
  11.         return;
  12.     }
  14.     ring[ring_end++] = data;
  15.     ring_end %= RING_SIZE;
  16.     ring_len++;
  17. }
  19. void ring_take(uint8_t *data) {
  20.     if(ring_len == 0) {
  21.         // ring is empty. 
  22.         return;
  23.     }
  24.     *data = ring[ring_head++];
  25.     ring_head %= RING_SIZE;
  26.     ring_len--;    
  27. }

解析逻辑

基于协议进行读取校验协议正确性

  1. void do_parse() {
  2.     //  帧头  |   命令  |   数据长度   |   数据  |  校验码  | 帧尾
  3.     //  1字节 |   1字节 |   1字节      |  n字节  |  1字节   | 1字节
  4.     //    命令: 请求类型的标识
  5.     //  数据长度: 表示后面 数据 的字节个数
  6.     //  校验码:  命令 + 数据长度 + 数据, 取高位
  8.     if(ring_len < 3) return;
  9.     uint8_t tmp;
  11.     uint8_t cmd;
  12.     uint8_t len;
  14.     // 判断帧头
  15.     if(ring[ring_head] != FRAME_RX_HEAD) {
  16.         // 不是，丢弃
  17.         ring_take(&tmp);
  18.         do_parse();
  19.         return;
  20.     }
  21.     // 解析命令
  22.     cmd = ring[(ring_head + 1) % RING_SIZE];
  24.     // 数据长度
  25.     len = ring[(ring_head + 2) % RING_SIZE];
  27.     // 判断数据长度
  28.     if(ring_len < len + 5) return;
  30.     // 帧尾
  31.     if(ring[(ring_head + 4 + len) % RING_SIZE] != FRAME_RX_END) {
  32.         // 丢弃第一个
  33.         // 不是，丢弃
  34.         ring_take(&tmp);
  35.         do_parse();
  36.         return;
  37.     }
  38.     uint16_t sum = 0;
  39.     sum += len;
  40.     uint32_t i;
  41.     for(i = 0;i < len; i++) {
  42.         sum += ring[(ring_head + 3 + i) % RING_SIZE];
  43.     }
  44.     sum >>= 8;
  45.     // 校验码
  46.     if(sum != ring[(ring_head + 3 + len) % RING_SIZE]) {
  47.         // 丢弃第一个
  48.         // 不是，丢弃
  49.         ring_take(&tmp);
  50.         do_parse();
  51.         return;
  52.     }
  54.     // 校验通过
  55.     uint8_t* action = malloc((len + 5) * sizeof(uint8_t));
  56.     for(i = 0;i < len + 5; i++) {
  57.         ring_take(&tmp);
  58.         action[i] = tmp;
  59.     }
  61.     Protocol_on_action(action, len + 5);
  63.     free(action);
  64. }

对于具体功能性协议，需要解析出具体功能，例如控制舵机角度的协议：

  1. static void Protocol_on_action(uint8_t *data, uint32_t len) {
  2.     uint8_t cmd = data[1];
  3.     if(cmd == CMD_PID && len == 18) {
  4.         uint8_t idx = data[3];
  6.         uint8_t tmp[4];
  7.         tmp[0] = data[4];
  8.         tmp[1] = data[5];
  9.         tmp[2] = data[6];
  10.         tmp[3] = data[7];
  11.         float p = bytesToFloat(tmp);
  12.         tmp[0] = data[8];
  13.         tmp[1] = data[9];
  14.         tmp[2] = data[10];
  15.         tmp[3] = data[11];
  16.         float i = bytesToFloat(tmp);
  17.         tmp[0] = data[12];
  18.         tmp[1] = data[13];
  19.         tmp[2] = data[14];
  20.         tmp[3] = data[15];
  21.         float d = bytesToFloat(tmp);
  23.         Protocol_on_pid(idx, p, i, d);
  24.     }
  25. }

处理数据转换

  1. typedef union {
  2.     float f;
  3.     unsigned char b[4];
  4. } FloatBytes;
  6. static void floatToBytes(float value, unsigned char *bytes) {
  7.     FloatBytes fb;
  8.     fb.f = value;
  9.     bytes[0] = fb.b[0];
  10.     bytes[1] = fb.b[1];
  11.     bytes[2] = fb.b[2];
  12.     bytes[3] = fb.b[3];
  13. }
  15. static float bytesToFloat(unsigned char *bytes) {
  16.     FloatBytes fb;
  17.     fb.b[0] = bytes[0];
  18.     fb.b[1] = bytes[1];
  19.     fb.b[2] = bytes[2];
  20.     fb.b[3] = bytes[3];
  21.     return fb.f;
  22. }

完整逻辑

  1. #include "protocol.h"
  2. #include <stdio.h>
  4. #define FRAME_RX_HEAD    0xFA
  5. #define FRAME_RX_END       0xFB
  7. #define CMD_LEG        0x01
  10. #define RING_SIZE    512
  11. uint8_t ring[RING_SIZE];
  12. uint32_t ring_head = 0;
  13. uint32_t ring_end = 0;
  14. uint32_t ring_len = 0;
  17. //////////////////////////// 数据转换：不同平台可能存在大小端问题 /////////
  18. ////////////////////// 当前是大端，修改顺序即可改为小端 ///////////////////
  19. typedef union {
  20.     float f;
  21.     unsigned char b[4];
  22. } FloatBytes;
  24. static void floatToBytes(float value, unsigned char *bytes) {
  25.     FloatBytes fb;
  26.     fb.f = value;
  27.     bytes[0] = fb.b[0];
  28.     bytes[1] = fb.b[1];
  29.     bytes[2] = fb.b[2];
  30.     bytes[3] = fb.b[3];
  31. }
  33. static float bytesToFloat(unsigned char *bytes) {
  34.     FloatBytes fb;
  35.     fb.b[0] = bytes[0];
  36.     fb.b[1] = bytes[1];
  37.     fb.b[2] = bytes[2];
  38.     fb.b[3] = bytes[3];
  39.     return fb.f;
  40. }
  41. ///////////////////////////////////////////////////////////////
  43. void ring_put(uint8_t data) {
  44.     if(ring_len > RING_SIZE) {
  45.         // ring is full. error
  46.         return;
  47.     }
  49.     ring[ring_end++] = data;
  50.     ring_end %= RING_SIZE;
  51.     ring_len++;
  52. }
  54. void ring_take(uint8_t *data) {
  55.     if(ring_len == 0) {
  56.         // ring is empty. 
  57.         return;
  58.     }
  59.     *data = ring[ring_head++];
  60.     ring_head %= RING_SIZE;
  61.     ring_len--;
  63. }
  67. void Protocol_init() {
  69. }
  71. static void Protocol_on_action(uint8_t *data, uint32_t len) {
  72.     uint8_t cmd = data[1];
  73.     if(cmd == CMD_PID && len == 18) {
  74.         uint8_t idx = data[3];
  76.         uint8_t tmp[4];
  77.         tmp[0] = data[4];
  78.         tmp[1] = data[5];
  79.         tmp[2] = data[6];
  80.         tmp[3] = data[7];
  81.         float p = bytesToFloat(tmp);
  82.         tmp[0] = data[8];
  83.         tmp[1] = data[9];
  84.         tmp[2] = data[10];
  85.         tmp[3] = data[11];
  86.         float i = bytesToFloat(tmp);
  87.         tmp[0] = data[12];
  88.         tmp[1] = data[13];
  89.         tmp[2] = data[14];
  90.         tmp[3] = data[15];
  91.         float d = bytesToFloat(tmp);
  93.         Protocol_on_pid(idx, p, i, d);
  94.     }
  95. }
  97. static void do_parse() {
  98.     //  帧头  |   命令  |   数据长度   |   数据  |  校验码  | 帧尾
  99.     //  1字节 |   1字节 |   1字节      |  n字节  |  1字节   | 1字节
  100.     //    命令: 请求类型的标识
  101.     //  数据长度: 表示后面 数据 的字节个数
  102.     //  校验码:  命令 + 数据长度 + 数据, 取高位
  104.     if(ring_len < 3) return;
  105.     uint8_t tmp;
  107.     uint8_t cmd;
  108.     uint8_t len;
  110.     // 判断帧头
  111.     if(ring[ring_head] != FRAME_RX_HEAD) {
  112.         // 不是，丢弃
  113.         ring_take(&tmp);
  114.         do_parse();
  115.         return;
  116.     }
  117.     // 解析命令
  118.     cmd = ring[(ring_head + 1) % RING_SIZE];
  120.     // 数据长度
  121.     len = ring[(ring_head + 2) % RING_SIZE];
  123.     // 判断数据长度
  124.     if(ring_len < len + 5) return;
  126.     // 帧尾
  127.     if(ring[(ring_head + 4 + len) % RING_SIZE] != FRAME_RX_END) {
  128.         // 丢弃第一个
  129.         // 不是，丢弃
  130.         ring_take(&tmp);
  131.         do_parse();
  132.         return;
  133.     }
  134.     uint16_t sum = 0;
  135.     sum += len;
  136.     uint32_t i;
  137.     for(i = 0;i < len; i++) {
  138.         sum += ring[(ring_head + 3 + i) % RING_SIZE];
  139.     }
  140.     sum >>= 8;
  141.     // 校验码
  142.     if(sum != ring[(ring_head + 3 + len) % RING_SIZE]) {
  143.         // 丢弃第一个
  144.         // 不是，丢弃
  145.         ring_take(&tmp);
  146.         do_parse();
  147.         return;
  148.     }
  150.     // 校验通过
  151.     uint8_t* action = malloc((len + 5) * sizeof(uint8_t));
  152.     for(i = 0;i < len + 5; i++) {
  153.         ring_take(&tmp);
  154.         action[i] = tmp;
  155.     }
  157.     Protocol_on_action(action, len + 5);
  159.     free(action);
  160. }
  162. void Protocol_parse(uint8_t *data, uint32_t len) {
  163.     uint32_t i;
  164.     for(i = 0;i < len;i++) {
  165.         ring_put(data[i]);
  166.     }
  168.     do_parse();
  169. }
  1. #ifndef __PROTOCOL_H__
  2. #define __PROTOCOL_H__
  4. #include "gd32f4xx.h"
  5. #include "systick.h"
  7. void Protocol_init();
  9. void Protocol_parse(uint8_t *data, uint32_t len);
  11. extern void Protocol_on_pid(uint8_t idx, float p, float i, float d);
  14. #endif

消息调度

串口只负责接收数据，不做重活。

  1. void Usart0_recv(uint8_t *data, uint32_t len) {
  2.     uint32_t i = 0, cnt = 32, size = len / cnt;
  3.     bt_rx_data_t buffer;
  5.     printf("BT recv \r\n");
  7.     //portENTER_CRITICAL();
  9.     for(i = 0; i < size; i++) {
  10.         printf("BT A recv %d \r\n", i);
  11.         memcpy(buffer.data, &data[i*cnt], cnt);
  12.         buffer.len = cnt;
  13.         xQueueSendFromISR(bt_rx_queue, &buffer, 0);
  14.     }
  16.     if(len % cnt != 0) {
  17.         printf("BT B recv %d \r\n", i);
  18.         memcpy(buffer.data, &data[i*cnt], len % cnt);
  19.         buffer.len = len % cnt;
  20.         xQueueSendFromISR(bt_rx_queue, &buffer, 0);
  21.         //xQueueSend(bt_rx_queue, &buffer, 0);
  22.     }
  23.     //portEXIT_CRITICAL();
  25.     printf("BT recv end\r\n");
  26. }

开启任务，负责专门的解析

  1. static void bt_rx_task() {
  2.     bt_rx_queue = xQueueCreate(64, sizeof(bt_rx_data_t));
  3.     bt_rx_data_t data;
  4.     uint32_t i;
  5.     while(1) {
  6.         if(xQueueReceive(bt_rx_queue, &data, portMAX_DELAY)) {
  7.             printf("BT parse \r\n");
  8.             for(i = 0; i < data.len; i++) {
  9.                 Protocol_pull(data.data[i]);
  11.                 printf("%02X ", data.data[i]);
  12.             }
  13.             printf("\r\n");
  14.             Protocol_parse();
  15.         }
  16.     }
  17. }

针对具体逻辑，开启消息队列处理具体事项。以四条腿同时动作为例：

  1. void PID_on_pid(unsigned char chn, float kp, float ki, float kd) {
  2.     if(chn == 0) {
  3.         Balance_Kp = kp;
  4.         Balance_Ki = ki;
  5.         Balance_Kd = kd;
  7.         printf("Update Bp: %4.2f Bi: %4.2f Bd: %4.2f\n", kp, ki, kd);
  9.         left = 0;
  10.         right = 0;
  11.         Motors_setSpeeds(0, 0);
  12.     } else if (chn == 1) {
  13.         Velocity_Kp = kp;
  14.         Velocity_Ki = kp / 200.0f;
  15.         printf("Update Vp: %4.2f Vi: %4.2f \n", Velocity_Kp, Velocity_Ki);
  16.         Motors_setSpeeds(0, 0);
  17.     }
  18. }

练习

  1. 上位机GUI实现下位机控制，协议为自定义