From 95607668e1959b828b1c5b7b42f3c49bd78f32b4 Mon Sep 17 00:00:00 2001 From: NeoZng Date: Thu, 3 Nov 2022 20:38:55 +0800 Subject: [PATCH] add vision protocol --- .vscode/settings.json | 4 +- Makefile | 6 +- modules/algorithm/crc16.c | 101 ++++++++++++++++ modules/algorithm/crc16.h | 14 +++ modules/algorithm/crc8.c | 61 ++++++++++ modules/algorithm/crc8.h | 11 ++ modules/algorithm/kalman_filter.h | 2 +- modules/algorithm/user_lib.c | 39 +++---- modules/master_machine/master_process.c | 55 ++++++++- modules/master_machine/master_process.h | 19 +++ modules/master_machine/seasky_protocol.c | 142 +++++++++++++++++++++++ modules/master_machine/seasky_protocol.h | 108 +++++++++++++++++ 12 files changed, 537 insertions(+), 25 deletions(-) create mode 100644 modules/algorithm/crc16.c create mode 100644 modules/algorithm/crc16.h create mode 100644 modules/algorithm/crc8.c create mode 100644 modules/algorithm/crc8.h create mode 100644 modules/master_machine/seasky_protocol.c create mode 100644 modules/master_machine/seasky_protocol.h diff --git a/.vscode/settings.json b/.vscode/settings.json index dcf3469..6b5447e 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -39,6 +39,8 @@ "typeinfo": "c", "chrono": "c", "complex": "c", - "usb_device.h": "c" + "usb_device.h": "c", + "vofa_protocol.h": "c", + "master_process.h": "c" } } \ No newline at end of file diff --git a/Makefile b/Makefile index 45b9a41..51cbaaa 100644 --- a/Makefile +++ b/Makefile @@ -118,7 +118,11 @@ modules/motor/LK9025.c \ modules/motor/motor_task.c \ modules/referee/referee.c \ modules/remote/remote_control.c \ -modules/super_cap/super_cap.c +modules/super_cap/super_cap.c \ +modules/master_machine/seasky_protocol.c \ +modules/algorithm/crc8.c \ +modules/algorithm/crc16.c + # ASM sources ASM_SOURCES = \ diff --git a/modules/algorithm/crc16.c b/modules/algorithm/crc16.c new file mode 100644 index 0000000..2833689 --- /dev/null +++ b/modules/algorithm/crc16.c @@ -0,0 +1,101 @@ +#include "crc16.h" + +static uint8_t crc_tab16_init = 0; +static uint16_t crc_tab16[256]; + +/* + * uint16_t crc_16( const unsigned char *input_str, size_t num_bytes ); + * + *函数crc_16()一次计算一个字节的16位CRC16 + *其开头已传递给函数的字符串。的数量 + *要检查的字节也是一个参数。字符串中的字节数为 + *受恒定大小最大值的限制。 + */ +uint16_t crc_16(const uint8_t *input_str, uint16_t num_bytes) +{ + uint16_t crc; + const uint8_t *ptr; + uint16_t a; + if (!crc_tab16_init) + init_crc16_tab(); + crc = CRC_START_16; + ptr = input_str; + if (ptr != NULL) + for (a = 0; a < num_bytes; a++) + { + crc = (crc >> 8) ^ crc_tab16[(crc ^ (uint16_t)*ptr++) & 0x00FF]; + } + return crc; +} + +/* + * uint16_t crc_modbus( const unsigned char *input_str, size_t num_bytes ); + * + *函数crc_modbus()一次计算16位modbus循环冗余校验 + *一个字节字符串，其开头已被传递给函数。这 + *要检查的字节数也是一个参数。 + */ + +uint16_t crc_modbus(const uint8_t *input_str, uint16_t num_bytes) +{ + uint16_t crc; + const uint8_t *ptr; + uint16_t a; + + if (!crc_tab16_init) + init_crc16_tab(); + + crc = CRC_START_MODBUS; + ptr = input_str; + if (ptr != NULL) + for (a = 0; a < num_bytes; a++) + { + + crc = (crc >> 8) ^ crc_tab16[(crc ^ (uint16_t)*ptr++) & 0x00FF]; + } + return crc; +} + +/* + * uint16_t update_crc_16( uint16_t crc, unsigned char c ); + * + *函数update_crc_16()根据 + *前一个循环冗余校验值和下一个要检查的数据字节。 + */ +uint16_t update_crc_16(uint16_t crc, uint8_t c) +{ + if (!crc_tab16_init) + init_crc16_tab(); + return (crc >> 8) ^ crc_tab16[(crc ^ (uint16_t)c) & 0x00FF]; +} + +/* + * static void init_crc16_tab( void ); + * + *为了获得最佳性能，使用CRC16例程查找带有值的表 + *可以直接在异或算法中使用的算法。 + *查找表首次由init_crc16_tab()例程计算 + *调用循环冗余校验函数。 + */ +void init_crc16_tab(void) +{ + uint16_t i; + uint16_t j; + uint16_t crc; + uint16_t c; + for (i = 0; i < 256; i++) + { + crc = 0; + c = i; + for (j = 0; j < 8; j++) + { + if ((crc ^ c) & 0x0001) + crc = (crc >> 1) ^ CRC_POLY_16; + else + crc = crc >> 1; + c = c >> 1; + } + crc_tab16[i] = crc; + } + crc_tab16_init = 1; +} diff --git a/modules/algorithm/crc16.h b/modules/algorithm/crc16.h new file mode 100644 index 0000000..7baffd8 --- /dev/null +++ b/modules/algorithm/crc16.h @@ -0,0 +1,14 @@ +#ifndef __CRC16_H +#define __CRC16_H +#include "main.h" + +#define CRC_START_16 0xFFFF +#define CRC_START_MODBUS 0xFFFF +#define CRC_POLY_16 0xA001 + +uint16_t crc_16(const uint8_t *input_str, uint16_t num_bytes); +uint16_t crc_modbus(const uint8_t *input_str, uint16_t num_bytes); +uint16_t update_crc_16(uint16_t crc, uint8_t c); +void init_crc16_tab(void); + +#endif diff --git a/modules/algorithm/crc8.c b/modules/algorithm/crc8.c new file mode 100644 index 0000000..20fe5df --- /dev/null +++ b/modules/algorithm/crc8.c @@ -0,0 +1,61 @@ +#include "crc8.h" + +/* + * static uint8_t sht75_crc_table[]; + * + * The SHT75 humidity sensor is capable of calculating an 8 bit CRC checksum to + * ensure data integrity. The lookup table crc_table[] is used to recalculate + * the CRC. + */ +static uint8_t sht75_crc_table[] = + { + 0, 49, 98, 83, 196, 245, 166, 151, 185, 136, 219, 234, 125, 76, 31, 46, + 67, 114, 33, 16, 135, 182, 229, 212, 250, 203, 152, 169, 62, 15, 92, 109, + 134, 183, 228, 213, 66, 115, 32, 17, 63, 14, 93, 108, 251, 202, 153, 168, + 197, 244, 167, 150, 1, 48, 99, 82, 124, 77, 30, 47, 184, 137, 218, 235, + 61, 12, 95, 110, 249, 200, 155, 170, 132, 181, 230, 215, 64, 113, 34, 19, + 126, 79, 28, 45, 186, 139, 216, 233, 199, 246, 165, 148, 3, 50, 97, 80, + 187, 138, 217, 232, 127, 78, 29, 44, 2, 51, 96, 81, 198, 247, 164, 149, + 248, 201, 154, 171, 60, 13, 94, 111, 65, 112, 35, 18, 133, 180, 231, 214, + 122, 75, 24, 41, 190, 143, 220, 237, 195, 242, 161, 144, 7, 54, 101, 84, + 57, 8, 91, 106, 253, 204, 159, 174, 128, 177, 226, 211, 68, 117, 38, 23, + 252, 205, 158, 175, 56, 9, 90, 107, 69, 116, 39, 22, 129, 176, 227, 210, + 191, 142, 221, 236, 123, 74, 25, 40, 6, 55, 100, 85, 194, 243, 160, 145, + 71, 118, 37, 20, 131, 178, 225, 208, 254, 207, 156, 173, 58, 11, 88, 105, + 4, 53, 102, 87, 192, 241, 162, 147, 189, 140, 223, 238, 121, 72, 27, 42, + 193, 240, 163, 146, 5, 52, 103, 86, 120, 73, 26, 43, 188, 141, 222, 239, + 130, 179, 224, 209, 70, 119, 36, 21, 59, 10, 89, 104, 255, 206, 157, 172}; + +/* + * uint8_t crc_8( const unsigned char *input_str, size_t num_bytes ); + * + *函数crc_8()计算输入字符串的8位宽crc + *给定长度。 + */ +uint8_t crc_8(const uint8_t *input_str, uint16_t num_bytes) +{ + uint16_t a; + uint8_t crc; + const uint8_t *ptr; + + crc = CRC_START_8; + ptr = input_str; + if (ptr != NULL) + for (a = 0; a < num_bytes; a++) + { + crc = sht75_crc_table[(*ptr++) ^ crc]; + } + return crc; +} + +/* + * uint8_t update_crc_8( unsigned char crc, unsigned char val ); + * + *给定一个数据字节和循环冗余校验值的前一个值 + *update_crc_8()计算并返回数据的新的实际crc值 + */ + +uint8_t update_crc_8(uint8_t crc, uint8_t val) +{ + return sht75_crc_table[val ^ crc]; +} diff --git a/modules/algorithm/crc8.h b/modules/algorithm/crc8.h new file mode 100644 index 0000000..278c70f --- /dev/null +++ b/modules/algorithm/crc8.h @@ -0,0 +1,11 @@ +#ifndef __CRC8_H +#define __CRC8_H + +#include "main.h" + +#define CRC_START_8 0x00 + +uint8_t crc_8(const uint8_t *input_str, uint16_t num_bytes); +uint8_t update_crc_8(uint8_t crc, uint8_t val); + +#endif diff --git a/modules/algorithm/kalman_filter.h b/modules/algorithm/kalman_filter.h index 7bdf880..8c65887 100644 --- a/modules/algorithm/kalman_filter.h +++ b/modules/algorithm/kalman_filter.h @@ -92,7 +92,7 @@ typedef struct kf_t void (*User_Func4_f)(struct kf_t *kf); void (*User_Func5_f)(struct kf_t *kf); void (*User_Func6_f)(struct kf_t *kf); - + // 矩阵存储空间指针 float *xhat_data, *xhatminus_data; float *u_data; diff --git a/modules/algorithm/user_lib.c b/modules/algorithm/user_lib.c index d091884..f1754b7 100644 --- a/modules/algorithm/user_lib.c +++ b/modules/algorithm/user_lib.c @@ -1,15 +1,15 @@ /** - ****************************************************************************** - * @file user_lib.c - * @author Wang Hongxi - * @version V1.0.0 - * @date 2021/2/18 - * @brief - ****************************************************************************** - * @attention - * - ****************************************************************************** - */ + ****************************************************************************** + * @file user_lib.c + * @author Wang Hongxi + * @version V1.0.0 + * @date 2021/2/18 + * @brief + ****************************************************************************** + * @attention + * + ****************************************************************************** + */ #include "stdlib.h" #include "string.h" #include "user_lib.h" @@ -51,16 +51,13 @@ float Sqrt(float x) return y; } - - - /** - * @brief 斜波函数计算，根据输入的值进行叠加， 输入单位为 /s 即一秒后增加输入的值 - * @author RM - * @param[in] 斜波函数结构体 - * @param[in] 输入值 - * @retval 返回空 - */ + * @brief 斜波函数计算，根据输入的值进行叠加， 输入单位为 /s 即一秒后增加输入的值 + * @author RM + * @param[in] 斜波函数结构体 + * @param[in] 输入值 + * @retval 返回空 + */ float ramp_calc(ramp_function_source_t *ramp_source_type, float input) { ramp_source_type->input = input; @@ -113,7 +110,7 @@ float float_deadband(float Value, float minValue, float maxValue) return Value; } -//int26死区 +// int26死区 int16_t int16_deadline(int16_t Value, int16_t minValue, int16_t maxValue) { if (Value < maxValue && Value > minValue) diff --git a/modules/master_machine/master_process.c b/modules/master_machine/master_process.c index 3adaad1..1fa3c18 100644 --- a/modules/master_machine/master_process.c +++ b/modules/master_machine/master_process.c @@ -1 +1,54 @@ -#include "Master_process.h" \ No newline at end of file +/** + * @file master_process.c + * @author neozng + * @brief module for recv&send vision data + * @version beta + * @date 2022-11-03 + * + * @copyright Copyright (c) 2022 + * + */ +#include "Master_process.h" + +/* use usart1 as vision communication*/ +static Vision_Recv_s recv_data; +static usart_instance vision_usart_instance; + +/** + * @brief 接收解包回调函数,将在bsp_usart.c中被usart rx callback调用 + * @todo 1.提高可读性,将get_protocol_info的第四个参数增加一个float buffer + * 2.添加标志位解码 + */ +static void DecodeVision() +{ + static uint16_t flag_register; + get_protocol_info(vision_usart_instance.recv_buff, &flag_register, &recv_data.pitch); + // TODO: code to resolve flag_register; +} + +/* 视觉通信初始化 */ +void VisionInit(UART_HandleTypeDef *handle) +{ + vision_usart_instance.module_callback = DecodeVision; + vision_usart_instance.recv_buff_size = VISION_RECV_SIZE; + vision_usart_instance.usart_handle = handle; + USARTRegister(&vision_usart_instance); +} + +/** + * @brief 发送函数 + * @todo 1.提高可读性,将get_protocol_info的第四个参数增加一个float buffer + * 2.添加标志位解码 + * + * @param send 待发送数据 + */ +void VisionSend(Vision_Send_s *send) +{ + static uint16_t flag_register; + static uint8_t send_buff[VISION_SEND_SIZE]; + static uint16_t tx_len; + // TODO: code to set flag_register + + get_protocol_send_data(0x02, flag_register, &send->yaw, 3, send_buff, &tx_len); + USARTSend(&vision_usart_instance, send_buff, tx_len); +} diff --git a/modules/master_machine/master_process.h b/modules/master_machine/master_process.h index 72b69b0..e0d0df3 100644 --- a/modules/master_machine/master_process.h +++ b/modules/master_machine/master_process.h @@ -2,7 +2,26 @@ #define MASTER_PROCESS_H #include "bsp_usart.h" +#include "usart.h" +#include "seasky_protocol.h" +#define VISION_RECV_SIZE 36u +#define VISION_SEND_SIZE 36u + + +/** + * @brief 调用此函数初始化和视觉的串口通信 + * + * @param handle 用于和视觉通信的串口handle(C板上一般为USART1,丝印为USART2,4pin) + */ +void VisionInit(UART_HandleTypeDef* handle); + +/** + * @brief 发送视觉视觉 + * + * @param send 视觉需要的数据 + */ +void VisionSend(Vision_Send_s *send); #endif // !MASTER_PROCESS_H \ No newline at end of file diff --git a/modules/master_machine/seasky_protocol.c b/modules/master_machine/seasky_protocol.c new file mode 100644 index 0000000..9b43c44 --- /dev/null +++ b/modules/master_machine/seasky_protocol.c @@ -0,0 +1,142 @@ +/** + * @file seasky_protocol.c + * @author Liu Wei + * @author modified by Neozng + * @brief 湖南大学RoBoMatster串口通信协议 + * @version 0.1 + * @date 2022-11-03 + * + * @copyright Copyright (c) 2022 + * + */ + +#include "seasky_protocol.h" +#include "crc8.h" +#include "crc16.h" + +/*获取CRC8校验码*/ +uint8_t Get_CRC8_Check(uint8_t *pchMessage, uint16_t dwLength) +{ + return crc_8(pchMessage, dwLength); +} +/*检验CRC8数据段*/ +uint8_t CRC8_Check_Sum(uint8_t *pchMessage, uint16_t dwLength) +{ + uint8_t ucExpected = 0; + if ((pchMessage == 0) || (dwLength <= 2)) + return 0; + ucExpected = Get_CRC8_Check(pchMessage, dwLength - 1); + return (ucExpected == pchMessage[dwLength - 1]); +} +/*获取CRC16校验码*/ +uint16_t Get_CRC16_Check(uint8_t *pchMessage, uint32_t dwLength) +{ + return crc_16(pchMessage, dwLength); +} +/*检验CRC16数据段*/ +uint16_t CRC16_Check_Sum(uint8_t *pchMessage, uint32_t dwLength) +{ + uint16_t wExpected = 0; + if ((pchMessage == 0) || (dwLength <= 2)) + { + return 0; + } + wExpected = Get_CRC16_Check(pchMessage, dwLength - 2); + return (((wExpected & 0xff) == pchMessage[dwLength - 2]) && (((wExpected >> 8) & 0xff) == pchMessage[dwLength - 1])); +} +/*检验数据帧头*/ +uint8_t protocol_heade_Check(protocol_rm_struct *pro,uint8_t *rx_buf) +{ + if (rx_buf[0] == PROTOCOL_CMD_ID) + { + pro->header.sof = rx_buf[0]; + if (CRC8_Check_Sum(&rx_buf[0], 4)) + { + pro->header.data_length = (rx_buf[2] << 8) | rx_buf[1]; + pro->header.crc_check = rx_buf[3]; + pro->cmd_id = (rx_buf[5] << 8) | rx_buf[4]; + return 1; + } + } + return 0; +} + +float float_protocol(uint8_t *dat_t) +{ + uint8_t f_data[4]; + f_data[0] = *(dat_t + 0); + f_data[1] = *(dat_t + 1); + f_data[2] = *(dat_t + 2); + f_data[3] = *(dat_t + 3); + return *(float *)f_data; +} +/* + 此函数根据待发送的数据更新数据帧格式以及内容，实现数据的打包操作 + 后续调用通信接口的发送函数发送tx_buf中的对应数据 +*/ +void get_protocol_send_data(uint16_t send_id, //信号id + uint16_t flags_register, // 16位寄存器 + float *tx_data, //待发送的float数据 + uint8_t float_length, // float的数据长度 + uint8_t *tx_buf, //待发送的数据帧 + uint16_t *tx_buf_len) //待发送的数据帧长度 +{ + static uint16_t crc16; + static uint16_t data_len; + + data_len = float_length * 4 + 2; + /*帧头部分*/ + tx_buf[0] = PROTOCOL_CMD_ID; + tx_buf[1] = data_len & 0xff; //低位在前 + tx_buf[2] = (data_len >> 8) & 0xff; //低位在前 + tx_buf[3] = Get_CRC8_Check(&tx_buf[0], 3); //获取CRC8校验位 + + /*数据的信号id*/ + tx_buf[4] = send_id & 0xff; + tx_buf[5] = (send_id >> 8) & 0xff; + + /*建立16位寄存器*/ + tx_buf[6] = flags_register & 0xff; + tx_buf[7] = (flags_register >> 8) & 0xff; + + /*float数据段*/ + for (int i = 0; i < 4 * float_length; i++) + { + tx_buf[i + 8] = ((uint8_t *)(&tx_data[i / 4]))[i % 4]; + } + + /*整包校验*/ + crc16 = Get_CRC16_Check(&tx_buf[0], data_len + 6); + tx_buf[data_len + 6] = crc16 & 0xff; + tx_buf[data_len + 7] = (crc16 >> 8) & 0xff; + + *tx_buf_len = data_len + 8; +} +/* + 此函数用于处理接收数据， + 返回数据内容的id +*/ +uint16_t get_protocol_info(uint8_t *rx_buf, //接收到的原始数据 + uint16_t *flags_register, //接收数据的16位寄存器地址 + float *rx_data) //接收的float数据存储地址 +{ + static protocol_rm_struct pro; + static uint16_t date_length; + if (protocol_heade_Check(&pro, rx_buf)) + { + date_length = OFFSET_BYTE + pro.header.data_length; + while (CRC16_Check_Sum(&rx_buf[0], date_length)) + { + *flags_register = (rx_buf[7] << 8) | rx_buf[6]; + for (int i = 0; i < (pro.header.data_length - 2) / 4; i++) + { + rx_data[i] = float_protocol(&rx_buf[8 + 4 * i]); + } + for (int i = 0; i < date_length; i++) + { + rx_buf[i] = 0; + } + return pro.cmd_id; + } + } +} diff --git a/modules/master_machine/seasky_protocol.h b/modules/master_machine/seasky_protocol.h new file mode 100644 index 0000000..bc3369a --- /dev/null +++ b/modules/master_machine/seasky_protocol.h @@ -0,0 +1,108 @@ +#ifndef __SEASKY_PROTOCOL_H +#define __SEASKY_PROTOCOL_H + +#include +#include + +#define PROTOCOL_CMD_ID 0XA5 + +#define OFFSET_BYTE 8 //出数据段外，其他部分所占字节数 + +typedef enum +{ + NO_FIRE = 0, + AUTO_FIRE = 1, + AUTO_AIM = 2 +} Fire_Mode_e; + +typedef enum +{ + NO_TARGET = 0, + TARGET_CONVERGING = 1, + READY_TO_FIRE = 2 +} Target_State_e; + +typedef enum +{ + NONE = 0, + HERO1 = 1, + ENGINEER2 = 2, + INFANTRY3 = 3, + INFANTRY4 = 4, + INFANTRY5 = 5, + OUTPOST = 6, + SENTRY = 7, + BASE = 8 +} Target_Type_e; + +typedef struct +{ + Fire_Mode_e fire_mode; + Target_State_e target_state; + Target_Type_e target_type; + + float yaw; + float pitch; +} Vision_Recv_s; + +typedef enum +{ + BLUE = 0, + RED = 1 +} Enemy_Color_e; + +typedef enum +{ + MODE_AIM = 0, + MODE_SMALL_BUFF = 1, + MODE_BIG_BUFF = 2 +} Work_Mode_e; + +typedef enum +{ + BIG_AMU_10 = 10, + SMALL_AMU_15 = 15, + BIG_AMU_16 = 16, + SMALL_AMU_18 = 18, + SMALL_AMU_30 = 30, +} Bullet_Speed_e; + +typedef struct +{ + Enemy_Color_e enemy_color; + Work_Mode_e work_mode; + Bullet_Speed_e bullet_speed; + + float yaw; + float pitch; + float roll; + + // uint32_t time_stamp; +} Vision_Send_s; + +typedef struct +{ + struct + { + uint8_t sof; + uint16_t data_length; + uint8_t crc_check; //帧头CRC校验 + } header; //数据帧头 + uint16_t cmd_id; //数据ID + uint16_t frame_tail; //帧尾CRC校验 +} protocol_rm_struct; + +/*更新发送数据帧，并计算发送数据帧长度*/ +void get_protocol_send_data(uint16_t send_id, //信号id + uint16_t flags_register, // 16位寄存器 + float *tx_data, //待发送的float数据 + uint8_t float_length, // float的数据长度 + uint8_t *tx_buf, //待发送的数据帧 + uint16_t *tx_buf_len); //待发送的数据帧长度 + +/*接收数据处理*/ +uint16_t get_protocol_info(uint8_t *rx_buf, //接收到的原始数据 + uint16_t *flags_register, //接收数据的16位寄存器地址 + float *rx_data); //接收的float数据存储地址 + +#endif