add vision protocol
This commit is contained in:
NeoZng
parent
25501650ce
commit
95607668e1
|
|
@ -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"
|
||||
}
|
||||
}
|
||||
6
Makefile
6
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 = \
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -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
|
||||
|
|
@ -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];
|
||||
}
|
||||
|
|
@ -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
|
||||
|
|
@ -51,9 +51,6 @@ float Sqrt(float x)
|
|||
return y;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
/**
|
||||
* @brief 斜波函数计算，根据输入的值进行叠加， 输入单位为 /s 即一秒后增加输入的值
|
||||
* @author RM
|
||||
|
|
|
|||
|
|
@ -1 +1,54 @@
|
|||
/**
|
||||
* @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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,108 @@
|
|||
#ifndef __SEASKY_PROTOCOL_H
|
||||
#define __SEASKY_PROTOCOL_H
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdint.h>
|
||||
|
||||
#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
|
||||
Loading…
Reference in New Issue