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

改一下

Browse Source
This commit is contained in:
Robofish 2026-01-12 17:53:57 +08:00
parent 68d12f34eb
commit 63f9dac29e
10 changed files with 985 additions and 716 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
.mxproject
View File

File diff suppressed because one or more lines are too long

1
CMakeLists.txt
View File

@ -67,6 +67,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
# User/device sources # User/device sources
User/device/bmi088.c User/device/bmi088.c
User/device/buzzer.c User/device/buzzer.c
User/device/ai.c
# User/task sources # User/task sources
User/task/ai.c User/task/ai.c

2
Core/Inc/stm32f4xx_it.h
View File

@ -58,6 +58,7 @@ void EXTI3_IRQHandler(void);
void EXTI4_IRQHandler(void); void EXTI4_IRQHandler(void);
void DMA1_Stream1_IRQHandler(void); void DMA1_Stream1_IRQHandler(void);
void DMA1_Stream2_IRQHandler(void); void DMA1_Stream2_IRQHandler(void);
void CAN1_TX_IRQHandler(void);
void CAN1_RX0_IRQHandler(void); void CAN1_RX0_IRQHandler(void);
void CAN1_RX1_IRQHandler(void); void CAN1_RX1_IRQHandler(void);
void EXTI9_5_IRQHandler(void); void EXTI9_5_IRQHandler(void);
@ -68,6 +69,7 @@ void TIM7_IRQHandler(void);
void DMA2_Stream1_IRQHandler(void); void DMA2_Stream1_IRQHandler(void);
void DMA2_Stream2_IRQHandler(void); void DMA2_Stream2_IRQHandler(void);
void DMA2_Stream3_IRQHandler(void); void DMA2_Stream3_IRQHandler(void);
void CAN2_TX_IRQHandler(void);
void CAN2_RX0_IRQHandler(void); void CAN2_RX0_IRQHandler(void);
void CAN2_RX1_IRQHandler(void); void CAN2_RX1_IRQHandler(void);
void OTG_FS_IRQHandler(void); void OTG_FS_IRQHandler(void);

6
Core/Src/can.c
View File

@ -122,6 +122,8 @@ void HAL_CAN_MspInit(CAN_HandleTypeDef* canHandle)
HAL_GPIO_Init(GPIOD, &GPIO_InitStruct); HAL_GPIO_Init(GPIOD, &GPIO_InitStruct);
/* CAN1 interrupt Init */ /* CAN1 interrupt Init */
HAL_NVIC_SetPriority(CAN1_TX_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(CAN1_TX_IRQn);
HAL_NVIC_SetPriority(CAN1_RX0_IRQn, 5, 0); HAL_NVIC_SetPriority(CAN1_RX0_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(CAN1_RX0_IRQn); HAL_NVIC_EnableIRQ(CAN1_RX0_IRQn);
HAL_NVIC_SetPriority(CAN1_RX1_IRQn, 5, 0); HAL_NVIC_SetPriority(CAN1_RX1_IRQn, 5, 0);
@ -155,6 +157,8 @@ void HAL_CAN_MspInit(CAN_HandleTypeDef* canHandle)
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct); HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
/* CAN2 interrupt Init */ /* CAN2 interrupt Init */
HAL_NVIC_SetPriority(CAN2_TX_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(CAN2_TX_IRQn);
HAL_NVIC_SetPriority(CAN2_RX0_IRQn, 5, 0); HAL_NVIC_SetPriority(CAN2_RX0_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(CAN2_RX0_IRQn); HAL_NVIC_EnableIRQ(CAN2_RX0_IRQn);
HAL_NVIC_SetPriority(CAN2_RX1_IRQn, 5, 0); HAL_NVIC_SetPriority(CAN2_RX1_IRQn, 5, 0);
@ -186,6 +190,7 @@ void HAL_CAN_MspDeInit(CAN_HandleTypeDef* canHandle)
HAL_GPIO_DeInit(GPIOD, GPIO_PIN_0|GPIO_PIN_1); HAL_GPIO_DeInit(GPIOD, GPIO_PIN_0|GPIO_PIN_1);
/* CAN1 interrupt Deinit */ /* CAN1 interrupt Deinit */
HAL_NVIC_DisableIRQ(CAN1_TX_IRQn);
HAL_NVIC_DisableIRQ(CAN1_RX0_IRQn); HAL_NVIC_DisableIRQ(CAN1_RX0_IRQn);
HAL_NVIC_DisableIRQ(CAN1_RX1_IRQn); HAL_NVIC_DisableIRQ(CAN1_RX1_IRQn);
/* USER CODE BEGIN CAN1_MspDeInit 1 */ /* USER CODE BEGIN CAN1_MspDeInit 1 */
@ -211,6 +216,7 @@ void HAL_CAN_MspDeInit(CAN_HandleTypeDef* canHandle)
HAL_GPIO_DeInit(GPIOB, GPIO_PIN_5|GPIO_PIN_6); HAL_GPIO_DeInit(GPIOB, GPIO_PIN_5|GPIO_PIN_6);
/* CAN2 interrupt Deinit */ /* CAN2 interrupt Deinit */
HAL_NVIC_DisableIRQ(CAN2_TX_IRQn);
HAL_NVIC_DisableIRQ(CAN2_RX0_IRQn); HAL_NVIC_DisableIRQ(CAN2_RX0_IRQn);
HAL_NVIC_DisableIRQ(CAN2_RX1_IRQn); HAL_NVIC_DisableIRQ(CAN2_RX1_IRQn);
/* USER CODE BEGIN CAN2_MspDeInit 1 */ /* USER CODE BEGIN CAN2_MspDeInit 1 */

314
Core/Src/freertos.c
View File

@ -1,157 +1,157 @@
/* USER CODE BEGIN Header */ /* USER CODE BEGIN Header */
/** /**
****************************************************************************** ******************************************************************************
* File Name : freertos.c * File Name : freertos.c
* Description : Code for freertos applications * Description : Code for freertos applications
****************************************************************************** ******************************************************************************
* @attention * @attention
* *
* Copyright (c) 2026 STMicroelectronics. * Copyright (c) 2026 STMicroelectronics.
* All rights reserved. * All rights reserved.
* *
* This software is licensed under terms that can be found in the LICENSE file * This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component. * in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS. * If no LICENSE file comes with this software, it is provided AS-IS.
* *
****************************************************************************** ******************************************************************************
*/ */
/* USER CODE END Header */ /* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/ /* Includes ------------------------------------------------------------------*/
#include "FreeRTOS.h" #include "FreeRTOS.h"
#include "task.h" #include "task.h"
#include "main.h" #include "main.h"
#include "cmsis_os.h" #include "cmsis_os.h"
/* Private includes ----------------------------------------------------------*/ /* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */ /* USER CODE BEGIN Includes */
#include "task/user_task.h" #include "task/user_task.h"
/* USER CODE END Includes */ /* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/ /* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */ /* USER CODE BEGIN PTD */
/* USER CODE END PTD */ /* USER CODE END PTD */
/* Private define ------------------------------------------------------------*/ /* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */ /* USER CODE BEGIN PD */
/* USER CODE END PD */ /* USER CODE END PD */
/* Private macro -------------------------------------------------------------*/ /* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */ /* USER CODE BEGIN PM */
/* USER CODE END PM */ /* USER CODE END PM */
/* Private variables ---------------------------------------------------------*/ /* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN Variables */ /* USER CODE BEGIN Variables */
/* USER CODE END Variables */ /* USER CODE END Variables */
/* Definitions for defaultTask */ /* Definitions for defaultTask */
osThreadId_t defaultTaskHandle; osThreadId_t defaultTaskHandle;
const osThreadAttr_t defaultTask_attributes = { const osThreadAttr_t defaultTask_attributes = {
.name = "defaultTask", .name = "defaultTask",
.stack_size = 128 * 4, .stack_size = 128 * 4,
.priority = (osPriority_t) osPriorityNormal, .priority = (osPriority_t) osPriorityNormal,
}; };
/* Private function prototypes -----------------------------------------------*/ /* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN FunctionPrototypes */ /* USER CODE BEGIN FunctionPrototypes */
/* USER CODE END FunctionPrototypes */ /* USER CODE END FunctionPrototypes */
void StartDefaultTask(void *argument); void StartDefaultTask(void *argument);
extern void MX_USB_DEVICE_Init(void); extern void MX_USB_DEVICE_Init(void);
void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */ void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */
/* Hook prototypes */ /* Hook prototypes */
void configureTimerForRunTimeStats(void); void configureTimerForRunTimeStats(void);
unsigned long getRunTimeCounterValue(void); unsigned long getRunTimeCounterValue(void);
void vApplicationStackOverflowHook(TaskHandle_t xTask, signed char *pcTaskName); void vApplicationStackOverflowHook(TaskHandle_t xTask, signed char *pcTaskName);
/* USER CODE BEGIN 1 */ /* USER CODE BEGIN 1 */
/* Functions needed when configGENERATE_RUN_TIME_STATS is on */ /* Functions needed when configGENERATE_RUN_TIME_STATS is on */
__weak void configureTimerForRunTimeStats(void) __weak void configureTimerForRunTimeStats(void)
{ {
} }
__weak unsigned long getRunTimeCounterValue(void) __weak unsigned long getRunTimeCounterValue(void)
{ {
return 0; return 0;
} }
/* USER CODE END 1 */ /* USER CODE END 1 */
/* USER CODE BEGIN 4 */ /* USER CODE BEGIN 4 */
void vApplicationStackOverflowHook(TaskHandle_t xTask, signed char *pcTaskName) void vApplicationStackOverflowHook(TaskHandle_t xTask, signed char *pcTaskName)
{ {
/* Run time stack overflow checking is performed if /* Run time stack overflow checking is performed if
configCHECK_FOR_STACK_OVERFLOW is defined to 1 or 2. This hook function is configCHECK_FOR_STACK_OVERFLOW is defined to 1 or 2. This hook function is
called if a stack overflow is detected. */ called if a stack overflow is detected. */
} }
/* USER CODE END 4 */ /* USER CODE END 4 */
/** /**
* @brief FreeRTOS initialization * @brief FreeRTOS initialization
* @param None * @param None
* @retval None * @retval None
*/ */
void MX_FREERTOS_Init(void) { void MX_FREERTOS_Init(void) {
/* USER CODE BEGIN Init */ /* USER CODE BEGIN Init */
/* USER CODE END Init */ /* USER CODE END Init */
/* USER CODE BEGIN RTOS_MUTEX */ /* USER CODE BEGIN RTOS_MUTEX */
/* add mutexes, ... */ /* add mutexes, ... */
/* USER CODE END RTOS_MUTEX */ /* USER CODE END RTOS_MUTEX */
/* USER CODE BEGIN RTOS_SEMAPHORES */ /* USER CODE BEGIN RTOS_SEMAPHORES */
/* add semaphores, ... */ /* add semaphores, ... */
/* USER CODE END RTOS_SEMAPHORES */ /* USER CODE END RTOS_SEMAPHORES */
/* USER CODE BEGIN RTOS_TIMERS */ /* USER CODE BEGIN RTOS_TIMERS */
/* start timers, add new ones, ... */ /* start timers, add new ones, ... */
/* USER CODE END RTOS_TIMERS */ /* USER CODE END RTOS_TIMERS */
/* USER CODE BEGIN RTOS_QUEUES */ /* USER CODE BEGIN RTOS_QUEUES */
/* add queues, ... */ /* add queues, ... */
/* USER CODE END RTOS_QUEUES */ /* USER CODE END RTOS_QUEUES */
/* Create the thread(s) */ /* Create the thread(s) */
/* creation of defaultTask */ /* creation of defaultTask */
defaultTaskHandle = osThreadNew(StartDefaultTask, NULL, &defaultTask_attributes); defaultTaskHandle = osThreadNew(StartDefaultTask, NULL, &defaultTask_attributes);
/* USER CODE BEGIN RTOS_THREADS */ /* USER CODE BEGIN RTOS_THREADS */
/* add threads, ... */ /* add threads, ... */
osThreadNew(Task_Init, NULL, &attr_init); // 创建初始化任务 osThreadNew(Task_Init, NULL, &attr_init); // 创建初始化任务
/* USER CODE END RTOS_THREADS */ /* USER CODE END RTOS_THREADS */
/* USER CODE BEGIN RTOS_EVENTS */ /* USER CODE BEGIN RTOS_EVENTS */
/* add events, ... */ /* add events, ... */
/* USER CODE END RTOS_EVENTS */ /* USER CODE END RTOS_EVENTS */
} }
/* USER CODE BEGIN Header_StartDefaultTask */ /* USER CODE BEGIN Header_StartDefaultTask */
/** /**
* @brief Function implementing the defaultTask thread. * @brief Function implementing the defaultTask thread.
* @param argument: Not used * @param argument: Not used
* @retval None * @retval None
*/ */
/* USER CODE END Header_StartDefaultTask */ /* USER CODE END Header_StartDefaultTask */
void StartDefaultTask(void *argument) void StartDefaultTask(void *argument)
{ {
/* init code for USB_DEVICE */ /* init code for USB_DEVICE */
MX_USB_DEVICE_Init(); MX_USB_DEVICE_Init();
/* USER CODE BEGIN StartDefaultTask */ /* USER CODE BEGIN StartDefaultTask */
osThreadTerminate(osThreadGetId()); osThreadTerminate(osThreadGetId());
/* USER CODE END StartDefaultTask */ /* USER CODE END StartDefaultTask */
} }
/* Private application code --------------------------------------------------*/ /* Private application code --------------------------------------------------*/
/* USER CODE BEGIN Application */ /* USER CODE BEGIN Application */
/* USER CODE END Application */ /* USER CODE END Application */

1052
Core/Src/stm32f4xx_it.c
View File

File diff suppressed because it is too large Load Diff

2
DevC.ioc
View File

@ -270,8 +270,10 @@ MxDb.Version=DB.6.0.150
NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
NVIC.CAN1_RX0_IRQn=true\:5\:0\:true\:false\:true\:true\:true\:true\:true NVIC.CAN1_RX0_IRQn=true\:5\:0\:true\:false\:true\:true\:true\:true\:true
NVIC.CAN1_RX1_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true NVIC.CAN1_RX1_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
NVIC.CAN1_TX_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
NVIC.CAN2_RX0_IRQn=true\:5\:0\:true\:false\:true\:true\:true\:true\:true NVIC.CAN2_RX0_IRQn=true\:5\:0\:true\:false\:true\:true\:true\:true\:true
NVIC.CAN2_RX1_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true NVIC.CAN2_RX1_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
NVIC.CAN2_TX_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
NVIC.DMA1_Stream1_IRQn=true\:5\:0\:true\:false\:true\:true\:false\:true\:true NVIC.DMA1_Stream1_IRQn=true\:5\:0\:true\:false\:true\:true\:false\:true\:true
NVIC.DMA1_Stream2_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true NVIC.DMA1_Stream2_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
NVIC.DMA1_Stream7_IRQn=true\:5\:0\:true\:false\:true\:true\:false\:true\:true NVIC.DMA1_Stream7_IRQn=true\:5\:0\:true\:false\:true\:true\:false\:true\:true

152
User/device/ai.c Normal file
View File

@ -0,0 +1,152 @@
/*
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;
}
}

75
User/device/ai.h Normal file
View File

@ -0,0 +1,75 @@
/*
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/user_math.h"
#include "component/filter.h"
#include "device/device.h"
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
/* AI命令状态 */
typedef struct __packed {
float vx;
float vy;
float wz;
} AI_ChassisMoveVec_t;
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

81
User/task/imu.c
View File

@ -44,13 +44,13 @@ static AHRS_All_t imu_data;
/** /**
* @brief 浮点数转换为无符号整数函数 * @brief 浮点数转换为无符号整数函数
*/ */
static uint16_t float_to_uint(float x, float x_min, float x_max, int bits) { static uint32_t float_to_uint(float x, float x_min, float x_max, int bits) {
float span = x_max - x_min; float span = x_max - x_min;
float offset = x_min; float offset = x_min;
// 限幅 // 限幅
if (x > x_max) x = x_max; if (x > x_max) x = x_max;
if (x < x_min) x = x_min; if (x < x_min) x = x_min;
return (uint16_t)((x - offset) * ((float)((1 << bits) - 1)) / span); return (uint32_t)((x - offset) * ((float)((1 << bits) - 1)) / span);
} }
/* Exported functions ------------------------------------------------------- */ /* Exported functions ------------------------------------------------------- */
@ -65,7 +65,7 @@ void Task_imu(void *argument) {
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
/* USER CODE INIT BEGIN */ /* USER CODE INIT BEGIN */
BSP_CAN_Init();
/* USER CODE INIT END */ /* USER CODE INIT END */
while (1) { while (1) {
@ -75,52 +75,55 @@ void Task_imu(void *argument) {
if (osMessageQueueGet(task_runtime.msgq.gimbal_imu, &imu_data, NULL, 0) == osOK) { if (osMessageQueueGet(task_runtime.msgq.gimbal_imu, &imu_data, NULL, 0) == osOK) {
BSP_CAN_StdDataFrame_t can_frame; BSP_CAN_StdDataFrame_t can_frame;
/* 包1: 加速度计数据 (temp, acc_x, acc_y, acc_z) */ /* 包1: 加速度计数据 (acc_x, acc_y, acc_z) - 使用21位精度 */
can_frame.id = CAN_ID_IMU_ACCEL; can_frame.id = CAN_ID_IMU_ACCEL;
can_frame.dlc = 8; can_frame.dlc = 8;
can_frame.data[0] = 0; // 预留 uint32_t acc_x = float_to_uint(imu_data.accl.x, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 21);
can_frame.data[1] = 0; // 温度数据（如需要可添加） uint32_t acc_y = float_to_uint(imu_data.accl.y, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 21);
uint16_t acc_x = float_to_uint(imu_data.accl.x, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16); uint32_t acc_z = float_to_uint(imu_data.accl.z, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 21);
uint16_t acc_y = float_to_uint(imu_data.accl.y, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16); // 打包: acc_x(21bit) acc_y(21bit) acc_z(21bit) = 63bits
uint16_t acc_z = float_to_uint(imu_data.accl.z, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16); can_frame.data[0] = (acc_x >> 13) & 0xFF;
can_frame.data[2] = acc_x & 0xFF; can_frame.data[1] = (acc_x >> 5) & 0xFF;
can_frame.data[3] = (acc_x >> 8) & 0xFF; can_frame.data[2] = ((acc_x << 3) & 0xF8) | ((acc_y >> 18) & 0x07);
can_frame.data[4] = acc_y & 0xFF; can_frame.data[3] = (acc_y >> 10) & 0xFF;
can_frame.data[5] = (acc_y >> 8) & 0xFF; can_frame.data[4] = (acc_y >> 2) & 0xFF;
can_frame.data[6] = acc_z & 0xFF; can_frame.data[5] = ((acc_y << 6) & 0xC0) | ((acc_z >> 15) & 0x3F);
can_frame.data[7] = (acc_z >> 8) & 0xFF; can_frame.data[6] = (acc_z >> 7) & 0xFF;
can_frame.data[7] = (acc_z << 1) & 0xFE;
BSP_CAN_TransmitStdDataFrame(BSP_CAN_2, &can_frame); BSP_CAN_TransmitStdDataFrame(BSP_CAN_2, &can_frame);
/* 包2: 陀螺仪数据 (gyro_x, gyro_y, gyro_z) */ /* 包2: 陀螺仪数据 (gyro_x, gyro_y, gyro_z) - 使用21位精度 */
can_frame.id = CAN_ID_IMU_GYRO; can_frame.id = CAN_ID_IMU_GYRO;
can_frame.dlc = 8; can_frame.dlc = 8;
can_frame.data[0] = 0; // 预留 uint32_t gyro_x = float_to_uint(imu_data.gyro.x, GYRO_CAN_MIN, GYRO_CAN_MAX, 21);
can_frame.data[1] = 0; // 预留 uint32_t gyro_y = float_to_uint(imu_data.gyro.y, GYRO_CAN_MIN, GYRO_CAN_MAX, 21);
uint16_t gyro_x = float_to_uint(imu_data.gyro.x, GYRO_CAN_MIN, GYRO_CAN_MAX, 16); uint32_t gyro_z = float_to_uint(imu_data.gyro.z, GYRO_CAN_MIN, GYRO_CAN_MAX, 21);
uint16_t gyro_y = float_to_uint(imu_data.gyro.y, GYRO_CAN_MIN, GYRO_CAN_MAX, 16); // 打包: gyro_x(21bit) gyro_y(21bit) gyro_z(21bit) = 63bits
uint16_t gyro_z = float_to_uint(imu_data.gyro.z, GYRO_CAN_MIN, GYRO_CAN_MAX, 16); can_frame.data[0] = (gyro_x >> 13) & 0xFF;
can_frame.data[2] = gyro_x & 0xFF; can_frame.data[1] = (gyro_x >> 5) & 0xFF;
can_frame.data[3] = (gyro_x >> 8) & 0xFF; can_frame.data[2] = ((gyro_x << 3) & 0xF8) | ((gyro_y >> 18) & 0x07);
can_frame.data[4] = gyro_y & 0xFF; can_frame.data[3] = (gyro_y >> 10) & 0xFF;
can_frame.data[5] = (gyro_y >> 8) & 0xFF; can_frame.data[4] = (gyro_y >> 2) & 0xFF;
can_frame.data[6] = gyro_z & 0xFF; can_frame.data[5] = ((gyro_y << 6) & 0xC0) | ((gyro_z >> 15) & 0x3F);
can_frame.data[7] = (gyro_z >> 8) & 0xFF; can_frame.data[6] = (gyro_z >> 7) & 0xFF;
can_frame.data[7] = (gyro_z << 1) & 0xFE;
BSP_CAN_TransmitStdDataFrame(BSP_CAN_2, &can_frame); BSP_CAN_TransmitStdDataFrame(BSP_CAN_2, &can_frame);
/* 包3: 欧拉角数据 (pit, yaw, rol) */ /* 包3: 欧拉角数据 (pit, yaw, rol) - 使用21位精度 */
can_frame.id = CAN_ID_IMU_EULER; can_frame.id = CAN_ID_IMU_EULER;
can_frame.dlc = 8; can_frame.dlc = 8;
can_frame.data[0] = 0; // 预留 uint32_t pit = float_to_uint(imu_data.eulr.pit, PITCH_CAN_MIN, PITCH_CAN_MAX, 21);
can_frame.data[1] = 0; // 预留 uint32_t yaw = float_to_uint(imu_data.eulr.yaw, YAW_CAN_MIN, YAW_CAN_MAX, 21);
uint16_t pit = float_to_uint(imu_data.eulr.pit, PITCH_CAN_MIN, PITCH_CAN_MAX, 16); uint32_t rol = float_to_uint(imu_data.eulr.rol, ROLL_CAN_MIN, ROLL_CAN_MAX, 21);
uint16_t yaw = float_to_uint(imu_data.eulr.yaw, YAW_CAN_MIN, YAW_CAN_MAX, 16); // 打包: pit(21bit) yaw(21bit) rol(21bit) = 63bits
uint16_t rol = float_to_uint(imu_data.eulr.rol, ROLL_CAN_MIN, ROLL_CAN_MAX, 16); can_frame.data[0] = (pit >> 13) & 0xFF;
can_frame.data[2] = pit & 0xFF; can_frame.data[1] = (pit >> 5) & 0xFF;
can_frame.data[3] = (pit >> 8) & 0xFF; can_frame.data[2] = ((pit << 3) & 0xF8) | ((yaw >> 18) & 0x07);
can_frame.data[4] = yaw & 0xFF; can_frame.data[3] = (yaw >> 10) & 0xFF;
can_frame.data[5] = (yaw >> 8) & 0xFF; can_frame.data[4] = (yaw >> 2) & 0xFF;
can_frame.data[6] = rol & 0xFF; can_frame.data[5] = ((yaw << 6) & 0xC0) | ((rol >> 15) & 0x3F);
can_frame.data[7] = (rol >> 8) & 0xFF; can_frame.data[6] = (rol >> 7) & 0xFF;
can_frame.data[7] = (rol << 1) & 0xFE;
BSP_CAN_TransmitStdDataFrame(BSP_CAN_2, &can_frame); BSP_CAN_TransmitStdDataFrame(BSP_CAN_2, &can_frame);
/* 包4: 四元数数据 (q0, q1, q2, q3) - 使用14位精度打包 */ /* 包4: 四元数数据 (q0, q1, q2, q3) - 使用14位精度打包 */