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

加ai

Browse Source
This commit is contained in:
Robofish 2026-01-14 00:36:11 +08:00
parent 70233c2f90
commit bfb2368082
20 changed files with 9379 additions and 9385 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
CMakeLists.txt
View File

@ -82,10 +82,11 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
# User/module sources
User/module/balance_chassis.c
User/module/config.c
User/module/shoot.c
User/module/gimbal.c
User/module/shoot.c
# User/task sources
User/task/ai.c
User/task/atti_esit.c
User/task/blink.c
User/task/ctrl_chassis.c

64
MDK-ARM/CtrBoard-H7_ALL.uvoptx
View File

@ -188,7 +188,17 @@
<Ww>
<count>6</count>
<WinNumber>1</WinNumber>
<ItemText>chassis</ItemText>
<ItemText>ai</ItemText>
</Ww>
<Ww>
<count>7</count>
<WinNumber>1</WinNumber>
<ItemText>cmd_ai</ItemText>
</Ww>
<Ww>
<count>8</count>
<WinNumber>1</WinNumber>
<ItemText>gimbal</ItemText>
</Ww>
</WatchWindow1>
<Tracepoint>
@ -1289,6 +1299,18 @@
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>83</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>..\User\device\vision_bridge.c</PathWithFileName>
<FilenameWithoutPath>vision_bridge.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
</Group>
<Group>
@ -1299,7 +1321,7 @@
<RteFlg>0</RteFlg>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>83</FileNumber>
<FileNumber>84</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1311,7 +1333,7 @@
</File>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>84</FileNumber>
<FileNumber>85</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1323,7 +1345,7 @@
</File>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>85</FileNumber>
<FileNumber>86</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1335,7 +1357,7 @@
</File>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>86</FileNumber>
<FileNumber>87</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1355,7 +1377,7 @@
<RteFlg>0</RteFlg>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>87</FileNumber>
<FileNumber>88</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1367,7 +1389,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>88</FileNumber>
<FileNumber>89</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1379,7 +1401,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>89</FileNumber>
<FileNumber>90</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1391,7 +1413,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>90</FileNumber>
<FileNumber>91</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1403,7 +1425,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>91</FileNumber>
<FileNumber>92</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1415,7 +1437,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>92</FileNumber>
<FileNumber>93</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1427,7 +1449,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>93</FileNumber>
<FileNumber>94</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1439,7 +1461,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>94</FileNumber>
<FileNumber>95</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1451,7 +1473,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>95</FileNumber>
<FileNumber>96</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1463,7 +1485,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>96</FileNumber>
<FileNumber>97</FileNumber>
<FileType>5</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1473,6 +1495,18 @@
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>98</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>..\User\task\ai.c</PathWithFileName>
<FilenameWithoutPath>ai.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
</Group>
<Group>

10
MDK-ARM/CtrBoard-H7_ALL.uvprojx
View File

@ -829,6 +829,11 @@
<FileType>1</FileType>
<FilePath>..\User\device\gimbal_imu.c</FilePath>
</File>
<File>
<FileName>vision_bridge.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\device\vision_bridge.c</FilePath>
</File>
</Files>
</Group>
<Group>
@ -909,6 +914,11 @@
<FileType>5</FileType>
<FilePath>..\User\task\user_task.h</FilePath>
</File>
<File>
<FileName>ai.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\task\ai.c</FilePath>
</File>
</Files>
</Group>
<Group>

BIN
MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.axf
View File

Binary file not shown.

18310
MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.hex
View File

File diff suppressed because it is too large Load Diff

152
User/device/ai.c
View File

@ -1,152 +0,0 @@
/*
AI
*/
/* Includes ----------------------------------------------------------------- */
#include "ai.h"
#include <string.h>
#include "bsp\delay.h"
#include "bsp\uart.h"
#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 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) {
UNUSED(ai);
ASSERT(ai);
if (inited) return DEVICE_ERR_INITED;
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;
}
int8_t AI_Restart(void) {
__HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_AI));
__HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_AI));
return DEVICE_OK;
}
int8_t AI_StartReceiving(AI_t *ai) {
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;
}
bool AI_WaitDmaCplt(void) {
return (osThreadFlagsWait(SIGNAL_AI_RAW_REDY, osFlagsWaitAll, 0) ==
SIGNAL_AI_RAW_REDY);
}
int8_t AI_ParseHost(AI_t *ai) {
if (!CRC16_Verify((const uint8_t *)&(rxbuf), sizeof(ai->from_host)))
goto error;
ai->ai_online = true;
memcpy(&(ai->from_host), rxbuf, sizeof(ai->from_host));
memset(rxbuf, 0, AI_LEN_RX_BUFF);
return DEVICE_OK;
error:
drop_message++;
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;
ai->ai_online = false;
memset(&(ai->from_host), 0, sizeof(ai->from_host));
memset(cmd_host, 0, sizeof(*cmd_host));
return 0;
}
int8_t AI_PackMCU(AI_t *ai, const AHRS_Quaternion_t *quat) {
ai->to_host.mcu.id = AI_ID_MCU;
memcpy((void *)&(ai->to_host.mcu.package.data.quat), (const void *)quat,
sizeof(*quat));
ai->to_host.mcu.package.data.notice = 0;
if (ai->status == AI_STATUS_AUTOAIM)
ai->to_host.mcu.package.data.notice |= AI_NOTICE_AUTOAIM;
else if (ai->status == AI_STATUS_HITSWITCH)
ai->to_host.mcu.package.data.notice |= AI_NOTICE_HITBUFF;
else if (ai->status == AI_STATUS_AUTOMATIC)
ai->to_host.mcu.package.data.notice |= AI_NOTICE_AUTOMATIC;
ai->to_host.mcu.package.crc16 = CRC16_Calc(
(const uint8_t *)&(ai->to_host.mcu.package),
sizeof(ai->to_host.mcu.package) - sizeof(uint16_t), CRC16_INIT);
return DEVICE_OK;
}
int8_t AI_PackRef(AI_t *ai, const Referee_ForAI_t *ref) {
(void)ref;
ai->to_host.ref.id = AI_ID_REF;
ai->to_host.ref.package.crc16 = CRC16_Calc(
(const uint8_t *)&(ai->to_host.ref.package),
sizeof(ai->to_host.ref.package) - sizeof(uint16_t), CRC16_INIT);
return DEVICE_OK;
}
int8_t AI_StartSend(AI_t *ai, bool ref_update) {
if (ref_update) {
if (HAL_UART_Transmit_DMA(
BSP_UART_GetHandle(BSP_UART_AI), (uint8_t *)&(ai->to_host),
sizeof(ai->to_host.ref) + sizeof(ai->to_host.mcu)) == HAL_OK)
return DEVICE_OK;
else
return DEVICE_ERR;
} else {
if (HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_AI),
(uint8_t *)&(ai->to_host.mcu),
sizeof(ai->to_host.mcu)) == HAL_OK)
return DEVICE_OK;
else
return DEVICE_ERR;
}
}

69
User/device/ai.h
View File

@ -1,69 +0,0 @@
/*
AI
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <cmsis_os2.h>
#include <stdbool.h>
#include <stdint.h>
#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"
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef struct __packed {
uint8_t id;
Protocol_UpPackageReferee_t package;
} AI_UpPackageReferee_t;
typedef struct __packed {
uint8_t id;
Protocol_UpPackageMCU_t package;
} AI_UpPackageMCU_t;
typedef struct __packed {
osThreadId_t thread_alert;
Protocol_DownPackage_t from_host;
struct {
AI_UpPackageReferee_t ref;
AI_UpPackageMCU_t mcu;
} to_host;
CMD_AI_Status_t status;
bool ai_online;
} AI_t;
/* Exported functions prototypes -------------------------------------------- */
int8_t AI_Init(AI_t *ai);
int8_t AI_Restart(void);
int8_t AI_StartReceiving(AI_t *ai);
bool AI_WaitDmaCplt(void);
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

7
User/module/config.c
View File

@ -344,7 +344,12 @@ Config_RobotParam_t robot_config = {
.x = 0.0f,
.phi = -0.1f,
}
},
.ai_param = {
.can = BSP_FDCAN_2,
.vision_id = 0x104,
},
/* USER CODE END robot_config */
};

2
User/module/config.h
View File

@ -13,6 +13,7 @@ extern "C" {
#include "module/shoot.h"
#include "module/balance_chassis.h"
#include "module/gimbal.h"
#include "device/vision_bridge.h"
/**
* @brief 机器人参数配置结构体
* @note 在此添加您的配置参数
@ -22,6 +23,7 @@ typedef struct {
Shoot_Params_t shoot_param;
Chassis_Params_t chassis_param;
Gimbal_Params_t gimbal_param;
AI_Param_t ai_param;
/* USER CODE END Config_RobotParam */
} Config_RobotParam_t;

36
User/module/gimbal.c
View File

@ -163,7 +163,7 @@ int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal){
*
* \return 函数运行结果
*/
int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd, Gimbal_AI_t *g_ai) {
if (g == NULL || g_cmd == NULL) {
return -1;
}
@ -222,20 +222,44 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
g->out.yaw = 0.0f;
g->out.pit = 0.0f;
break;
case GIMBAL_MODE_AI_CONTROL:
if (g_ai != NULL && g_ai->ctrl) {
g->setpoint.eulr.yaw = g_ai->yaw;
g->setpoint.eulr.pit = g_ai->pit;
/* 限位处理 - 使用圆周误差保持一致性 */
if (g->param->travel.yaw > 0) {
if (CircleError(g->setpoint.eulr.yaw, g->limit.yaw.max, M_2PI) > 0) {
g->setpoint.eulr.yaw = g->limit.yaw.max;
}
if (CircleError(g->setpoint.eulr.yaw, g->limit.yaw.min, M_2PI) < 0) {
g->setpoint.eulr.yaw = g->limit.yaw.min;
}
}
if (g->param->travel.pit > 0) {
if (CircleError(g->setpoint.eulr.pit, g->limit.pit.max, M_2PI) > 0) {
g->setpoint.eulr.pit = g->limit.pit.max;
}
if (CircleError(g->setpoint.eulr.pit, g->limit.pit.min, M_2PI) < 0) {
g->setpoint.eulr.pit = g->limit.pit.min;
}
}
}
case GIMBAL_MODE_ABSOLUTE:
case GIMBAL_MODE_RELATIVE:
/* AI 控制模式：直接使用 AI 下发的目标角度 */
/* PID 控制计算 - AI 和手动控制都需要执行 */
yaw_omega_set_point = PID_Calc(&(g->pid.yaw_angle), g->setpoint.eulr.yaw,
g->feedback.imu.eulr.yaw, 0.0f, g->dt);
g->out.yaw = PID_Calc(&(g->pid.pit_omega), yaw_omega_set_point,
g->feedback.imu.gyro.z, 0.f, g->dt);
g->out.yaw = PID_Calc(&(g->pid.yaw_omega), yaw_omega_set_point,
g->feedback.imu.gyro.z, 0.0f, g->dt);
pit_omega_set_point = PID_Calc(&(g->pid.pit_angle), g->setpoint.eulr.pit,
g->feedback.imu.eulr.rol, 0.0f, g->dt);
g->out.pit = PID_Calc(&(g->pid.pit_omega), pit_omega_set_point,
g->feedback.imu.gyro.y, 0.f, g->dt);
g->feedback.imu.gyro.y, 0.0f, g->dt);
break;
}

9
User/module/gimbal.h
View File

@ -30,6 +30,7 @@ typedef enum {
GIMBAL_MODE_RELAX, /* 放松模式，电机不输出。一般情况云台初始化之后的模式 */
GIMBAL_MODE_ABSOLUTE, /* 绝对坐标系控制，控制在空间内的绝对姿态 */
GIMBAL_MODE_RELATIVE, /* 相对坐标系控制，控制相对于底盘的姿态 */
GIMBAL_MODE_AI_CONTROL /* AI控制模式，直接接受AI下发的目标角度 */
} Gimbal_Mode_t;
typedef struct {
@ -38,6 +39,12 @@ typedef struct {
float delta_pit;
} Gimbal_CMD_t;
typedef struct {
bool ctrl;
float yaw;
float pit;
} Gimbal_AI_t;
/* 软件限位 */
typedef struct {
float max;
@ -168,7 +175,7 @@ int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal);
*
* \return 函数运行结果
*/
int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd);
int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd, Gimbal_AI_t *g_ai);
/**
* \brief 云台输出

48
User/task/ai.c Normal file
View File

@ -0,0 +1,48 @@
/*
ai Task
*/
/* Includes ----------------------------------------------------------------- */
#include "task/user_task.h"
/* USER INCLUDE BEGIN */
#include "bsp/fdcan.h"
#include "module/config.h"
#include "device/vision_bridge.h"
/* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN */
AI_cmd_t cmd_ai;
AI_t ai;
/* USER STRUCT END */
/* Private function --------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
void Task_ai(void *argument) {
(void)argument; /* 未使用argument，消除警告 */
/* 计算任务运行到指定频率需要等待的tick数 */
const uint32_t delay_tick = osKernelGetTickFreq() / AI_FREQ;
osDelay(AI_INIT_DELAY); /* 延时一段时间再开启任务 */
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
/* USER CODE INIT BEGIN */
AI_Init(&ai, &Config_GetRobotParam()->ai_param);
/* 注册CAN接收ID */
/* USER CODE INIT END */
while (1) {
tick += delay_tick; /* 计算下一个唤醒时刻 */
/* USER CODE BEGIN */
AI_ParseCmdFromCan( &ai,&cmd_ai);
/* USER CODE END */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
}
}

7
User/task/config.yaml
View File

@ -46,3 +46,10 @@
function: Task_ctrl_shoot
name: ctrl_shoot
stack: 256
- delay: 0
description: ''
freq_control: true
frequency: 500.0
function: Task_ai
name: ai
stack: 256

2
User/task/ctrl_chassis.c
View File

@ -68,5 +68,3 @@ void Task_ctrl_chassis(void *argument) {
}
}

5
User/task/ctrl_gimbal.c
View File

@ -19,6 +19,7 @@
/* USER STRUCT BEGIN */
Gimbal_t gimbal;
Gimbal_CMD_t gimbal_cmd;
Gimbal_AI_t gimbal_ai;
// BSP_FDCAN_StdDataFrame_t can_frame;
/* USER STRUCT END */
@ -54,8 +55,8 @@ void Task_ctrl_gimbal(void *argument) {
osMessageQueueReset(task_runtime.msgq.chassis.yaw); // 重置消息队列，防止阻塞
osMessageQueuePut(task_runtime.msgq.chassis.yaw, &gimbal.feedback.motor.yaw, 0, 0);
Gimbal_Control(&gimbal, &gimbal_cmd);
osMessageQueueGet(task_runtime.msgq.gimbal.ai_cmd, &gimbal_ai, NULL, 0);
Gimbal_Control(&gimbal, &gimbal_cmd, &gimbal_ai);
Gimbal_Output(&gimbal);

1
User/task/ctrl_shoot.c
View File

@ -4,7 +4,6 @@
*/
/* Includes ----------------------------------------------------------------- */
#include "device/motor_rm.h"
#include "task/user_task.h"
/* USER INCLUDE BEGIN */
#include "module/shoot.h"

1
User/task/init.c
View File

@ -39,6 +39,7 @@ void Task_Init(void *argument) {
task_runtime.thread.monitor = osThreadNew(Task_monitor, NULL, &attr_monitor);
task_runtime.thread.blink = osThreadNew(Task_blink, NULL, &attr_blink);
task_runtime.thread.ctrl_shoot = osThreadNew(Task_ctrl_shoot, NULL, &attr_ctrl_shoot);
task_runtime.thread.ai = osThreadNew(Task_ai, NULL, &attr_ai);
// 创建消息队列
/* USER MESSAGE BEGIN */

12
User/task/rc.c
View File

@ -85,12 +85,20 @@ void Task_rc(void *argument) {
cmd_for_gimbal.delta_pit = 0.0f;
break;
case DR16_SW_MID:
cmd_for_gimbal.mode = GIMBAL_MODE_ABSOLUTE;
if (dr16.data.sw_r == DR16_SW_UP || dr16.data.sw_r == DR16_SW_ERR) {
cmd_for_gimbal.mode = GIMBAL_MODE_RELATIVE;
} else {
cmd_for_gimbal.mode = GIMBAL_MODE_AI_CONTROL;
}
cmd_for_gimbal.delta_yaw = -dr16.data.ch_r_x * 5.0f;
cmd_for_gimbal.delta_pit = dr16.data.ch_r_y * 5.0f;
break;
case DR16_SW_DOWN:
cmd_for_gimbal.mode = GIMBAL_MODE_ABSOLUTE;
if (dr16.data.sw_r == DR16_SW_UP || dr16.data.sw_r == DR16_SW_ERR) {
cmd_for_gimbal.mode = GIMBAL_MODE_RELATIVE;
} else {
cmd_for_gimbal.mode = GIMBAL_MODE_AI_CONTROL;
}
cmd_for_gimbal.delta_yaw = -dr16.data.ch_r_x * 5.0f;
cmd_for_gimbal.delta_pit = dr16.data.ch_r_y * 5.0f;
break;

11
User/task/user_task.c
View File

@ -22,12 +22,12 @@ const osThreadAttr_t attr_atti_esit = {
const osThreadAttr_t attr_ctrl_chassis = {
.name = "ctrl_chassis",
.priority = osPriorityNormal,
.stack_size = 256 * 8,
.stack_size = 256 * 4,
};
const osThreadAttr_t attr_ctrl_gimbal = {
.name = "ctrl_gimbal",
.priority = osPriorityNormal,
.stack_size = 256 * 8,
.stack_size = 256 * 4,
};
const osThreadAttr_t attr_monitor = {
.name = "monitor",
@ -42,5 +42,10 @@ const osThreadAttr_t attr_blink = {
const osThreadAttr_t attr_ctrl_shoot = {
.name = "ctrl_shoot",
.priority = osPriorityNormal,
.stack_size = 256 * 8,
.stack_size = 256 * 4,
};
const osThreadAttr_t attr_ai = {
.name = "ai",
.priority = osPriorityNormal,
.stack_size = 256 * 4,
};

9
User/task/user_task.h
View File

@ -19,6 +19,7 @@ extern "C" {
#define MONITOR_FREQ (500.0)
#define BLINK_FREQ (500.0)
#define CTRL_SHOOT_FREQ (500.0)
#define AI_FREQ (500.0)
/* 任务初始化延时ms */
#define TASK_INIT_DELAY (100u)
@ -29,6 +30,7 @@ extern "C" {
#define MONITOR_INIT_DELAY (0)
#define BLINK_INIT_DELAY (0)
#define CTRL_SHOOT_INIT_DELAY (0)
#define AI_INIT_DELAY (0)
/* Exported defines --------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
@ -45,6 +47,7 @@ typedef struct {
osThreadId_t monitor;
osThreadId_t blink;
osThreadId_t ctrl_shoot;
osThreadId_t ai;
} thread;
/* USER MESSAGE BEGIN */
@ -58,6 +61,7 @@ typedef struct {
struct {
osMessageQueueId_t imu;
osMessageQueueId_t cmd;
osMessageQueueId_t ai_cmd;
}gimbal;
struct {
osMessageQueueId_t shoot_cmd; /* 发射命令队列 */
@ -96,6 +100,7 @@ typedef struct {
UBaseType_t monitor;
UBaseType_t blink;
UBaseType_t ctrl_shoot;
UBaseType_t ai;
} stack_water_mark;
/* 各任务运行频率 */
@ -106,6 +111,7 @@ typedef struct {
float monitor;
float blink;
float ctrl_shoot;
float ai;
} freq;
/* 任务最近运行时间 */
@ -116,6 +122,7 @@ typedef struct {
float monitor;
float blink;
float ctrl_shoot;
float ai;
} last_up_time;
} Task_Runtime_t;
@ -132,6 +139,7 @@ extern const osThreadAttr_t attr_ctrl_gimbal;
extern const osThreadAttr_t attr_monitor;
extern const osThreadAttr_t attr_blink;
extern const osThreadAttr_t attr_ctrl_shoot;
extern const osThreadAttr_t attr_ai;
/* 任务函数声明 */
void Task_Init(void *argument);
@ -142,6 +150,7 @@ void Task_ctrl_gimbal(void *argument);
void Task_monitor(void *argument);
void Task_blink(void *argument);
void Task_ctrl_shoot(void *argument);
void Task_ai(void *argument);
#ifdef __cplusplus
}