From 8ccffa1b1ae90bee160e2136f600280ed3c100f8 Mon Sep 17 00:00:00 2001 From: yxming66 <2389287465@qq.com> Date: Thu, 8 Jan 2026 21:36:30 +0800 Subject: [PATCH] bc --- User/task/ai.c | 52 +++++++++++++++++++++---------------------- User/task/atti_esti.c | 2 +- User/task/cmd.c | 39 +++++++++++++++++++++++++++++++- User/task/init.c | 2 +- 4 files changed, 66 insertions(+), 29 deletions(-) diff --git a/User/task/ai.c b/User/task/ai.c index 5ae142a..f7c4611 100644 --- a/User/task/ai.c +++ b/User/task/ai.c @@ -7,10 +7,10 @@ #include "task/user_task.h" /* USER INCLUDE BEGIN */ -#include "module/vision_bridge.h" -#include "component/ahrs/ahrs.h" -#include "module/gimbal.h" -#include "module/shoot.h" +// #include "module/vision_bridge.h" +// #include "component/ahrs/ahrs.h" +// #include "module/gimbal.h" +// #include "module/shoot.h" /* USER INCLUDE END */ /* Private typedef ---------------------------------------------------------- */ @@ -18,12 +18,12 @@ /* Private macro ------------------------------------------------------------ */ /* Private variables -------------------------------------------------------- */ /* USER STRUCT BEGIN */ -AI_t ai; -AI_cmd_t aiToCmd; +// AI_t ai; +// AI_cmd_t aiToCmd; -AI_RawData_t rawdata; -AHRS_Quaternion_t quat_from_imu; -Gimbal_Feedback_t rawdata_from_gimbal; +// AI_RawData_t rawdata; +// AHRS_Quaternion_t quat_from_imu; +// Gimbal_Feedback_t rawdata_from_gimbal; /* USER STRUCT END */ /* Private function --------------------------------------------------------- */ @@ -46,24 +46,24 @@ void Task_ai(void *argument) { tick += delay_tick; /* 计算下一个唤醒时刻 */ /* USER CODE BEGIN */ - osMessageQueueGet(task_runtime.msgq.ai.rawdata_FromGimbal, &rawdata_from_gimbal, NULL, 0); - osMessageQueueGet(task_runtime.msgq.ai.rawdata_FromIMU, &quat_from_imu, NULL, 0); - rawdata.mode=1; - rawdata.pitch=rawdata_from_gimbal.motor.pit.rotor_abs_angle; - rawdata.yaw=rawdata_from_gimbal.motor.yaw.rotor_abs_angle; - rawdata.pitch_vel=rawdata_from_gimbal.motor.pit.rotor_speed; - rawdata.yaw_vel=rawdata_from_gimbal.motor.yaw.rotor_speed; - rawdata.q[0]=quat_from_imu.q0; - rawdata.q[1]=quat_from_imu.q1; - rawdata.q[2]=quat_from_imu.q2; - rawdata.q[3]=quat_from_imu.q3; - AI_ParseForHost(&ai,&rawdata); - AI_StartSend(&ai); + // osMessageQueueGet(task_runtime.msgq.ai.rawdata_FromGimbal, &rawdata_from_gimbal, NULL, 0); + // osMessageQueueGet(task_runtime.msgq.ai.rawdata_FromIMU, &quat_from_imu, NULL, 0); + // rawdata.mode=1; + // rawdata.pitch=rawdata_from_gimbal.motor.pit.rotor_abs_angle; + // rawdata.yaw=rawdata_from_gimbal.motor.yaw.rotor_abs_angle; + // rawdata.pitch_vel=rawdata_from_gimbal.motor.pit.rotor_speed; + // rawdata.yaw_vel=rawdata_from_gimbal.motor.yaw.rotor_speed; + // rawdata.q[0]=quat_from_imu.q0; + // rawdata.q[1]=quat_from_imu.q1; + // rawdata.q[2]=quat_from_imu.q2; + // rawdata.q[3]=quat_from_imu.q3; + // AI_ParseForHost(&ai,&rawdata); + // AI_StartSend(&ai); - AI_StartReceiving(&ai); - AI_Get(&ai,&aiToCmd); - osMessageQueueReset(task_runtime.msgq.cmd.nuc); - osMessageQueuePut(task_runtime.msgq.cmd.nuc, &aiToCmd, 0, 0); + // AI_StartReceiving(&ai); + // AI_Get(&ai,&aiToCmd); + // osMessageQueueReset(task_runtime.msgq.cmd.nuc); + // osMessageQueuePut(task_runtime.msgq.cmd.nuc, &aiToCmd, 0, 0); /* USER CODE END */ osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ } diff --git a/User/task/atti_esti.c b/User/task/atti_esti.c index eb5cbe8..1bff0af 100644 --- a/User/task/atti_esti.c +++ b/User/task/atti_esti.c @@ -21,7 +21,7 @@ /* Private typedef ---------------------------------------------------------- */ /* Private define ----------------------------------------------------------- */ -#define ATTI_EULR_CAN_ID (0x321u) +#define ATTI_EULR_CAN_ID (0x100u) #define ATTI_EULR_SCALE (10000.0f) /* 0.0001 rad per LSB */ #define ATTI_EULR_CAN_DLC (6u) /* Private macro ------------------------------------------------------------ */ diff --git a/User/task/cmd.c b/User/task/cmd.c index a576820..3cabccf 100644 --- a/User/task/cmd.c +++ b/User/task/cmd.c @@ -14,11 +14,15 @@ #include "module/shoot.h" #include "module/track.h" #include "module/cmd/cmd.h" +#include "bsp/fdcan.h" //#define DEAD_AREA 0.05f /* USER INCLUDE END */ /* Private typedef ---------------------------------------------------------- */ /* Private define ----------------------------------------------------------- */ +#define AI_CMD_CAN_ID (0x101u) +#define AI_CMD_CAN_DLC (5u) /* 1字节mode + 2字节yaw + 2字节pit */ +#define AI_CMD_ANGLE_SCALE (10000.0f) /* 0.0001 rad per LSB */ /* Private macro ------------------------------------------------------------ */ /* Private variables -------------------------------------------------------- */ /* USER STRUCT BEGIN */ @@ -37,6 +41,7 @@ static CMD_t cmd; /* USER STRUCT END */ /* Private function --------------------------------------------------------- */ +static void AI_ParseCmdFromCan(const BSP_FDCAN_Message_t *msg, AI_cmd_t *cmd); /* Exported functions ------------------------------------------------------- */ void Task_cmd(void *argument) { (void)argument; /* 未使用argument，消除警告 */ @@ -50,6 +55,9 @@ void Task_cmd(void *argument) { uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ /* USER CODE INIT BEGIN */ CMD_Init(&cmd, &Config_GetRobotParam()->cmd_param); + + /* 注册CAN接收ID */ + BSP_FDCAN_RegisterId(BSP_FDCAN_2, AI_CMD_CAN_ID, BSP_FDCAN_DEFAULT_QUEUE_SIZE); /* USER CODE INIT END */ while (1) { @@ -60,8 +68,13 @@ void Task_cmd(void *argument) { #elif CMD_RCTypeTable_Index == 1 osMessageQueueGet(task_runtime.msgq.cmd.rc, &cmd_at9s, NULL, 0); #endif - osMessageQueueGet(task_runtime.msgq.cmd.nuc, &cmd_ai, NULL, 0); + /* 从CAN2接收AI命令 */ + BSP_FDCAN_Message_t can_msg; + if (BSP_FDCAN_GetMessage(BSP_FDCAN_2, AI_CMD_CAN_ID, &can_msg, BSP_FDCAN_TIMEOUT_IMMEDIATE) == 0) { + AI_ParseCmdFromCan(&can_msg, &cmd_ai); + } + CMD_Update(&cmd); /* 获取命令发送到各模块 */ @@ -81,4 +94,28 @@ void Task_cmd(void *argument) { osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ } +} + +/* 从CAN消息解析AI命令 (mode + yaw + pit) */ +static void AI_ParseCmdFromCan(const BSP_FDCAN_Message_t *msg, AI_cmd_t *cmd) { + if (msg == NULL || cmd == NULL || msg->dlc < AI_CMD_CAN_DLC) { + return; + } + + /* 解析mode (1字节) */ + cmd->mode = msg->data[0]; + + /* 解析yaw (2字节，高字节在前) */ + int16_t yaw_raw = (int16_t)((msg->data[1] << 8) | msg->data[2]); + cmd->gimbal.setpoint.yaw = (float)yaw_raw / AI_CMD_ANGLE_SCALE; + + /* 解析pit (2字节，高字节在前) */ + int16_t pit_raw = (int16_t)((msg->data[3] << 8) | msg->data[4]); + cmd->gimbal.setpoint.pit = (float)pit_raw / AI_CMD_ANGLE_SCALE; + + /* 其他字段根据需要可以初始化为0 */ + cmd->gimbal.vel.yaw = 0.0f; + cmd->gimbal.vel.pit = 0.0f; + cmd->gimbal.accl.yaw = 0.0f; + cmd->gimbal.accl.pit = 0.0f; } \ No newline at end of file diff --git a/User/task/init.c b/User/task/init.c index 0bde494..e778d31 100644 --- a/User/task/init.c +++ b/User/task/init.c @@ -43,7 +43,7 @@ void Task_Init(void *argument) { task_runtime.thread.ctrl_shoot = osThreadNew(Task_ctrl_shoot, NULL, &attr_ctrl_shoot); task_runtime.thread.cmd = osThreadNew(Task_cmd, NULL, &attr_cmd); task_runtime.thread.supercap = osThreadNew(Task_supercap, NULL, &attr_supercap); - task_runtime.thread.ai = osThreadNew(Task_ai, NULL, &attr_ai); + // task_runtime.thread.ai = osThreadNew(Task_ai, NULL, &attr_ai); // 创建消息队列 /* USER MESSAGE BEGIN */