robofish/RMUL2025
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

添加视觉通讯

Browse Source
This commit is contained in:
RB 2025-03-23 04:09:42 +08:00
parent ebe3c2243d
commit e52785886f
8 changed files with 9407 additions and 9287 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
MDK-ARM/DevC/DevC.axf
View File

Binary file not shown.

18427
MDK-ARM/DevC/DevC.hex
View File

File diff suppressed because it is too large Load Diff

114
Middlewares/Third_Party/Protocol/protocol.h vendored
View File

@ -7,13 +7,15 @@
#include <stdint.h>
#ifdef __cplusplus
extern "C" {
extern "C"
{
#endif
#define AI_NOTICE_AUTOAIM (1 << 0)
#define AI_NOTICE_HITBUFF (1 << 1)
#define AI_NOTICE_AUTOMATIC (1 << 2)
#define AI_NOTICE_FIRE (1 << 3)
#define AI_NOTICE_SEARCH_ARMOR (1 << 4)
#define AI_ID_MCU (0xC4)
#define AI_ID_REF (0xA8)
@ -21,66 +23,70 @@ extern "C" {
#define AI_TEAM_RED (0x01)
#define AI_TEAM_BLUE (0x02)
typedef uint8_t Protocol_ID_t;
typedef uint8_t Protocol_ID_t;
/* 电控 -> 视觉 MCU数据结构体*/
typedef struct __attribute__((packed)) {
struct __attribute__((packed)) {
float q0;
float q1;
float q2;
float q3;
} quat; /* 四元数 */
/* 电控 -> 视觉 MCU数据结构体*/
typedef struct __attribute__((packed))
{
struct __attribute__((packed))
{
float q0;
float q1;
float q2;
float q3;
} quat; /* 四元数 */
struct __attribute__((packed))
{
float yaw;
float pit;
float rol;
} gimbal; /* 欧拉角 */
uint8_t notice; /* 控制命令 */
} Protocol_UpDataMCU_t;
uint8_t notice; /* 控制命令 */
/* 电控 -> 视觉 裁判系统数据结构体*/
typedef struct __attribute__((packed))
{
uint16_t team; /* 本身队伍 */
uint16_t time; /* 比赛开始时间 */
} Protocol_UpDataReferee_t;
float ball_speed; /* 子弹初速度 */
/* 视觉 -> 电控 数据结构体*/
typedef struct __attribute__((packed))
{
struct __attribute__((packed))
{
float yaw; /* 偏航角（Yaw angle） */
float pit; /* 俯仰角（Pitch angle） */
float rol; /* 翻滚角（Roll angle） */
} gimbal; /* 欧拉角 */
struct __attribute__((packed)) {
float left;
float right;
} distance; /* 左右距离(哨兵) */
struct __attribute__((packed))
{
float vx; /* x轴移动速度 */
float vy; /* y轴移动速度 */
float wz; /* z轴转动速度 */
} chassis_move_vec; /* 底盘移动向量 */
uint8_t notice; /* 控制命令 */
} Protocol_DownData_t;
float chassis_speed; /* 底盘速度(哨兵) */
} Protocol_UpDataMCU_t;
typedef struct __attribute__((packed))
{
Protocol_UpDataMCU_t data;
uint16_t crc16;
} Protocol_UpPackageMCU_t;
/* 电控 -> 视觉 裁判系统数据结构体*/
typedef struct __attribute__((packed)) {
uint16_t team; /* 本身队伍 */
uint16_t time; /* 比赛开始时间 */
} Protocol_UpDataReferee_t;
typedef struct __attribute__((packed))
{
Protocol_UpDataReferee_t data;
uint16_t crc16;
} Protocol_UpPackageReferee_t;
/* 视觉 -> 电控 数据结构体*/
typedef struct __attribute__((packed)) {
struct __attribute__((packed)) {
float yaw; /* 偏航角（Yaw angle） */
float pit; /* 俯仰角（Pitch angle） */
float rol; /* 翻滚角（Roll angle） */
} gimbal; /* 欧拉角 */
uint8_t notice; /* 控制命令 */
struct __attribute__((packed)) {
float vx; /* x轴移动速度 */
float vy; /* y轴移动速度 */
float wz; /* z轴转动速度 */
} chassis_move_vec; /* 底盘移动向量 */
} Protocol_DownData_t;
typedef struct __attribute__((packed)) {
Protocol_UpDataMCU_t data;
uint16_t crc16;
} Protocol_UpPackageMCU_t;
typedef struct __attribute__((packed)) {
Protocol_UpDataReferee_t data;
uint16_t crc16;
} Protocol_UpPackageReferee_t;
typedef struct __attribute__((packed)) {
Protocol_DownData_t data;
uint16_t crc16;
} Protocol_DownPackage_t;
typedef struct __attribute__((packed))
{
Protocol_DownData_t data;
uint16_t crc16;
} Protocol_DownPackage_t;
#ifdef __cplusplus
}

19
User/component/user_math.c
View File

@ -27,6 +27,10 @@ inline float AbsClip(float in, float limit) {
return (in < -limit) ? -limit : ((in > limit) ? limit : in);
}
float fAbs(float in){
return (in > 0) ? in : -in;
}
inline void Clip(float *origin, float min, float max) {
if (*origin > max) *origin = max;
if (*origin < min) *origin = min;
@ -103,7 +107,6 @@ inline float CalculateRpm(float bullet_speed, float fric_radius, bool is17mm) {
if (is17mm) {
if (bullet_speed == 15.0f) return 4670.f;
if (bullet_speed == 18.0f) return 5200.f;
if (bullet_speed == 25.0f) return 6600.f;
if (bullet_speed == 30.0f) return 7350.f;
} else {
if (bullet_speed == 10.0f) return 4450.f;
@ -113,3 +116,17 @@ inline float CalculateRpm(float bullet_speed, float fric_radius, bool is17mm) {
/* 不为裁判系统设定值时,计算转速 */
return 60.0f * (float)bullet_speed / (M_2PI * fric_radius);
}
/**
* @brief 断言失败处理
*
* @param file 文件名
* @param line 行号
*/
void VerifyFailed(const char *file, uint32_t line) {
UNUSED(file);
UNUSED(line);
while (1) {
__NOP();
}
}

52
User/component/user_math.h
View File

@ -51,6 +51,8 @@ float InvSqrt(float x);
float AbsClip(float in, float limit);
float fAbs(float in);
void Clip(float *origin, float min, float max);
float Sign(float in);
@ -104,3 +106,53 @@ float CalculateRpm(float bullet_speed, float fric_radius, bool is17mm);
#ifdef __cplusplus
}
#endif
#ifdef DEBUG
/**
* @brief 如果表达式的值为假则运行处理函数
*
*/
#define ASSERT(expr) \
do { \
if (!(expr)) { \
VerifyFailed(__FILE__, __LINE__); \
} \
} while (0)
#else
/**
* @brief 未定DEBUG，表达式不会运行，断言被忽略
*
*/
#define ASSERT(expr) ((void)(0))
#endif
#ifdef DEBUG
/**
* @brief 如果表达式的值为假则运行处理函数
*
*/
#define VERIFY(expr) \
do { \
if (!(expr)) { \
VerifyFailed(__FILE__, __LINE__); \
} \
} while (0)
#else
/**
* @brief 表达式会运行，忽略表达式结果
*
*/
#define VERIFY(expr) ((void)(expr))
#endif
/**
* @brief 断言失败处理
*
* @param file 文件名
* @param line 行号
*/
void VerifyFailed(const char *file, uint32_t line);

57
User/device/ai.c
View File

@ -1,5 +1,5 @@
/*
AI
AI
*/
/* Includes ----------------------------------------------------------------- */
@ -12,31 +12,45 @@
#include "component\crc16.h"
#include "component\crc8.h"
#include "component\user_math.h"
#include "component\filter.h"
/* Private define ----------------------------------------------------------- */
#define AI_LEN_RX_BUFF (sizeof(Protocol_DownPackage_t))
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
static volatile uint32_t drop_message = 0;
static osThreadId_t thread_alert;
static uint8_t rxbuf[AI_LEN_RX_BUFF];
static bool inited = false;
static osThreadId_t thread_alert;
/* Private function -------------------------------------------------------- */
static void Ai_RxCpltCallback(void) {
osThreadFlagsSet(thread_alert, SIGNAL_AI_RAW_REDY);
}
static void Ai_IdleLineCallback(void) {
osThreadFlagsSet(thread_alert, SIGNAL_AI_RAW_REDY);
}
/* Exported functions ------------------------------------------------------- */
int8_t AI_Init(AI_t *ai) {
if (ai == NULL) return DEVICE_ERR_NULL;
UNUSED(ai);
ASSERT(ai);
if (inited) return DEVICE_ERR_INITED;
if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;
VERIFY((thread_alert = osThreadGetId()) != NULL);
BSP_UART_RegisterCallback(BSP_UART_AI, BSP_UART_RX_CPLT_CB,
Ai_RxCpltCallback);
BSP_UART_RegisterCallback(BSP_UART_AI, BSP_UART_IDLE_LINE_CB,
Ai_IdleLineCallback);
inited = true;
return 0;
}
@ -48,9 +62,9 @@ int8_t AI_Restart(void) {
}
int8_t AI_StartReceiving(AI_t *ai) {
if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_AI),
(uint8_t *)&(ai->form_host),
sizeof(ai->form_host)) == HAL_OK)
UNUSED(ai);
if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_AI), rxbuf,
AI_LEN_RX_BUFF) == HAL_OK)
return DEVICE_OK;
return DEVICE_ERR;
}
@ -60,17 +74,12 @@ bool AI_WaitDmaCplt(void) {
SIGNAL_AI_RAW_REDY);
}
int8_t AI_ParseHost(AI_t *ai, CMD_Host_t *cmd_host) {
(void)cmd_host;
if (!CRC16_Verify((const uint8_t *)&(ai->form_host), sizeof(ai->form_host)))
int8_t AI_ParseHost(AI_t *ai) {
if (!CRC16_Verify((const uint8_t *)&(rxbuf), sizeof(ai->from_host)))
goto error;
cmd_host->gimbal_delta.pit = ai->form_host.data.gimbal.pit;
cmd_host->gimbal_delta.yaw = ai->form_host.data.gimbal.yaw;
cmd_host->gimbal_delta.rol = ai->form_host.data.gimbal.rol;
cmd_host->fire = (ai->form_host.data.notice & AI_NOTICE_FIRE);
cmd_host->chassis_move_vec.vx = ai->form_host.data.chassis_move_vec.vx;
cmd_host->chassis_move_vec.vy = ai->form_host.data.chassis_move_vec.vy;
cmd_host->chassis_move_vec.wz = ai->form_host.data.chassis_move_vec.wz;
ai->ai_online = true;
memcpy(&(ai->from_host), rxbuf, sizeof(ai->from_host));
memset(rxbuf, 0, AI_LEN_RX_BUFF);
return DEVICE_OK;
error:
@ -78,11 +87,20 @@ error:
return DEVICE_ERR;
}
void AI_PackCmd(AI_t *ai, CMD_Host_t *cmd_host) {
cmd_host->gimbal_delta.yaw = ai->from_host.data.gimbal.yaw;
cmd_host->gimbal_delta.pit = ai->from_host.data.gimbal.pit;
cmd_host->fire = (ai->from_host.data.notice & AI_NOTICE_FIRE);
cmd_host->chassis_move_vec.vx = ai->from_host.data.chassis_move_vec.vx;
cmd_host->chassis_move_vec.vy = ai->from_host.data.chassis_move_vec.vy;
cmd_host->chassis_move_vec.wz = ai->from_host.data.chassis_move_vec.wz;
}
int8_t AI_HandleOffline(AI_t *ai, CMD_Host_t *cmd_host) {
if (ai == NULL) return DEVICE_ERR_NULL;
if (cmd_host == NULL) return DEVICE_ERR_NULL;
memset(&(ai->form_host), 0, sizeof(ai->form_host));
ai->ai_online = false;
memset(&(ai->from_host), 0, sizeof(ai->from_host));
memset(cmd_host, 0, sizeof(*cmd_host));
return 0;
}
@ -131,3 +149,4 @@ int8_t AI_StartSend(AI_t *ai, bool ref_update) {
return DEVICE_ERR;
}
}

10
User/device/ai.h
View File

@ -16,6 +16,7 @@ extern "C" {
#include "component\ahrs.h"
#include "component\cmd.h"
#include "component\user_math.h"
#include "component\filter.h"
#include "device\device.h"
#include "device\referee.h"
#include "protocol.h"
@ -37,7 +38,7 @@ typedef struct __packed {
typedef struct __packed {
osThreadId_t thread_alert;
Protocol_DownPackage_t form_host;
Protocol_DownPackage_t from_host;
struct {
AI_UpPackageReferee_t ref;
@ -45,6 +46,7 @@ typedef struct __packed {
} to_host;
CMD_AI_Status_t status;
bool ai_online;
} AI_t;
/* Exported functions prototypes -------------------------------------------- */
@ -53,11 +55,15 @@ int8_t AI_Restart(void);
int8_t AI_StartReceiving(AI_t *ai);
bool AI_WaitDmaCplt(void);
int8_t AI_ParseHost(AI_t *ai, CMD_Host_t *cmd_host);
int8_t AI_ParseHost(AI_t *ai);
int8_t AI_HandleOffline(AI_t *ai, CMD_Host_t *cmd_host);
int8_t AI_PackMCU(AI_t *ai, const AHRS_Quaternion_t *quat);
int8_t AI_PackRef(AI_t *ai, const Referee_ForAI_t *ref);
int8_t AI_StartSend(AI_t *ai, bool option);
void AI_PackCmd(AI_t *ai, CMD_Host_t *cmd_host);
#ifdef __cplusplus
}
#endif

15
User/task/ai.c
View File

@ -42,6 +42,7 @@ void Task_Ai(void *argument) {
AI_Init(&ai);
uint32_t tick = osKernelGetTickCount();
uint32_t last_online_tick = tick;
while (1) {
#ifdef DEBUG
task_runtime.stack_water_mark.ai = osThreadGetStackSpace(osThreadGetId());
@ -51,17 +52,23 @@ void Task_Ai(void *argument) {
AI_StartReceiving(&ai);
if (AI_WaitDmaCplt()) {
AI_ParseHost(&ai, &cmd_host);
AI_ParseHost(&ai);
last_online_tick = tick;
} else {
AI_HandleOffline(&ai, &cmd_host);
if (tick - last_online_tick > 300) AI_HandleOffline(&ai,&cmd_host);
}
if (ai.status != AI_STATUS_STOP && ai.ai_online){
AI_PackCmd(&ai, &cmd_host);
osMessageQueueReset(task_runtime.msgq.cmd.raw.host);
osMessageQueuePut(task_runtime.msgq.cmd.raw.host, &(cmd_host), 0, 0);
}
osMessageQueueReset(task_runtime.msgq.cmd.raw.host);
osMessageQueuePut(task_runtime.msgq.cmd.raw.host, &(cmd_host), 0, 0);
osMessageQueueGet(task_runtime.msgq.ai.quat, &(quat), NULL, 0);
osMessageQueueGet(task_runtime.msgq.cmd.ai, &(ai.status), NULL, 0);
bool ref_update = (osMessageQueueGet(task_runtime.msgq.referee.ai,
&(referee_ai), NULL, 0) == osOK);
AI_PackMCU(&ai, &quat);
if (ref_update) AI_PackRef(&ai, &(referee_ai));