HFUT_Firmament_EC
/
sentry_gimbal_hzz
11
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

add vision protocol

This commit is contained in:
NeoZng 2022-11-03 20:38:55 +08:00
parent 25501650ce
commit 95607668e1
12 changed files with 537 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
.vscode/settings.json vendored
View File

@ -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
View File

@ -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 = \

101
modules/algorithm/crc16.c Normal file
View File

@ -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;
}

14
modules/algorithm/crc16.h Normal file
View File

@ -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

61
modules/algorithm/crc8.c Normal file
View File

@ -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];
}

11
modules/algorithm/crc8.h Normal file
View File

@ -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

2
modules/algorithm/kalman_filter.h
View File

@ -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;

39
modules/algorithm/user_lib.c
View File

@ -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)

55
modules/master_machine/master_process.c
View File

@ -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);
}

19
modules/master_machine/master_process.h
View File

@ -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

142
modules/master_machine/seasky_protocol.c Normal file
View File

@ -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;
}
}
}

108
modules/master_machine/seasky_protocol.h Normal file
View File

@ -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