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

Compare commits

base: robofish:哨兵
Branches Tags
robofish:main
robofish:原版
robofish:auto_sentry
robofish:哨兵
robofish:哨兵底盘
..
compare: robofish:main
Branches Tags
robofish:原版
robofish:auto_sentry
robofish:哨兵
robofish:main
robofish:哨兵底盘

2 Commits
哨兵 ... main

Author SHA1 Message Date
Robofish
b84b648d54 正常的英雄 2025-01-17 16:18:34 +08:00
Robofish
9f67e5e851 英雄版本 2025-01-15 14:14:41 +08:00
28 changed files with 9599 additions and 9902 deletions
Show Stats Expand all files Collapse all files

6
.vscode/settings.json vendored
View File

@ -1,6 +0,0 @@
{
"files.associations": {
"user_math.h": "c",
"shoot.h": "c"
}
}

2
Core/Src/main.c
View File

@ -74,7 +74,7 @@ void MX_FREERTOS_Init(void);
* @retval int * @retval int
*/ */
int main(void) int main(void)
{ {
/* USER CODE BEGIN 1 */ /* USER CODE BEGIN 1 */
/* USER CODE END 1 */ /* USER CODE END 1 */

29
MDK-ARM/DevC.uvoptx
View File

@ -412,31 +412,6 @@
<WinNumber>1</WinNumber> <WinNumber>1</WinNumber>
<ItemText>cmd_host</ItemText> <ItemText>cmd_host</ItemText>
</Ww> </Ww>
<Ww>
<count>11</count>
<WinNumber>1</WinNumber>
<ItemText>gimbal_ahrs</ItemText>
</Ww>
<Ww>
<count>12</count>
<WinNumber>1</WinNumber>
<ItemText>eulr_to_send</ItemText>
</Ww>
<Ww>
<count>13</count>
<WinNumber>1</WinNumber>
<ItemText>up_data</ItemText>
</Ww>
<Ww>
<count>14</count>
<WinNumber>1</WinNumber>
<ItemText>AHRS_Eulr_t</ItemText>
</Ww>
<Ww>
<count>15</count>
<WinNumber>1</WinNumber>
<ItemText>gimbal_encoder</ItemText>
</Ww>
</WatchWindow1> </WatchWindow1>
<Tracepoint> <Tracepoint>
<THDelay>0</THDelay> <THDelay>0</THDelay>
@ -1700,7 +1675,7 @@
<Group> <Group>
<GroupName>User/device</GroupName> <GroupName>User/device</GroupName>
<tvExp>1</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel> <cbSel>0</cbSel>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
@ -1780,7 +1755,7 @@
<Group> <Group>
<GroupName>User/module</GroupName> <GroupName>User/module</GroupName>
<tvExp>1</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
<cbSel>0</cbSel> <cbSel>0</cbSel>
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>

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

Binary file not shown.

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

File diff suppressed because it is too large Load Diff

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

@ -10,15 +10,12 @@
extern "C" { extern "C" {
#endif #endif
#define AI_NOTICE_FIRE (1 << 0) #define AI_NOTICE_AUTOAIM (1 << 0)
#define AI_NOTICE_TRACKING (1 << 1) #define AI_NOTICE_HITBUFF (1 << 1)
#define AI_NOTICE_AUTOMATIC (1 << 2)
#define AI_NOTICE_AUTOAIM (1 << 2) #define AI_NOTICE_FIRE (1 << 3)
#define AI_NOTICE_HITBUFF (1 << 3)
#define AI_NOTICE_AUTOMATIC (1 << 4)
#define AI_ID_MCU (0xC4) #define AI_ID_MCU (0xC4)
#define AI_ID_MCU2 (0xD4)
#define AI_ID_REF (0xA8) #define AI_ID_REF (0xA8)
#define AI_TEAM_RED (0x01) #define AI_TEAM_RED (0x01)
@ -36,15 +33,16 @@ typedef struct __attribute__((packed)) {
float q3; float q3;
} quat; /* 四元数 */ } quat; /* 四元数 */
// float chassis_speed; /* 底盘速度(哨兵) */
struct __attribute__((packed)) {
float yaw; /* 偏航角（Yaw angle） */
float pit; /* 俯仰角（Pitch angle） */
float rol; /* 翻滚角（Roll angle） */
} gimbal; /* 欧拉角 */
uint8_t notice; /* 控制命令 */ uint8_t notice; /* 控制命令 */
float ball_speed; /* 子弹初速度 */
struct __attribute__((packed)) {
float left;
float right;
} distance; /* 左右距离(哨兵) */
float chassis_speed; /* 底盘速度(哨兵) */
} Protocol_UpDataMCU_t; } Protocol_UpDataMCU_t;
/* 电控 -> 视觉 裁判系统数据结构体*/ /* 电控 -> 视觉 裁判系统数据结构体*/
@ -55,21 +53,19 @@ typedef struct __attribute__((packed)) {
/* 视觉 -> 电控 数据结构体*/ /* 视觉 -> 电控 数据结构体*/
typedef struct __attribute__((packed)) { typedef struct __attribute__((packed)) {
struct __attribute__((packed)) { struct __attribute__((packed)) {
float yaw; /* 偏航角（Yaw angle） */ float yaw; /* 偏航角（Yaw angle） */
float pit; /* 俯仰角（Pitch angle） */ float pit; /* 俯仰角（Pitch angle） */
float rol; /* 翻滚角（Roll angle） */ float rol; /* 翻滚角（Roll angle） */
} gimbal; /* 欧拉角 */ } gimbal; /* 欧拉角 */
uint8_t notice; /* 控制命令 */
struct __attribute__((packed)) { struct __attribute__((packed)) {
float vx; /* x轴移动速度 */ float vx; /* x轴移动速度 */
float vy; /* y轴移动速度 */ float vy; /* y轴移动速度 */
float wz; /* z轴转动速度 */ float wz; /* z轴转动速度 */
} chassis_move_vec; /* 底盘移动向量 */ } chassis_move_vec; /* 底盘移动向量 */
uint8_t notice; /* 控制命令 */
} Protocol_DownData_t; } Protocol_DownData_t;
typedef struct __attribute__((packed)) { typedef struct __attribute__((packed)) {

4
User/bsp/mm.h
View File

@ -13,10 +13,6 @@ extern "C" {
/* Exported types ----------------------------------------------------------- */ /* Exported types ----------------------------------------------------------- */
/* Exported functions prototypes -------------------------------------------- */ /* Exported functions prototypes -------------------------------------------- */
void *BSP_Malloc(size_t size); void *BSP_Malloc(size_t size);
/**
* @brief
* @param pv
*/
void BSP_Free(void *pv); void BSP_Free(void *pv);
#ifdef __cplusplus #ifdef __cplusplus

33
User/component/ballistics.c
View File

@ -4,40 +4,9 @@
#include "ballistics.h" #include "ballistics.h"
/**
* @brief
* @param b
*/
void Ballistics_Init(Ballistics_t *b) { (void)b; } void Ballistics_Init(Ballistics_t *b) { (void)b; }
void Ballistics_Apply(Ballistics_t *b, float bullet_speed) {
/**
* @brief
* @param b
* @param bullet_speed
*/
void Ballistics_Apply(Ballistics_t *b, float bullet_speed)
{
(void)b; (void)b;
(void)bullet_speed; (void)bullet_speed;
} }
/**
* @brief
* @param bullet_speed 弹丸速度
* @param distance 距离
* @return 高度补偿值
*/
float Simpal_Ballistics_Apply(float bullet_speed, float distance)
{
return distance /1000 / bullet_speed;
}
/**
* @brief
* @param b
*/
void Ballistics_Reset(Ballistics_t *b) { (void)b; } void Ballistics_Reset(Ballistics_t *b) { (void)b; }

8
User/component/ballistics.h
View File

@ -16,14 +16,6 @@ typedef struct {
void Ballistics_Init(Ballistics_t *b); void Ballistics_Init(Ballistics_t *b);
void Ballistics_Apply(Ballistics_t *b, float bullet_speed); void Ballistics_Apply(Ballistics_t *b, float bullet_speed);
/**
* @brief
* @param bullet_speed 弹丸速度
* @param distance 距离
* @return 高度补偿值
*/
float Simpal_Ballistics_Apply(float bullet_speed, float distance);
void Ballistics_Reset(Ballistics_t *b); void Ballistics_Reset(Ballistics_t *b);
#ifdef __cplusplus #ifdef __cplusplus

50
User/component/cmd.c
View File

@ -158,8 +158,8 @@ static void CMD_PcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
CMD_RefereeAdd(&(cmd->referee), CMD_UI_AUTO_AIM_STOP); CMD_RefereeAdd(&(cmd->referee), CMD_UI_AUTO_AIM_STOP);
} else { } else {
/* ai切换至自瞄模式，启用host控制 */ /* ai切换至自瞄模式，启用host控制 */
cmd->host_overwrite = true;
cmd->ai_status = AI_STATUS_AUTOAIM; cmd->ai_status = AI_STATUS_AUTOAIM;
cmd->host_overwrite = true;
CMD_RefereeAdd(&(cmd->referee), CMD_UI_AUTO_AIM_START); CMD_RefereeAdd(&(cmd->referee), CMD_UI_AUTO_AIM_START);
} }
} else { } else {
@ -190,19 +190,27 @@ static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
/* 左拨杆相应行为选择和解析 */ /* 左拨杆相应行为选择和解析 */
case CMD_SW_UP: case CMD_SW_UP:
cmd->chassis.mode = CHASSIS_MODE_BREAK; cmd->chassis.mode = CHASSIS_MODE_BREAK;
// cmd->host_overwrite = false;
// cmd->shoot.ai_fire = false;
break; break;
case CMD_SW_MID: case CMD_SW_MID:
cmd->chassis.mode = CHASSIS_MODE_FOLLOW_GIMBAL; cmd->chassis.mode = CHASSIS_MODE_FOLLOW_GIMBAL;
// cmd->host_overwrite = true;
// cmd->shoot.ai_fire = false;
break; break;
case CMD_SW_DOWN: case CMD_SW_DOWN:
cmd->chassis.mode = CHASSIS_MODE_ROTOR; cmd->chassis.mode = CHASSIS_MODE_ROTOR;
cmd->chassis.mode_rotor = ROTOR_MODE_CW; cmd->chassis.mode_rotor = ROTOR_MODE_CW;
// cmd->host_overwrite = true;
// cmd->shoot.ai_fire = true;
break; break;
case CMD_SW_ERR: case CMD_SW_ERR:
cmd->chassis.mode = CHASSIS_MODE_RELAX; cmd->chassis.mode = CHASSIS_MODE_RELAX;
// cmd->host_overwrite = false;
// cmd->shoot.ai_fire = false;
break; break;
} }
switch (rc->sw_r) { switch (rc->sw_r) {
@ -223,19 +231,36 @@ static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
cmd->shoot.mode = SHOOT_MODE_LOADED; cmd->shoot.mode = SHOOT_MODE_LOADED;
cmd->shoot.fire_mode = FIRE_MODE_SINGLE; cmd->shoot.fire_mode = FIRE_MODE_SINGLE;
cmd->shoot.fire = true; cmd->shoot.fire = true;
break;
/*
case CMD_SW_UP:
cmd->gimbal.mode = GIMBAL_MODE_RELAX;
cmd->shoot.mode = SHOOT_MODE_SAFE;
break; break;
case CMD_SW_MID:
cmd->gimbal.mode = GIMBAL_MODE_RELAX;
cmd->shoot.fire = false;
cmd->shoot.mode = SHOOT_MODE_LOADED;
break;
case CMD_SW_DOWN:
cmd->gimbal.mode = GIMBAL_MODE_RELAX;
cmd->shoot.mode = SHOOT_MODE_LOADED;
cmd->shoot.fire_mode = FIRE_MODE_SINGLE;
cmd->shoot.fire = true;
break;
*/
case CMD_SW_ERR: case CMD_SW_ERR:
cmd->gimbal.mode = GIMBAL_MODE_RELAX; cmd->gimbal.mode = GIMBAL_MODE_RELAX;
cmd->shoot.mode = SHOOT_MODE_RELAX; cmd->shoot.mode = SHOOT_MODE_RELAX;
// cmd->host_overwrite = false;
} }
/* 将操纵杆的对应值转换为底盘的控制向量和云台变化的欧拉角 */ /* 将操纵杆的对应值转换为底盘的控制向量和云台变化的欧拉角 */
if (cmd->ai_status == AI_STATUS_OFFLINE || cmd->host_overwrite == false) { cmd->chassis.ctrl_vec.vx = rc->ch_l_x;
cmd->chassis.ctrl_vec.vx = rc->ch_l_x; cmd->chassis.ctrl_vec.vy = rc->ch_l_y;
cmd->chassis.ctrl_vec.vy = rc->ch_l_y; cmd->gimbal.delta_eulr.yaw = rc->ch_r_x * dt_sec * cmd->param->sens_rc;
cmd->gimbal.delta_eulr.yaw = rc->ch_r_x * dt_sec * cmd->param->sens_rc; cmd->gimbal.delta_eulr.pit = rc->ch_r_y * dt_sec * cmd->param->sens_rc;
cmd->gimbal.delta_eulr.pit = rc->ch_r_y * dt_sec * cmd->param->sens_rc;
}
} }
/** /**
@ -330,17 +355,12 @@ int8_t CMD_ParseHost(const CMD_Host_t *host, CMD_t *cmd, float dt_sec) {
cmd->gimbal.delta_eulr.yaw = host->gimbal_delta.yaw; cmd->gimbal.delta_eulr.yaw = host->gimbal_delta.yaw;
cmd->gimbal.delta_eulr.pit = host->gimbal_delta.pit; cmd->gimbal.delta_eulr.pit = host->gimbal_delta.pit;
cmd->shoot.mode = SHOOT_MODE_LOADED;
if (host->tracking){
cmd->gimbal.mode = GIMBAL_MODE_ABSOLUTE;
}else{
cmd->gimbal.mode = GIMBAL_MODE_SEARCH;
}
/* host射击命令，设置不同的射击频率和弹丸初速度 */ /* host射击命令，设置不同的射击频率和弹丸初速度 */
if (host->fire && cmd->shoot.ai_fire) { if (host->fire) {
cmd->shoot.mode = SHOOT_MODE_LOADED;
cmd->shoot.fire = true; cmd->shoot.fire = true;
} else { } else {
cmd->shoot.fire = true; cmd->shoot.mode = SHOOT_MODE_SAFE;
} }
return 0; return 0;
} }

3
User/component/cmd.h
View File

@ -42,7 +42,6 @@ typedef enum {
typedef enum { typedef enum {
GIMBAL_MODE_RELAX, /* 放松模式，电机不输出。一般情况云台初始化之后的模式 */ GIMBAL_MODE_RELAX, /* 放松模式，电机不输出。一般情况云台初始化之后的模式 */
GIMBAL_MODE_ABSOLUTE, /* 绝对坐标系控制，控制在空间内的绝对姿态 */ GIMBAL_MODE_ABSOLUTE, /* 绝对坐标系控制，控制在空间内的绝对姿态 */
GIMBAL_MODE_SEARCH, /* 搜索模式，通过云台旋转搜索目标 */
GIMBAL_MODE_RELATIVE, /* 相对坐标系控制，控制相对于底盘的姿态 */ GIMBAL_MODE_RELATIVE, /* 相对坐标系控制，控制相对于底盘的姿态 */
} CMD_GimbalMode_t; } CMD_GimbalMode_t;
@ -179,7 +178,6 @@ typedef struct {
/* AI行为状态 */ /* AI行为状态 */
typedef enum { typedef enum {
AI_STATUS_OFFLINE, /* 离线状态 */
AI_STATUS_STOP, /* 停止状态 */ AI_STATUS_STOP, /* 停止状态 */
AI_STATUS_AUTOAIM, /* 自瞄状态 */ AI_STATUS_AUTOAIM, /* 自瞄状态 */
AI_STATUS_HITSWITCH, /* 打符状态 */ AI_STATUS_HITSWITCH, /* 打符状态 */
@ -256,7 +254,6 @@ typedef struct {
float wz; /* z轴转动速度 */ float wz; /* z轴转动速度 */
} chassis_move_vec; /* 底盘移动向量 */ } chassis_move_vec; /* 底盘移动向量 */
bool tracking; /* 是否追踪目标 */
bool fire; /* 开火状态 */ bool fire; /* 开火状态 */
} CMD_Host_t; } CMD_Host_t;

14
User/component/user_math.c
View File

@ -112,17 +112,3 @@ inline float CalculateRpm(float bullet_speed, float fric_radius, bool is17mm) {
/* 不为裁判系统设定值时,计算转速 */ /* 不为裁判系统设定值时,计算转速 */
return 60.0f * (float)bullet_speed / (M_2PI * fric_radius); 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();
}
}

50
User/component/user_math.h
View File

@ -104,53 +104,3 @@ float CalculateRpm(float bullet_speed, float fric_radius, bool is17mm);
#ifdef __cplusplus #ifdef __cplusplus
} }
#endif #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);

84
User/device/ai.c
View File

@ -12,51 +12,34 @@
#include "component\crc16.h" #include "component\crc16.h"
#include "component\crc8.h" #include "component\crc8.h"
#include "component\user_math.h" #include "component\user_math.h"
#include "component\filter.h"
/* Private define ----------------------------------------------------------- */ /* Private define ----------------------------------------------------------- */
#define AI_HOST_MAX_CONTROL_VALUE (0.001f)
#define AI_LEN_RX_BUFF (sizeof(Protocol_DownPackage_t))
/* Private macro ------------------------------------------------------------ */ /* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */ /* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */ /* Private variables -------------------------------------------------------- */
static volatile uint32_t drop_message = 0; static volatile uint32_t drop_message = 0;
static uint8_t rxbuf[AI_LEN_RX_BUFF];
static osThreadId_t thread_alert; static osThreadId_t thread_alert;
static bool inited = false; static bool inited = false;
static LowPassFilter2p_t low_pass_filter;
/* Private function -------------------------------------------------------- */ /* Private function -------------------------------------------------------- */
static void Ai_RxCpltCallback(void) { static void Ai_RxCpltCallback(void) {
osThreadFlagsSet(thread_alert, SIGNAL_AI_RAW_REDY); osThreadFlagsSet(thread_alert, SIGNAL_AI_RAW_REDY);
} }
static void Ai_IdleLineCallback(void) {
osThreadFlagsSet(thread_alert, SIGNAL_AI_RAW_REDY);
}
/* Exported functions ------------------------------------------------------- */ /* Exported functions ------------------------------------------------------- */
int8_t AI_Init(AI_t *ai) { int8_t AI_Init(AI_t *ai) {
UNUSED(ai); if (ai == NULL) return DEVICE_ERR_NULL;
ASSERT(ai);
if (inited) return DEVICE_ERR_INITED; if (inited) return DEVICE_ERR_INITED;
VERIFY((thread_alert = osThreadGetId()) != NULL); if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;
BSP_UART_RegisterCallback(BSP_UART_AI, BSP_UART_RX_CPLT_CB, BSP_UART_RegisterCallback(BSP_UART_AI, BSP_UART_RX_CPLT_CB,
Ai_RxCpltCallback); Ai_RxCpltCallback);
BSP_UART_RegisterCallback(BSP_UART_AI, BSP_UART_IDLE_LINE_CB,
Ai_IdleLineCallback);
LowPassFilter2p_Init(&low_pass_filter, 1000.0f, 5.0f); // 示例参数
inited = true; inited = true;
ai->ai_online = false;
return 0; return 0;
} }
@ -67,9 +50,9 @@ int8_t AI_Restart(void) {
} }
int8_t AI_StartReceiving(AI_t *ai) { int8_t AI_StartReceiving(AI_t *ai) {
UNUSED(ai); if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_AI),
if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_AI), rxbuf, ai->form_host.rx_buffer,
AI_LEN_RX_BUFF) == HAL_OK) sizeof(ai->form_host.rx_buffer)) == HAL_OK)
return DEVICE_OK; return DEVICE_OK;
return DEVICE_ERR; return DEVICE_ERR;
} }
@ -79,19 +62,27 @@ bool AI_WaitDmaCplt(void) {
SIGNAL_AI_RAW_REDY); SIGNAL_AI_RAW_REDY);
} }
int8_t AI_ParseHost(AI_t *ai) { int8_t AI_ParseHost(AI_t *ai, CMD_Host_t *cmd_host) {
if (!CRC16_Verify((const uint8_t *)&(rxbuf), sizeof(ai->form_host))) (void)cmd_host;
goto error;
ai->ai_online = true; ai->ai_online = true;
if (!CRC16_Verify(ai->form_host.rx_buffer, sizeof(ai->form_host.rx_buffer)))
goto error;
memcpy(&(ai->form_host.data), ai->form_host.rx_buffer, sizeof(ai->form_host.data));
// 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;
cmd_host->gimbal_delta.pit = ai->form_host.data.data.gimbal.pit;
cmd_host->gimbal_delta.yaw = ai->form_host.data.data.gimbal.yaw;
cmd_host->gimbal_delta.rol = ai->form_host.data.data.gimbal.rol;
cmd_host->fire = (ai->form_host.data.data.notice & AI_NOTICE_FIRE);
cmd_host->chassis_move_vec.vx = ai->form_host.data.data.chassis_move_vec.vx;
cmd_host->chassis_move_vec.vy = ai->form_host.data.data.chassis_move_vec.vy;
cmd_host->chassis_move_vec.wz = ai->form_host.data.data.chassis_move_vec.wz;
// 对接收到的数据进行滤波处理
for (size_t i = 0; i < sizeof(ai->form_host.data.gimbal); ++i) {
((float*)&ai->form_host.data.gimbal)[i] =
LowPassFilter2p_Apply(&low_pass_filter, ((float*)&ai->form_host.data.gimbal)[i]);
}
memcpy(&(ai->form_host), rxbuf, sizeof(ai->form_host));
memset(rxbuf, 0, AI_LEN_RX_BUFF);
return DEVICE_OK; return DEVICE_OK;
error: error:
@ -99,37 +90,20 @@ error:
return DEVICE_ERR; return DEVICE_ERR;
} }
void AI_PackCmd(AI_t *ai, CMD_Host_t *cmd_host) {
cmd_host->gimbal_delta.yaw = -ai->form_host.data.gimbal.yaw;
cmd_host->gimbal_delta.yaw = cmd_host->gimbal_delta.yaw - 0.00025;
cmd_host->gimbal_delta.pit = -ai->form_host.data.gimbal.pit;
cmd_host->gimbal_delta.pit = cmd_host->gimbal_delta.pit + 0.0005;
Clip(&(cmd_host->gimbal_delta.yaw), -AI_HOST_MAX_CONTROL_VALUE,
AI_HOST_MAX_CONTROL_VALUE);
Clip(&(cmd_host->gimbal_delta.pit), -AI_HOST_MAX_CONTROL_VALUE,
AI_HOST_MAX_CONTROL_VALUE);
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;
}
int8_t AI_HandleOffline(AI_t *ai, CMD_Host_t *cmd_host) { int8_t AI_HandleOffline(AI_t *ai, CMD_Host_t *cmd_host) {
if (ai == NULL) return DEVICE_ERR_NULL; if (ai == NULL) return DEVICE_ERR_NULL;
if (cmd_host == NULL) return DEVICE_ERR_NULL; if (cmd_host == NULL) return DEVICE_ERR_NULL;
ai->ai_online = false; ai->ai_online = false;
memset(&(ai->form_host), 0, sizeof(ai->form_host)); memset(ai->form_host.rx_buffer, 0, sizeof(ai->form_host.rx_buffer));
memset(&(ai->form_host.data), 0, sizeof(ai->form_host.data));
memset(cmd_host, 0, sizeof(*cmd_host)); memset(cmd_host, 0, sizeof(*cmd_host));
return 0; return 0;
} }
int8_t AI_PackMCU(AI_t *ai, const AHRS_Quaternion_t *quat, const AHRS_Eulr_t *gimbal_ai) { int8_t AI_PackMCU(AI_t *ai, const AHRS_Quaternion_t *quat) {
ai->to_host.mcu.id = AI_ID_MCU; ai->to_host.mcu.id = AI_ID_MCU;
memcpy((void *)&(ai->to_host.mcu.package.data.quat), (const void *)quat, memcpy((void *)&(ai->to_host.mcu.package.data.quat), (const void *)quat,
sizeof(*quat)); sizeof(*quat));
memcpy((void *)&(ai->to_host.mcu.package.data.gimbal), (const void *)gimbal_ai,
sizeof(*gimbal_ai));
ai->to_host.mcu.package.data.notice = 0; ai->to_host.mcu.package.data.notice = 0;
if (ai->status == AI_STATUS_AUTOAIM) if (ai->status == AI_STATUS_AUTOAIM)
ai->to_host.mcu.package.data.notice |= AI_NOTICE_AUTOAIM; ai->to_host.mcu.package.data.notice |= AI_NOTICE_AUTOAIM;

13
User/device/ai.h
View File

@ -16,7 +16,6 @@ extern "C" {
#include "component\ahrs.h" #include "component\ahrs.h"
#include "component\cmd.h" #include "component\cmd.h"
#include "component\user_math.h" #include "component\user_math.h"
#include "component\filter.h"
#include "device\device.h" #include "device\device.h"
#include "device\referee.h" #include "device\referee.h"
#include "protocol.h" #include "protocol.h"
@ -37,8 +36,10 @@ typedef struct __packed {
typedef struct __packed { typedef struct __packed {
osThreadId_t thread_alert; osThreadId_t thread_alert;
struct {
Protocol_DownPackage_t form_host; uint8_t rx_buffer[sizeof(Protocol_DownPackage_t)];
Protocol_DownPackage_t data;
}form_host;
struct { struct {
AI_UpPackageReferee_t ref; AI_UpPackageReferee_t ref;
@ -46,6 +47,7 @@ typedef struct __packed {
} to_host; } to_host;
CMD_AI_Status_t status; CMD_AI_Status_t status;
bool ai_online; bool ai_online;
} AI_t; } AI_t;
@ -55,12 +57,11 @@ int8_t AI_Restart(void);
int8_t AI_StartReceiving(AI_t *ai); int8_t AI_StartReceiving(AI_t *ai);
bool AI_WaitDmaCplt(void); bool AI_WaitDmaCplt(void);
int8_t AI_ParseHost(AI_t *ai); int8_t AI_ParseHost(AI_t *ai, CMD_Host_t *cmd_host);
int8_t AI_HandleOffline(AI_t *ai, CMD_Host_t *cmd_host); int8_t AI_HandleOffline(AI_t *ai, CMD_Host_t *cmd_host);
int8_t AI_PackMCU(AI_t *ai, const AHRS_Quaternion_t *quat, const AHRS_Eulr_t *gimbal_ai); 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_PackRef(AI_t *ai, const Referee_ForAI_t *ref);
int8_t AI_StartSend(AI_t *ai, bool option); int8_t AI_StartSend(AI_t *ai, bool option);
void AI_PackCmd(AI_t *ai, CMD_Host_t *cmd_host);
#ifdef __cplusplus #ifdef __cplusplus
} }
#endif #endif

2
User/device/can.h
View File

@ -200,7 +200,7 @@ typedef union {
} CAN_GimbalMotor_t; } CAN_GimbalMotor_t;
typedef union { typedef union {
CAN_MotorFeedback_t as_array[4]; CAN_MotorFeedback_t as_array[3];
struct { struct {
CAN_MotorFeedback_t fric1; CAN_MotorFeedback_t fric1;
CAN_MotorFeedback_t fric2; CAN_MotorFeedback_t fric2;

6
User/device/referee.h
View File

@ -23,9 +23,9 @@ extern "C" {
#define REF_SWITCH_STATUS(ref, stat) ((ref).ref_status = (stat)) #define REF_SWITCH_STATUS(ref, stat) ((ref).ref_status = (stat))
#define CHASSIS_POWER_MAX_WITHOUT_REF 2000.0f /* 裁判系统离线底盘最大功率 */ #define CHASSIS_POWER_MAX_WITHOUT_REF 2000.0f /* 裁判系统离线底盘最大功率 */
#define REF_UI_MAX_GRAPIC_NUM (7) /* 最大图形数量 */ #define REF_UI_MAX_GRAPIC_NUM (7)
#define REF_UI_MAX_STRING_NUM (7) /* 最大字符串数量 */ #define REF_UI_MAX_STRING_NUM (7)
#define REF_UI_MAX_DEL_NUM (3) /* 最大删除数量 */ #define REF_UI_MAX_DEL_NUM (3)
/* Exported types ----------------------------------------------------------- */ /* Exported types ----------------------------------------------------------- */
typedef struct __packed { typedef struct __packed {

127
User/module/config.c
View File

@ -138,12 +138,7 @@ static const Config_RobotParam_t param_default = {
}, },
}, /* pid */ }, /* pid */
.limit_and_imu = { .pitch_travel_rad = 1.05f,
.yaw_travel_rad = -1.0f,
.pitch_travel_rad = 2.0f,
.yaw_imu = false,
.pit_imu = false,
},
.low_pass_cutoff_freq = { .low_pass_cutoff_freq = {
.out = -1.0f, .out = -1.0f,
@ -155,6 +150,7 @@ static const Config_RobotParam_t param_default = {
.pit = true, .pit = true,
}, },
.pit_ctrl_reverse = false,
}, /* gimbal */ }, /* gimbal */
.shoot = { /* 射击模块参数 */ .shoot = { /* 射击模块参数 */
@ -195,7 +191,7 @@ static const Config_RobotParam_t param_default = {
.cover_close_duty = 0.050f, .cover_close_duty = 0.050f,
.model = SHOOT_MODEL_17MM, .model = SHOOT_MODEL_17MM,
.bullet_speed = 30.f, .bullet_speed = 30.f,
.min_shoot_delay = (uint32_t)(1000.0f / 5.0f), .min_shoot_delay = (uint32_t)(1000.0f / 10.0f),
}, /* shoot */ }, /* shoot */
.can = { .can = {
@ -206,7 +202,11 @@ static const Config_RobotParam_t param_default = {
}, /* can */ }, /* can */
}; /* param_default */ }; /* param_default */
#ifdef DEBUG
Config_RobotParam_t param_hero = {
#else
static const Config_RobotParam_t param_hero = { static const Config_RobotParam_t param_hero = {
#endif
.model = ROBOT_MODEL_HERO, .model = ROBOT_MODEL_HERO,
.chassis = { /* 底盘模块参数 */ .chassis = { /* 底盘模块参数 */
@ -240,7 +240,7 @@ static const Config_RobotParam_t param_hero = {
}, },
.reverse = { .reverse = {
.yaw = true, .yaw = false,
}, },
}, /* chassis */ }, /* chassis */
@ -289,12 +289,7 @@ static const Config_RobotParam_t param_hero = {
}, },
}, /* pid */ }, /* pid */
.limit_and_imu = { .pitch_travel_rad = 0.9685447f,
.yaw_travel_rad = -1.0f,
.pitch_travel_rad = 1.0f,
.yaw_imu = true,
.pit_imu = true,
},
.low_pass_cutoff_freq = { .low_pass_cutoff_freq = {
.out = -1.0f, .out = -1.0f,
@ -302,10 +297,11 @@ static const Config_RobotParam_t param_hero = {
}, },
.reverse = { .reverse = {
.yaw = true, .yaw = false,
.pit = true, .pit = false,
}, },
.pit_ctrl_reverse = false,
}, /* gimbal */ }, /* gimbal */
.shoot = { /* 射击模块参数 */ .shoot = { /* 射击模块参数 */
@ -339,8 +335,8 @@ static const Config_RobotParam_t param_hero = {
.trig = -1.0f, .trig = -1.0f,
}, },
}, },
.num_trig_tooth = 6.0f, .num_trig_tooth = 5.0f,
.trig_gear_ratio = 3591.0f / 187.0f, .trig_gear_ratio = 19.0f,
.fric_radius = 0.03f, .fric_radius = 0.03f,
.cover_open_duty = 0.125f, .cover_open_duty = 0.125f,
.cover_close_duty = 0.075f, .cover_close_duty = 0.075f,
@ -403,6 +399,26 @@ static const Config_RobotParam_t param_sentry_chassis = {
.gimbal = { /* 云台模块参数 */ .gimbal = { /* 云台模块参数 */
.pid = { .pid = {
{ {
// /* GIMBAL_PID_YAW_OMEGA_IDX */
// .k = 0.25f,
// .p = 1.0f,
// .i = 1.0f,
// .d = 0.0f,
// .i_limit = 1.0f,
// .out_limit = 1.0f,
// .d_cutoff_freq = -1.0f,
// .range = -1.0f,
// }, {
// /* GIMBAL_PID_YAW_ANGLE_IDX */
// .k = 12.0f,
// .p = 1.0f,
// .i = 0.0f,
// .d = 0.05f,
// .i_limit = 0.0f,
// .out_limit = 10.0f,
// .d_cutoff_freq = -1.0f,
// .range = M_2PI,
/* GIMBAL_PID_YAW_OMEGA_IDX */
.k = 0.24f, .k = 0.24f,
.p = 1.0f, .p = 1.0f,
.i = 0.5f, .i = 0.5f,
@ -422,6 +438,25 @@ static const Config_RobotParam_t param_sentry_chassis = {
.d_cutoff_freq = -1.0f, .d_cutoff_freq = -1.0f,
.range = M_2PI, .range = M_2PI,
}, { }, {
// /* GIMBAL_PID_PIT_OMEGA_IDX */
// .k = 0.35f,
// .p = 1.0f,
// .i = 0.f,
// .d = 0.003f,
// .i_limit = 1.0f,
// .out_limit = 1.0f,
// .d_cutoff_freq = -1.0f,
// .range = -1.0f,
// }, {
// /* GIMBAL_PID_PIT_ANGLE_IDX */
// .k = 15.0f,
// .p = 1.0f,
// .i = 0.0f,
// .d = 0.0f,
// .i_limit = 0.0f,
// .out_limit = 10.0f,
// .d_cutoff_freq = -1.0f,
// .range = M_2PI,
/* GIMBAL_PID_PIT_OMEGA_IDX */ /* GIMBAL_PID_PIT_OMEGA_IDX */
.k = 0.25f, .k = 0.25f,
.p = 1.0f, .p = 1.0f,
@ -444,12 +479,7 @@ static const Config_RobotParam_t param_sentry_chassis = {
}, },
}, /* pid */ }, /* pid */
.limit_and_imu = { .pitch_travel_rad = 0.85f,
.yaw_travel_rad = -1.0f,
.pitch_travel_rad = 1.0f,
.yaw_imu = true,
.pit_imu = true,
},
.low_pass_cutoff_freq = { .low_pass_cutoff_freq = {
.out = -1.0f, .out = -1.0f,
@ -461,6 +491,7 @@ static const Config_RobotParam_t param_sentry_chassis = {
.pit = true, .pit = true,
}, },
.pit_ctrl_reverse = true,
}, /* gimbal */ }, /* gimbal */
.shoot = { /* 射击模块参数 */ .shoot = { /* 射击模块参数 */
@ -558,6 +589,25 @@ static const Config_RobotParam_t param_sentry_gimbal = {
.gimbal = { /* 云台模块参数 */ .gimbal = { /* 云台模块参数 */
.pid = { .pid = {
{ {
// /* GIMBAL_PID_YAW_OMEGA_IDX */
// .k = 0.25f,
// .p = 1.0f,
// .i = 1.0f,
// .d = 0.0f,
// .i_limit = 1.0f,
// .out_limit = 1.0f,
// .d_cutoff_freq = -1.0f,
// .range = -1.0f,
// }, {
// /* GIMBAL_PID_YAW_ANGLE_IDX */
// .k = 12.0f,
// .p = 1.0f,
// .i = 0.0f,
// .d = 0.05f,
// .i_limit = 0.0f,
// .out_limit = 10.0f,
// .d_cutoff_freq = -1.0f,
// .range = M_2PI,
/* GIMBAL_PID_YAW_OMEGA_IDX */ /* GIMBAL_PID_YAW_OMEGA_IDX */
.k = 0.24f, .k = 0.24f,
.p = 1.0f, .p = 1.0f,
@ -578,6 +628,25 @@ static const Config_RobotParam_t param_sentry_gimbal = {
.d_cutoff_freq = -1.0f, .d_cutoff_freq = -1.0f,
.range = M_2PI, .range = M_2PI,
}, { }, {
// /* GIMBAL_PID_PIT_OMEGA_IDX */
// .k = 0.35f,
// .p = 1.0f,
// .i = 0.f,
// .d = 0.003f,
// .i_limit = 1.0f,
// .out_limit = 1.0f,
// .d_cutoff_freq = -1.0f,
// .range = -1.0f,
// }, {
// /* GIMBAL_PID_PIT_ANGLE_IDX */
// .k = 15.0f,
// .p = 1.0f,
// .i = 0.0f,
// .d = 0.0f,
// .i_limit = 0.0f,
// .out_limit = 10.0f,
// .d_cutoff_freq = -1.0f,
// .range = M_2PI,
/* GIMBAL_PID_PIT_OMEGA_IDX */ /* GIMBAL_PID_PIT_OMEGA_IDX */
.k = 0.25f, .k = 0.25f,
.p = 1.0f, .p = 1.0f,
@ -600,12 +669,7 @@ static const Config_RobotParam_t param_sentry_gimbal = {
}, },
}, /* pid */ }, /* pid */
.limit_and_imu = { .pitch_travel_rad = 0.85f,
.yaw_travel_rad = -1.0f,
.pitch_travel_rad = 1.0f,
.yaw_imu = true,
.pit_imu = false,
},
.low_pass_cutoff_freq = { .low_pass_cutoff_freq = {
.out = -1.0f, .out = -1.0f,
@ -617,6 +681,7 @@ static const Config_RobotParam_t param_sentry_gimbal = {
.pit = true, .pit = true,
}, },
.pit_ctrl_reverse = true,
}, /* gimbal */ }, /* gimbal */
.shoot = { /* 射击模块参数 */ .shoot = { /* 射击模块参数 */
@ -657,7 +722,7 @@ static const Config_RobotParam_t param_sentry_gimbal = {
.cover_close_duty = 0.050f, .cover_close_duty = 0.050f,
.model = SHOOT_MODEL_17MM, .model = SHOOT_MODEL_17MM,
.bullet_speed = 25.f, .bullet_speed = 25.f,
.min_shoot_delay = (uint32_t)(1000.0f / 1.0f), .min_shoot_delay = (uint32_t)(1000.0f / 20.0f),
}, /* shoot */ }, /* shoot */
.can = { .can = {

4
User/module/config.h
View File

@ -47,8 +47,8 @@ typedef struct {
} cali; /* 校准 */ } cali; /* 校准 */
AHRS_Eulr_t mech_zero; /* 机械零点 */ AHRS_Eulr_t mech_zero; /* 机械零点 */
float gimbal_pitch_limit; /* 云台pitch轴软件限位最高点 */ float gimbal_limit; /* 云台pitch轴软件限位最高点 */
float gimbal_yaw_limit; /* 云台yaw轴软件限位最高点 */
} Config_t; } Config_t;
/* 机器人参数和对应字符串的映射 */ /* 机器人参数和对应字符串的映射 */

178
User/module/gimbal.c
View File

@ -25,10 +25,6 @@ static int8_t Gimbal_SetMode(Gimbal_t *g, CMD_GimbalMode_t mode) {
if (g == NULL) return -1; if (g == NULL) return -1;
if (mode == g->mode) return GIMBAL_OK; if (mode == g->mode) return GIMBAL_OK;
/* 获取当前反馈值，防止位置突变 */
g->setpoint.eulr.yaw = g->feedback.eulr.encoder.yaw;
g->setpoint.eulr.pit = g->feedback.eulr.encoder.pit;
/* 切换模式后重置PID和滤波器 */ /* 切换模式后重置PID和滤波器 */
for (uint8_t i = 0; i < GIMBAL_PID_NUM; i++) { for (uint8_t i = 0; i < GIMBAL_PID_NUM; i++) {
PID_Reset(g->pid + i); PID_Reset(g->pid + i);
@ -40,15 +36,9 @@ static int8_t Gimbal_SetMode(Gimbal_t *g, CMD_GimbalMode_t mode) {
AHRS_ResetEulr(&(g->setpoint.eulr)); /* 切换模式后重置设定值 */ AHRS_ResetEulr(&(g->setpoint.eulr)); /* 切换模式后重置设定值 */
if (g->mode == GIMBAL_MODE_RELAX) { if (g->mode == GIMBAL_MODE_RELAX) {
if (mode == GIMBAL_MODE_ABSOLUTE) { if (mode == GIMBAL_MODE_ABSOLUTE) {
if (g->param->limit_and_imu.yaw_imu) { g->setpoint.eulr.yaw = g->feedback.eulr.imu.yaw;
g->setpoint.eulr.yaw = g->feedback.eulr.imu.yaw;
} else {
g->setpoint.eulr.yaw = g->feedback.eulr.encoder.yaw;
}
} else if (mode == GIMBAL_MODE_RELATIVE) { } else if (mode == GIMBAL_MODE_RELATIVE) {
g->setpoint.eulr.yaw = g->feedback.eulr.encoder.yaw; g->setpoint.eulr.yaw = g->feedback.eulr.encoder.yaw;
} else if (mode == GIMBAL_MODE_SEARCH) {
g->setpoint.eulr.yaw = g->feedback.eulr.encoder.yaw;
} }
} }
@ -76,12 +66,9 @@ int8_t Gimbal_Init(Gimbal_t *g, const Gimbal_Params_t *param, float limit_max,
/* 设置软件限位 */ /* 设置软件限位 */
if (g->param->reverse.pit) CircleReverse(&limit_max); if (g->param->reverse.pit) CircleReverse(&limit_max);
g->limit.pit_min = g->limit.pit_max = limit_max; g->limit.min = g->limit.max = limit_max;
CircleAdd(&(g->limit.pit_min), -g->param->limit_and_imu.pitch_travel_rad, M_2PI); CircleAdd(&(g->limit.min), -g->param->pitch_travel_rad, M_2PI);
if (g->param->reverse.yaw) CircleReverse(&limit_max);
g->limit.yaw_min = g->limit.yaw_max = limit_max;
CircleAdd(&(g->limit.yaw_min), -g->param->limit_and_imu.yaw_travel_rad, M_2PI);
/* 初始化云台电机控制PID和LPF */ /* 初始化云台电机控制PID和LPF */
PID_Init(&(g->pid[GIMBAL_PID_YAW_ANGLE_IDX]), KPID_MODE_NO_D, target_freq, PID_Init(&(g->pid[GIMBAL_PID_YAW_ANGLE_IDX]), KPID_MODE_NO_D, target_freq,
&(g->param->pid[GIMBAL_PID_YAW_ANGLE_IDX])); &(g->param->pid[GIMBAL_PID_YAW_ANGLE_IDX]));
@ -143,80 +130,38 @@ int8_t Gimbal_Control(Gimbal_t *g, CMD_GimbalCmd_t *g_cmd, uint32_t now) {
Gimbal_SetMode(g, g_cmd->mode); Gimbal_SetMode(g, g_cmd->mode);
/* yaw坐标正方向与遥控器操作逻辑相反 */ /* yaw坐标正方向与遥控器操作逻辑相反 */
g_cmd->delta_eulr.pit = -g_cmd->delta_eulr.pit; g_cmd->delta_eulr.pit = g_cmd->delta_eulr.pit;
g_cmd->delta_eulr.yaw = -g_cmd->delta_eulr.yaw; if (g->param->pit_ctrl_reverse){
g_cmd->delta_eulr.yaw = -g_cmd->delta_eulr.yaw;
}else{
g_cmd->delta_eulr.yaw = -g_cmd->delta_eulr.yaw;
}
// /* 处理yaw控制命令 */ /* 处理yaw控制命令 */
// CircleAdd(&(g->setpoint.eulr.yaw), g_cmd->delta_eulr.yaw, M_2PI); CircleAdd(&(g->setpoint.eulr.yaw), g_cmd->delta_eulr.yaw, M_2PI);
if (g->param->limit_and_imu.yaw_travel_rad > 0.0f) {
/* 处理yaw控制命令，软件限位 */
float delta_yaw_max;
float delta_yaw_min;
if (g->param->limit_and_imu.yaw_imu)
{
delta_yaw_max =
CircleError(g->limit.yaw_max,
(g->feedback.eulr.encoder.yaw + g->setpoint.eulr.yaw -
g->feedback.eulr.imu.yaw),
M_2PI);
delta_yaw_min =
CircleError(g->limit.yaw_min,
(g->feedback.eulr.encoder.yaw + g->setpoint.eulr.yaw -
g->feedback.eulr.imu.yaw),
M_2PI);
}else
{
delta_yaw_max =
CircleError(g->limit.yaw_max,
(g->feedback.eulr.encoder.yaw + g->setpoint.eulr.yaw -
g->feedback.eulr.encoder.yaw),
M_2PI);
delta_yaw_min =
CircleError(g->limit.yaw_min,
(g->feedback.eulr.encoder.yaw + g->setpoint.eulr.yaw -
g->feedback.eulr.encoder.yaw),
M_2PI);
}
Clip(&(g_cmd->delta_eulr.yaw), delta_yaw_min, delta_yaw_max);
}else (CircleAdd(&(g->setpoint.eulr.yaw), g_cmd->delta_eulr.yaw, M_2PI));
g->setpoint.eulr.yaw += g_cmd->delta_eulr.yaw;
if (g->param->limit_and_imu.pitch_travel_rad > 0.0f) {
/* 处理pitch控制命令，软件限位 */
float delta_pitch_max;
float delta_pitch_min;
if (g->param->limit_and_imu.pit_imu)
{
delta_pitch_max =
CircleError(g->limit.pit_max,
(g->feedback.eulr.encoder.pit + g->setpoint.eulr.pit -
g->feedback.eulr.imu.pit),
M_2PI);
delta_pitch_min =
CircleError(g->limit.pit_min,
(g->feedback.eulr.encoder.pit + g->setpoint.eulr.pit -
g->feedback.eulr.imu.pit),
M_2PI);
}else
{
delta_pitch_max =
CircleError(g->limit.pit_max,
(g->feedback.eulr.encoder.pit + g->setpoint.eulr.pit -
g->feedback.eulr.encoder.pit),
M_2PI);
delta_pitch_min =
CircleError(g->limit.pit_min,
(g->feedback.eulr.encoder.pit + g->setpoint.eulr.pit -
g->feedback.eulr.encoder.pit),
M_2PI);
}
Clip(&(g_cmd->delta_eulr.pit), delta_pitch_min, delta_pitch_max);
}else (CircleAdd(&(g->setpoint.eulr.pit), g_cmd->delta_eulr.pit, M_2PI));
/* 处理pitch控制命令，软件限位 */
const float delta_max =
CircleError(g->limit.max,
(g->feedback.eulr.encoder.pit + g->setpoint.eulr.pit -
g->feedback.eulr.imu.pit),
M_2PI);
const float delta_min =
CircleError(g->limit.min,
(g->feedback.eulr.encoder.pit + g->setpoint.eulr.pit -
g->feedback.eulr.imu.pit),
M_2PI);
// const float delta_max =
// CircleError(g->limit.max,
// (g->feedback.eulr.encoder.pit + g->setpoint.eulr.pit -
// g->feedback.eulr.encoder.pit),
// M_2PI);
// const float delta_min =
// CircleError(g->limit.min,
// (g->feedback.eulr.encoder.pit + g->setpoint.eulr.pit -
// g->feedback.eulr.encoder.pit),
// M_2PI);
Clip(&(g_cmd->delta_eulr.pit), delta_min, delta_max);
g->setpoint.eulr.pit += g_cmd->delta_eulr.pit; g->setpoint.eulr.pit += g_cmd->delta_eulr.pit;
/* 重置输入指令，防止重复处理 */ /* 重置输入指令，防止重复处理 */
@ -229,42 +174,28 @@ int8_t Gimbal_Control(Gimbal_t *g, CMD_GimbalCmd_t *g_cmd, uint32_t now) {
for (uint8_t i = 0; i < GIMBAL_ACTR_NUM; i++) g->out[i] = 0.0f; for (uint8_t i = 0; i < GIMBAL_ACTR_NUM; i++) g->out[i] = 0.0f;
break; break;
case GIMBAL_MODE_SEARCH:
g->setpoint.eulr.pit = g->limit.pit_min + (g->limit.pit_max - g->limit.pit_min) * (sinf(now / 100.0f) + 1.0f) / 2.0f; // 在min和max之间摆动
g->setpoint.eulr.yaw = g->limit.yaw_min + (g->limit.yaw_max - g->limit.yaw_min) * (sinf(now / 100.0f) + 1.0f) / 2.0f; // 在min和max之间摆动
case GIMBAL_MODE_ABSOLUTE: case GIMBAL_MODE_ABSOLUTE:
if (g->param->limit_and_imu.yaw_imu && g->param->limit_and_imu.pit_imu){ yaw_omega_set_point =
yaw_omega_set_point = PID_Calc(&(g->pid[GIMBAL_PID_YAW_ANGLE_IDX]), g->setpoint.eulr.yaw,
PID_Calc(&(g->pid[GIMBAL_PID_YAW_ANGLE_IDX]), g->setpoint.eulr.yaw, g->feedback.eulr.imu.yaw, 0.0f, g->dt);
g->feedback.eulr.imu.yaw, 0.0f, g->dt); g->out[GIMBAL_ACTR_YAW_IDX] =
g->out[GIMBAL_ACTR_YAW_IDX] = PID_Calc(&(g->pid[GIMBAL_PID_YAW_OMEGA_IDX]), yaw_omega_set_point,
PID_Calc(&(g->pid[GIMBAL_PID_YAW_OMEGA_IDX]), yaw_omega_set_point, g->feedback.gyro.z, 0.f, g->dt);
g->feedback.gyro.z, 0.f, g->dt);
pit_omega_set_point = pit_omega_set_point =
PID_Calc(&(g->pid[GIMBAL_PID_PIT_ANGLE_IDX]), g->setpoint.eulr.pit, PID_Calc(&(g->pid[GIMBAL_PID_PIT_ANGLE_IDX]), g->setpoint.eulr.pit,
g->feedback.eulr.imu.pit, 0.0f, g->dt); g->feedback.eulr.imu.pit, 0.0f, g->dt);
// pit_omega_set_point =
// PID_Calc(&(g->pid[GIMBAL_PID_PIT_ANGLE_IDX]), g->setpoint.eulr.pit,
// g->feedback.eulr.encoder.pit, 0.0f, g->dt);
g->out[GIMBAL_ACTR_PIT_IDX] = g->out[GIMBAL_ACTR_PIT_IDX] =
PID_Calc(&(g->pid[GIMBAL_PID_PIT_OMEGA_IDX]), pit_omega_set_point, PID_Calc(&(g->pid[GIMBAL_PID_PIT_OMEGA_IDX]), pit_omega_set_point,
0.0, 0.f, g->dt); g->feedback.gyro.x, 0.f, g->dt);
}else{
yaw_omega_set_point =
PID_Calc(&(g->pid[GIMBAL_PID_YAW_ANGLE_IDX]), g->setpoint.eulr.yaw,
g->feedback.eulr.encoder.yaw, 0.0f, g->dt);
g->out[GIMBAL_ACTR_YAW_IDX] =
PID_Calc(&(g->pid[GIMBAL_PID_YAW_OMEGA_IDX]), yaw_omega_set_point,
g->feedback.gyro.z, 0.f, g->dt);
pit_omega_set_point =
PID_Calc(&(g->pid[GIMBAL_PID_PIT_ANGLE_IDX]), g->setpoint.eulr.pit,
g->feedback.eulr.encoder.pit, 0.0f, g->dt);
g->out[GIMBAL_ACTR_PIT_IDX] =
PID_Calc(&(g->pid[GIMBAL_PID_PIT_OMEGA_IDX]), pit_omega_set_point,
0.0, 0.f, g->dt);
}
// g->out[GIMBAL_ACTR_PIT_IDX] =
// PID_Calc(&(g->pid[GIMBAL_PID_PIT_OMEGA_IDX]), pit_omega_set_point,
// 0.0, 0.f, g->dt);
break; break;
case GIMBAL_MODE_RELATIVE: case GIMBAL_MODE_RELATIVE:
@ -292,13 +223,6 @@ int8_t Gimbal_Control(Gimbal_t *g, CMD_GimbalCmd_t *g_cmd, uint32_t now) {
return 0; return 0;
} }
uint8_t Gimbal_PackAI(Gimbal_t *gimbal, const AHRS_Eulr_t *eulr_gimbal_for_ai) {
memcpy((void *)eulr_gimbal_for_ai, (const void *)&(gimbal->feedback.eulr.encoder),
sizeof(gimbal->feedback.eulr.encoder));
return 0;
}
/** /**
* \brief 复制云台输出值 * \brief 复制云台输出值
* *

22
User/module/gimbal.h
View File

@ -52,13 +52,7 @@ typedef struct {
float gyro; /* 陀螺仪数据 */ float gyro; /* 陀螺仪数据 */
} low_pass_cutoff_freq; } low_pass_cutoff_freq;
/* 设置云台限位和是否启用imu */ float pitch_travel_rad; /* 云台pitch轴行程弧度 */
struct {
float yaw_travel_rad; /* 云台yaw轴行程弧度 */
float pitch_travel_rad; /* 云台pitch轴行程弧度 */
bool yaw_imu;
bool pit_imu;
} limit_and_imu;
/* 设置默认运动方向 */ /* 设置默认运动方向 */
struct { struct {
@ -66,14 +60,14 @@ typedef struct {
bool pit; bool pit;
} reverse; } reverse;
bool pit_ctrl_reverse; /* 云台pitch遥控方向 */
} Gimbal_Params_t; } Gimbal_Params_t;
/* 软件限位 */ /* 软件限位 */
typedef struct { typedef struct {
float yaw_max; float max;
float yaw_min; float min;
float pit_max;
float pit_min;
} Gimbal_Limit_t; } Gimbal_Limit_t;
/* 云台反馈数据的结构体，包含反馈控制用的反馈数据 */ /* 云台反馈数据的结构体，包含反馈控制用的反馈数据 */
@ -141,9 +135,6 @@ int8_t Gimbal_Init(Gimbal_t *g, const Gimbal_Params_t *param, float limit,
*/ */
int8_t Gimbal_UpdateFeedback(Gimbal_t *gimbal, const CAN_t *can); int8_t Gimbal_UpdateFeedback(Gimbal_t *gimbal, const CAN_t *can);
uint8_t Gimbal_PackAI(Gimbal_t *gimbal, const AHRS_Eulr_t *eulr_gimbal_for_ai);
/** /**
* \brief 运行云台控制逻辑 * \brief 运行云台控制逻辑
* *
@ -156,9 +147,6 @@ uint8_t Gimbal_PackAI(Gimbal_t *gimbal, const AHRS_Eulr_t *eulr_gimbal_for_ai);
*/ */
int8_t Gimbal_Control(Gimbal_t *g, CMD_GimbalCmd_t *g_cmd, uint32_t now); int8_t Gimbal_Control(Gimbal_t *g, CMD_GimbalCmd_t *g_cmd, uint32_t now);
uint8_t Gimbal_PackAI(Gimbal_t *gimbal, const AHRS_Eulr_t *eulr_gimbal_for_ai);
/** /**
* \brief 复制云台输出值 * \brief 复制云台输出值
* *

6
User/module/shoot.c
View File

@ -252,9 +252,9 @@ int8_t Shoot_Control(Shoot_t *s, CMD_ShootCmd_t *s_cmd,
float fric_rpm = CalculateRpm(s->fire_ctrl.bullet_speed, s->param->fric_radius, float fric_rpm = CalculateRpm(s->fire_ctrl.bullet_speed, s->param->fric_radius,
(s->param->model == SHOOT_MODEL_17MM)); (s->param->model == SHOOT_MODEL_17MM));
s->setpoint.fric_rpm[0] = fric_rpm; s->setpoint.fric_rpm[0] = -fric_rpm;
s->setpoint.fric_rpm[1] = -fric_rpm; s->setpoint.fric_rpm[1] = fric_rpm;
s->setpoint.fric_rpm[2] = fric_rpm; s->setpoint.fric_rpm[2] = -fric_rpm;
/* 计算拨弹电机位置的目标值 */ /* 计算拨弹电机位置的目标值 */
if (((now - s->fire_ctrl.last_shoot) >= s->fire_ctrl.period_ms) && if (((now - s->fire_ctrl.last_shoot) >= s->fire_ctrl.period_ms) &&

20
User/task/ai.c
View File

@ -17,13 +17,11 @@ AI_t ai;
CMD_Host_t cmd_host; CMD_Host_t cmd_host;
AHRS_Quaternion_t quat; AHRS_Quaternion_t quat;
Referee_ForAI_t referee_ai; Referee_ForAI_t referee_ai;
AHRS_Eulr_t gimbal_ai;
#else #else
static AI_t ai; static AI_t ai;
static CMD_Host_t cmd_host; static CMD_Host_t cmd_host;
static AHRS_Quaternion_t quat; static AHRS_Quaternion_t quat;
static Referee_ForAI_t referee_ai; static Referee_ForAI_t referee_ai;
static AHRS_Eulr_t gimbal_ai;
#endif #endif
/* Private function --------------------------------------------------------- */ /* Private function --------------------------------------------------------- */
@ -44,7 +42,9 @@ void Task_Ai(void *argument) {
AI_Init(&ai); AI_Init(&ai);
uint32_t tick = osKernelGetTickCount(); uint32_t tick = osKernelGetTickCount();
uint32_t last_online_tick = tick; uint32_t last_online_tick = tick;
while (1) { while (1) {
#ifdef DEBUG #ifdef DEBUG
task_runtime.stack_water_mark.ai = osThreadGetStackSpace(osThreadGetId()); task_runtime.stack_water_mark.ai = osThreadGetStackSpace(osThreadGetId());
@ -54,25 +54,19 @@ void Task_Ai(void *argument) {
AI_StartReceiving(&ai); AI_StartReceiving(&ai);
if (AI_WaitDmaCplt()) { if (AI_WaitDmaCplt()) {
AI_ParseHost(&ai); AI_ParseHost(&ai, &cmd_host);
last_online_tick = tick; last_online_tick = tick;
} else { } else {
if (tick - last_online_tick > 300) 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.ai.quat, &(quat), NULL, 0);
osMessageQueueGet(task_runtime.msgq.cmd.ai, &(ai.status), NULL, 0); osMessageQueueGet(task_runtime.msgq.cmd.ai, &(ai.status), NULL, 0);
osMessageQueueGet(task_runtime.msgq.gimbal.eulr_encoder, &(gimbal_ai), NULL, 0);
bool ref_update = (osMessageQueueGet(task_runtime.msgq.referee.ai, bool ref_update = (osMessageQueueGet(task_runtime.msgq.referee.ai,
&(referee_ai), NULL, 0) == osOK); &(referee_ai), NULL, 0) == osOK);
AI_PackMCU(&ai, &quat);
AI_PackMCU(&ai, &quat, &gimbal_ai);
if (ref_update) AI_PackRef(&ai, &(referee_ai)); if (ref_update) AI_PackRef(&ai, &(referee_ai));
AI_StartSend(&(ai), ref_update); AI_StartSend(&(ai), ref_update);

4
User/task/atti_esti.c
View File

@ -126,10 +126,10 @@ void Task_AttiEsti(void *argument) {
osMessageQueuePut(task_runtime.msgq.gimbal.accl, &bmi088.accl, 0, 0); osMessageQueuePut(task_runtime.msgq.gimbal.accl, &bmi088.accl, 0, 0);
osMessageQueueReset(task_runtime.msgq.gimbal.eulr_imu); osMessageQueueReset(task_runtime.msgq.gimbal.eulr_imu);
osMessageQueuePut(task_runtime.msgq.gimbal.eulr_imu, &eulr_to_send, 0, 0); osMessageQueuePut(task_runtime.msgq.gimbal.eulr_imu, &eulr_to_send, 0, 0);
osMessageQueueReset(task_runtime.msgq.gimbal.gyro);
osMessageQueuePut(task_runtime.msgq.gimbal.gyro, &bmi088.gyro, 0, 0);
osMessageQueueReset(task_runtime.msgq.ai.quat); osMessageQueueReset(task_runtime.msgq.ai.quat);
osMessageQueuePut(task_runtime.msgq.ai.quat, &(gimbal_ahrs.quat), 0, 0); osMessageQueuePut(task_runtime.msgq.ai.quat, &(gimbal_ahrs.quat), 0, 0);
osMessageQueueReset(task_runtime.msgq.gimbal.gyro);
osMessageQueuePut(task_runtime.msgq.gimbal.gyro, &bmi088.gyro, 0, 0);
/* PID控制IMU温度，PWM输出 */ /* PID控制IMU温度，PWM输出 */
BSP_PWM_Set(BSP_PWM_IMU_HEAT, BSP_PWM_Set(BSP_PWM_IMU_HEAT,

12
User/task/cli.c
View File

@ -523,7 +523,7 @@ static BaseType_t Command_SetGimbalLim(char *out_buffer, size_t len,
switch (fsm.stage) { switch (fsm.stage) {
case 0: case 0:
Config_Get(&cfg); Config_Get(&cfg);
snprintf(out_buffer, len, "(old)pit_limit:%f,(old)yaw_limit:%f\r\n", cfg.gimbal_pitch_limit, cfg.gimbal_yaw_limit); snprintf(out_buffer, len, "(old)limit:%f\r\n", cfg.gimbal_limit);
fsm.stage = 1; fsm.stage = 1;
return pdPASS; return pdPASS;
case 1: case 1:
@ -538,12 +538,12 @@ static BaseType_t Command_SetGimbalLim(char *out_buffer, size_t len,
} }
Config_Get(&cfg); Config_Get(&cfg);
osThreadResume(task_runtime.thread.ctrl_gimbal); osThreadResume(task_runtime.thread.ctrl_gimbal);
cfg.gimbal_pitch_limit = can.motor.gimbal.named.pit.rotor_angle; cfg.gimbal_limit = can.motor.gimbal.named.pit.rotor_angle;
cfg.gimbal_yaw_limit = can.motor.gimbal.named.yaw.rotor_angle;
Config_Set(&cfg); Config_Set(&cfg);
Config_Get(&cfg); Config_Get(&cfg);
snprintf(out_buffer, len, "(new)limit_pitch:%f,limit_yaw%f\r\nDone.", cfg.gimbal_pitch_limit, cfg.gimbal_yaw_limit); snprintf(out_buffer, len, "(new)limit:%f\r\nDone.", cfg.gimbal_limit);
fsm.stage = 2; fsm.stage = 2;
return pdPASS; return pdPASS;
case 2: case 2:
@ -692,7 +692,7 @@ void Task_CLI(void *argument) {
BSP_USB_Printf(CLI_WELCOME_MESSAGE); BSP_USB_Printf(CLI_WELCOME_MESSAGE);
/* 开始运行命令行界面 */ /* 开始运行命令行界面 */
BSP_USB_Printf("qut-move-rm>"); BSP_USB_Printf("rm>");
while (1) { while (1) {
#ifdef DEBUG #ifdef DEBUG
/* 记录任务所使用的的栈空间 */ /* 记录任务所使用的的栈空间 */
@ -730,7 +730,7 @@ void Task_CLI(void *argument) {
index = 0; /* 重置索引，准备接收下一段命令 */ index = 0; /* 重置索引，准备接收下一段命令 */
memset(input, 0x00, strlen(input)); /* 清空输入缓存 */ memset(input, 0x00, strlen(input)); /* 清空输入缓存 */
} }
BSP_USB_Printf("qut-move-rm>"); BSP_USB_Printf("rm>");
} else if (rx_char == '\b' || rx_char == 0x7Fu) { } else if (rx_char == '\b' || rx_char == 0x7Fu) {
/* 如果输入的是退格键则清空一位输入缓存，同时进行界限保护 */ /* 如果输入的是退格键则清空一位输入缓存，同时进行界限保护 */
if (index > 0) { if (index > 0) {

7
User/task/ctrl_gimbal.c
View File

@ -22,13 +22,11 @@ CMD_GimbalCmd_t gimbal_cmd;
Gimbal_t gimbal; Gimbal_t gimbal;
CAN_GimbalOutput_t gimbal_out; CAN_GimbalOutput_t gimbal_out;
Referee_GimbalUI_t gimbal_ui; Referee_GimbalUI_t gimbal_ui;
AHRS_Eulr_t eulr_gimbal_for_ai;
#else #else
static CMD_GimbalCmd_t gimbal_cmd; static CMD_GimbalCmd_t gimbal_cmd;
static Gimbal_t gimbal; static Gimbal_t gimbal;
static CAN_GimbalOutput_t gimbal_out; static CAN_GimbalOutput_t gimbal_out;
static Referee_GimbalUI_t gimbal_ui; static Referee_GimbalUI_t gimbal_ui;
static AHRS_Eulr_t eulr_gimbal_for_ai;
#endif #endif
/* Private function --------------------------------------------------------- */ /* Private function --------------------------------------------------------- */
@ -45,7 +43,7 @@ void Task_CtrlGimbal(void *argument) {
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CTRL_GIMBAL; const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CTRL_GIMBAL;
/* 初始化云台 */ /* 初始化云台 */
Gimbal_Init(&gimbal, &(task_runtime.cfg.robot_param->gimbal), Gimbal_Init(&gimbal, &(task_runtime.cfg.robot_param->gimbal),
task_runtime.cfg.gimbal_pitch_limit, (float)TASK_FREQ_CTRL_GIMBAL); task_runtime.cfg.gimbal_limit, (float)TASK_FREQ_CTRL_GIMBAL);
/* 延时一段时间再开启任务 */ /* 延时一段时间再开启任务 */
osMessageQueueGet(task_runtime.msgq.can.feedback.gimbal, &can, NULL, osMessageQueueGet(task_runtime.msgq.can.feedback.gimbal, &can, NULL,
@ -68,15 +66,12 @@ void Task_CtrlGimbal(void *argument) {
osKernelLock(); /* 锁住RTOS内核防止控制过程中断，造成错误 */ osKernelLock(); /* 锁住RTOS内核防止控制过程中断，造成错误 */
Gimbal_UpdateFeedback(&gimbal, &can); Gimbal_UpdateFeedback(&gimbal, &can);
Gimbal_PackAI(&gimbal, &eulr_gimbal_for_ai);
Gimbal_Control(&gimbal, &gimbal_cmd, tick); Gimbal_Control(&gimbal, &gimbal_cmd, tick);
Gimbal_DumpOutput(&gimbal, &gimbal_out); Gimbal_DumpOutput(&gimbal, &gimbal_out);
osKernelUnlock(); osKernelUnlock();
osMessageQueueReset(task_runtime.msgq.can.output.gimbal); osMessageQueueReset(task_runtime.msgq.can.output.gimbal);
osMessageQueuePut(task_runtime.msgq.can.output.gimbal, &gimbal_out, 0, 0); osMessageQueuePut(task_runtime.msgq.can.output.gimbal, &gimbal_out, 0, 0);
osMessageQueueReset(task_runtime.msgq.gimbal.eulr_encoder);
osMessageQueuePut(task_runtime.msgq.gimbal.eulr_encoder, &eulr_gimbal_for_ai, 0, 0);
Gimbal_DumpUI(&gimbal, &gimbal_ui); Gimbal_DumpUI(&gimbal, &gimbal_ui);
osMessageQueueReset(task_runtime.msgq.ui.gimbal); osMessageQueueReset(task_runtime.msgq.ui.gimbal);

176
User/task/init.c
View File

@ -30,103 +30,101 @@
* *
* \param argument 未使用 * \param argument 未使用
*/ */
void Task_Init(void *argument) void Task_Init(void *argument) {
{ (void)argument; /* 未使用argument，消除警告 */
(void)argument; /* 未使用argument，消除警告 */
Config_Get(&task_runtime.cfg); /* 获取机器人配置 */ Config_Get(&task_runtime.cfg); /* 获取机器人配置 */
osKernelLock(); osKernelLock();
/* 创建任务 */ /* 创建任务 */
task_runtime.thread.atti_esti = task_runtime.thread.atti_esti =
osThreadNew(Task_AttiEsti, NULL, &attr_atti_esti); osThreadNew(Task_AttiEsti, NULL, &attr_atti_esti);
task_runtime.thread.cli = osThreadNew(Task_CLI, NULL, &attr_cli); task_runtime.thread.cli = osThreadNew(Task_CLI, NULL, &attr_cli);
task_runtime.thread.command = osThreadNew(Task_Command, NULL, &attr_command); task_runtime.thread.command = osThreadNew(Task_Command, NULL, &attr_command);
task_runtime.thread.ctrl_chassis = task_runtime.thread.ctrl_chassis =
osThreadNew(Task_CtrlChassis, NULL, &attr_ctrl_chassis); osThreadNew(Task_CtrlChassis, NULL, &attr_ctrl_chassis);
task_runtime.thread.ctrl_gimbal = task_runtime.thread.ctrl_gimbal =
osThreadNew(Task_CtrlGimbal, NULL, &attr_ctrl_gimbal); osThreadNew(Task_CtrlGimbal, NULL, &attr_ctrl_gimbal);
task_runtime.thread.ctrl_shoot = task_runtime.thread.ctrl_shoot =
osThreadNew(Task_CtrlShoot, NULL, &attr_ctrl_shoot); osThreadNew(Task_CtrlShoot, NULL, &attr_ctrl_shoot);
task_runtime.thread.info = osThreadNew(Task_Info, NULL, &attr_info); task_runtime.thread.info = osThreadNew(Task_Info, NULL, &attr_info);
task_runtime.thread.monitor = osThreadNew(Task_Monitor, NULL, &attr_monitor); task_runtime.thread.monitor = osThreadNew(Task_Monitor, NULL, &attr_monitor);
task_runtime.thread.can = osThreadNew(Task_Can, NULL, &attr_can); task_runtime.thread.can = osThreadNew(Task_Can, NULL, &attr_can);
task_runtime.thread.referee = osThreadNew(Task_Referee, NULL, &attr_referee); task_runtime.thread.referee = osThreadNew(Task_Referee, NULL, &attr_referee);
task_runtime.thread.ai = osThreadNew(Task_Ai, NULL, &attr_ai); task_runtime.thread.ai = osThreadNew(Task_Ai, NULL, &attr_ai);
task_runtime.thread.rc = osThreadNew(Task_RC, NULL, &attr_rc); task_runtime.thread.rc = osThreadNew(Task_RC, NULL, &attr_rc);
task_runtime.thread.cap = osThreadNew(Task_Cap, NULL, &attr_cap); task_runtime.thread.cap = osThreadNew(Task_Cap, NULL, &attr_cap);
/* 创建消息队列 */ /* 创建消息队列 */
/* motor */ /* motor */
task_runtime.msgq.can.feedback.chassis = task_runtime.msgq.can.feedback.chassis =
osMessageQueueNew(2u, sizeof(CAN_t), NULL); osMessageQueueNew(2u, sizeof(CAN_t), NULL);
task_runtime.msgq.can.feedback.gimbal = task_runtime.msgq.can.feedback.gimbal =
osMessageQueueNew(2u, sizeof(CAN_t), NULL); osMessageQueueNew(2u, sizeof(CAN_t), NULL);
task_runtime.msgq.can.feedback.shoot = task_runtime.msgq.can.feedback.shoot =
osMessageQueueNew(2u, sizeof(CAN_t), NULL); osMessageQueueNew(2u, sizeof(CAN_t), NULL);
task_runtime.msgq.can.feedback.cap = task_runtime.msgq.can.feedback.cap =
osMessageQueueNew(2u, sizeof(CAN_t), NULL); osMessageQueueNew(2u, sizeof(CAN_t), NULL);
task_runtime.msgq.can.output.chassis = task_runtime.msgq.can.output.chassis =
osMessageQueueNew(2u, sizeof(CAN_ChassisOutput_t), NULL); osMessageQueueNew(2u, sizeof(CAN_ChassisOutput_t), NULL);
task_runtime.msgq.can.output.gimbal = task_runtime.msgq.can.output.gimbal =
osMessageQueueNew(2u, sizeof(CAN_GimbalOutput_t), NULL); osMessageQueueNew(2u, sizeof(CAN_GimbalOutput_t), NULL);
task_runtime.msgq.can.output.shoot = task_runtime.msgq.can.output.shoot =
osMessageQueueNew(2u, sizeof(CAN_ShootOutput_t), NULL); osMessageQueueNew(2u, sizeof(CAN_ShootOutput_t), NULL);
task_runtime.msgq.can.output.cap = task_runtime.msgq.can.output.cap =
osMessageQueueNew(2u, sizeof(CAN_CapOutput_t), NULL); osMessageQueueNew(2u, sizeof(CAN_CapOutput_t), NULL);
/* command */ /* command */
task_runtime.msgq.cmd.chassis = task_runtime.msgq.cmd.chassis =
osMessageQueueNew(3u, sizeof(CMD_ChassisCmd_t), NULL); osMessageQueueNew(3u, sizeof(CMD_ChassisCmd_t), NULL);
task_runtime.msgq.cmd.gimbal = task_runtime.msgq.cmd.gimbal =
osMessageQueueNew(3u, sizeof(CMD_GimbalCmd_t), NULL); osMessageQueueNew(3u, sizeof(CMD_GimbalCmd_t), NULL);
task_runtime.msgq.cmd.shoot = task_runtime.msgq.cmd.shoot =
osMessageQueueNew(3u, sizeof(CMD_ShootCmd_t), NULL); osMessageQueueNew(3u, sizeof(CMD_ShootCmd_t), NULL);
task_runtime.msgq.cmd.ai = task_runtime.msgq.cmd.ai =
osMessageQueueNew(3u, sizeof(CMD_AI_Status_t), NULL); osMessageQueueNew(3u, sizeof(CMD_AI_Status_t), NULL);
task_runtime.msgq.cmd.referee = osMessageQueueNew(6u, sizeof(CMD_UI_t), NULL); task_runtime.msgq.cmd.referee = osMessageQueueNew(6u, sizeof(CMD_UI_t), NULL);
/* atti_esti */ /* atti_esti */
task_runtime.msgq.cmd.raw.rc = osMessageQueueNew(3u, sizeof(CMD_RC_t), NULL); task_runtime.msgq.cmd.raw.rc = osMessageQueueNew(3u, sizeof(CMD_RC_t), NULL);
task_runtime.msgq.cmd.raw.host = task_runtime.msgq.cmd.raw.host =
osMessageQueueNew(3u, sizeof(CMD_Host_t), NULL); osMessageQueueNew(3u, sizeof(CMD_Host_t), NULL);
task_runtime.msgq.gimbal.accl = task_runtime.msgq.gimbal.accl =
osMessageQueueNew(2u, sizeof(AHRS_Accl_t), NULL); osMessageQueueNew(2u, sizeof(AHRS_Accl_t), NULL);
task_runtime.msgq.gimbal.eulr_imu = task_runtime.msgq.gimbal.eulr_imu =
osMessageQueueNew(2u, sizeof(AHRS_Eulr_t), NULL); osMessageQueueNew(2u, sizeof(AHRS_Eulr_t), NULL);
task_runtime.msgq.gimbal.gyro = task_runtime.msgq.gimbal.gyro =
osMessageQueueNew(2u, sizeof(AHRS_Gyro_t), NULL); osMessageQueueNew(2u, sizeof(AHRS_Gyro_t), NULL);
task_runtime.msgq.gimbal.eulr_encoder =
osMessageQueueNew(2u, sizeof(AHRS_Eulr_t), NULL);
task_runtime.msgq.cap_info =
osMessageQueueNew(2u, sizeof(CAN_Capacitor_t), NULL);
/* AI */ task_runtime.msgq.cap_info =
task_runtime.msgq.ai.quat = osMessageQueueNew(2u, sizeof(CAN_Capacitor_t), NULL);
osMessageQueueNew(2u, sizeof(AHRS_Quaternion_t), NULL);
/* 裁判系统 */ /* AI */
task_runtime.msgq.referee.ai = task_runtime.msgq.ai.quat =
osMessageQueueNew(2u, sizeof(Referee_ForAI_t), NULL); osMessageQueueNew(2u, sizeof(AHRS_Quaternion_t), NULL);
task_runtime.msgq.referee.chassis =
osMessageQueueNew(2u, sizeof(Referee_ForChassis_t), NULL);
task_runtime.msgq.referee.cap =
osMessageQueueNew(2u, sizeof(Referee_ForCap_t), NULL);
task_runtime.msgq.referee.shoot =
osMessageQueueNew(2u, sizeof(Referee_ForShoot_t), NULL);
/* UI */ /* 裁判系统 */
task_runtime.msgq.ui.chassis = task_runtime.msgq.referee.ai =
osMessageQueueNew(2u, sizeof(Referee_ChassisUI_t), NULL); osMessageQueueNew(2u, sizeof(Referee_ForAI_t), NULL);
task_runtime.msgq.ui.cap = task_runtime.msgq.referee.chassis =
osMessageQueueNew(2u, sizeof(Referee_CapUI_t), NULL); osMessageQueueNew(2u, sizeof(Referee_ForChassis_t), NULL);
task_runtime.msgq.ui.gimbal = task_runtime.msgq.referee.cap =
osMessageQueueNew(2u, sizeof(Referee_GimbalUI_t), NULL); osMessageQueueNew(2u, sizeof(Referee_ForCap_t), NULL);
task_runtime.msgq.ui.shoot = task_runtime.msgq.referee.shoot =
osMessageQueueNew(2u, sizeof(Referee_ShootUI_t), NULL); osMessageQueueNew(2u, sizeof(Referee_ForShoot_t), NULL);
task_runtime.msgq.ui.cmd = osMessageQueueNew(2u, sizeof(bool), NULL);
osKernelUnlock(); /* UI */
osThreadTerminate(osThreadGetId()); /* 结束自身 */ task_runtime.msgq.ui.chassis =
osMessageQueueNew(2u, sizeof(Referee_ChassisUI_t), NULL);
task_runtime.msgq.ui.cap =
osMessageQueueNew(2u, sizeof(Referee_CapUI_t), NULL);
task_runtime.msgq.ui.gimbal =
osMessageQueueNew(2u, sizeof(Referee_GimbalUI_t), NULL);
task_runtime.msgq.ui.shoot =
osMessageQueueNew(2u, sizeof(Referee_ShootUI_t), NULL);
task_runtime.msgq.ui.cmd = osMessageQueueNew(2u, sizeof(bool), NULL);
osKernelUnlock();
osThreadTerminate(osThreadGetId()); /* 结束自身 */
} }

2
User/task/user_task.h
View File

@ -58,8 +58,6 @@ typedef struct {
osMessageQueueId_t accl; /* IMU读取 */ osMessageQueueId_t accl; /* IMU读取 */
osMessageQueueId_t gyro; /* IMU读取 */ osMessageQueueId_t gyro; /* IMU读取 */
osMessageQueueId_t eulr_imu; /* 姿态解算得到 */ osMessageQueueId_t eulr_imu; /* 姿态解算得到 */
osMessageQueueId_t eulr_encoder; /* 读编码器得到 */
} gimbal; } gimbal;
/* 控制指令 */ /* 控制指令 */