add vision protocol
This commit is contained in:
NeoZng
parent
25501650ce
commit
95607668e1
|
|
@ -39,6 +39,8 @@
|
||||||
"typeinfo": "c",
|
"typeinfo": "c",
|
||||||
"chrono": "c",
|
"chrono": "c",
|
||||||
"complex": "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/motor/motor_task.c \
|
||||||
modules/referee/referee.c \
|
modules/referee/referee.c \
|
||||||
modules/remote/remote_control.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
|
||||||
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
|
||||||
|
|
@ -92,7 +92,7 @@ typedef struct kf_t
|
||||||
void (*User_Func4_f)(struct kf_t *kf);
|
void (*User_Func4_f)(struct kf_t *kf);
|
||||||
void (*User_Func5_f)(struct kf_t *kf);
|
void (*User_Func5_f)(struct kf_t *kf);
|
||||||
void (*User_Func6_f)(struct kf_t *kf);
|
void (*User_Func6_f)(struct kf_t *kf);
|
||||||
|
|
||||||
// 矩阵存储空间指针
|
// 矩阵存储空间指针
|
||||||
float *xhat_data, *xhatminus_data;
|
float *xhat_data, *xhatminus_data;
|
||||||
float *u_data;
|
float *u_data;
|
||||||
|
|
|
||||||
|
|
@ -1,15 +1,15 @@
|
||||||
/**
|
/**
|
||||||
******************************************************************************
|
******************************************************************************
|
||||||
* @file user_lib.c
|
* @file user_lib.c
|
||||||
* @author Wang Hongxi
|
* @author Wang Hongxi
|
||||||
* @version V1.0.0
|
* @version V1.0.0
|
||||||
* @date 2021/2/18
|
* @date 2021/2/18
|
||||||
* @brief
|
* @brief
|
||||||
******************************************************************************
|
******************************************************************************
|
||||||
* @attention
|
* @attention
|
||||||
*
|
*
|
||||||
******************************************************************************
|
******************************************************************************
|
||||||
*/
|
*/
|
||||||
#include "stdlib.h"
|
#include "stdlib.h"
|
||||||
#include "string.h"
|
#include "string.h"
|
||||||
#include "user_lib.h"
|
#include "user_lib.h"
|
||||||
|
|
@ -51,16 +51,13 @@ float Sqrt(float x)
|
||||||
return y;
|
return y;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* @brief 斜波函数计算，根据输入的值进行叠加， 输入单位为 /s 即一秒后增加输入的值
|
* @brief 斜波函数计算，根据输入的值进行叠加， 输入单位为 /s 即一秒后增加输入的值
|
||||||
* @author RM
|
* @author RM
|
||||||
* @param[in] 斜波函数结构体
|
* @param[in] 斜波函数结构体
|
||||||
* @param[in] 输入值
|
* @param[in] 输入值
|
||||||
* @retval 返回空
|
* @retval 返回空
|
||||||
*/
|
*/
|
||||||
float ramp_calc(ramp_function_source_t *ramp_source_type, float input)
|
float ramp_calc(ramp_function_source_t *ramp_source_type, float input)
|
||||||
{
|
{
|
||||||
ramp_source_type->input = input;
|
ramp_source_type->input = input;
|
||||||
|
|
@ -113,7 +110,7 @@ float float_deadband(float Value, float minValue, float maxValue)
|
||||||
return Value;
|
return Value;
|
||||||
}
|
}
|
||||||
|
|
||||||
//int26死区
|
// int26死区
|
||||||
int16_t int16_deadline(int16_t Value, int16_t minValue, int16_t maxValue)
|
int16_t int16_deadline(int16_t Value, int16_t minValue, int16_t maxValue)
|
||||||
{
|
{
|
||||||
if (Value < maxValue && Value > minValue)
|
if (Value < maxValue && Value > minValue)
|
||||||
|
|
|
||||||
|
|
@ -1 +1,54 @@
|
||||||
#include "Master_process.h"
|
/**
|
||||||
|
* @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
|
#define MASTER_PROCESS_H
|
||||||
|
|
||||||
#include "bsp_usart.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
|
#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