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/bmi088.c
User/device/buzzer.c
User/device/ai.c
# User/task sources
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 DMA1_Stream1_IRQHandler(void);
void DMA1_Stream2_IRQHandler(void);
void CAN1_TX_IRQHandler(void);
void CAN1_RX0_IRQHandler(void);
void CAN1_RX1_IRQHandler(void);
void EXTI9_5_IRQHandler(void);
@ -68,6 +69,7 @@ void TIM7_IRQHandler(void);
void DMA2_Stream1_IRQHandler(void);
void DMA2_Stream2_IRQHandler(void);
void DMA2_Stream3_IRQHandler(void);
void CAN2_TX_IRQHandler(void);
void CAN2_RX0_IRQHandler(void);
void CAN2_RX1_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);
/* 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_EnableIRQ(CAN1_RX0_IRQn);
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);
/* 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_EnableIRQ(CAN2_RX0_IRQn);
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);
/* CAN1 interrupt Deinit */
HAL_NVIC_DisableIRQ(CAN1_TX_IRQn);
HAL_NVIC_DisableIRQ(CAN1_RX0_IRQn);
HAL_NVIC_DisableIRQ(CAN1_RX1_IRQn);
/* 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);
/* CAN2 interrupt Deinit */
HAL_NVIC_DisableIRQ(CAN2_TX_IRQn);
HAL_NVIC_DisableIRQ(CAN2_RX0_IRQn);
HAL_NVIC_DisableIRQ(CAN2_RX1_IRQn);
/* USER CODE BEGIN CAN2_MspDeInit 1 */

314
Core/Src/freertos.c
View File

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

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