From 7ac7f7d8689bb3eaf3e378c58b1504a6e194fd12 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sun, 5 Oct 2025 00:03:38 +0800 Subject: [PATCH] =?UTF-8?q?=E6=94=B9=E5=8F=91=E9=80=81?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/task/imu.c | 105 +++++++++++++++++++++++++----------------------- 1 file changed, 55 insertions(+), 50 deletions(-) diff --git a/User/task/imu.c b/User/task/imu.c index 02f99ea..1d782fe 100644 --- a/User/task/imu.c +++ b/User/task/imu.c @@ -1,6 +1,6 @@ /* imu Task - + */ /* Includes ----------------------------------------------------------------- */ @@ -18,7 +18,7 @@ /* Private variables -------------------------------------------------------- */ /* USER STRUCT BEGIN */ DM_IMU_t dm_imu; -int i= 0; +int i = 0; /* USER STRUCT END */ /* Private function --------------------------------------------------------- */ @@ -26,7 +26,6 @@ int i= 0; void Task_imu(void *argument) { (void)argument; /* 未使用argument，消除警告 */ - /* 计算任务运行到指定频率需要等待的tick数 */ const uint32_t delay_tick = osKernelGetTickFreq() / IMU_FREQ; @@ -37,96 +36,102 @@ void Task_imu(void *argument) { BSP_CAN_Init(); DM_IMU_Init(&dm_imu, &Config_GetRobotParam()->imu_param); /* USER CODE INIT END */ - + while (1) { tick += delay_tick; /* 计算下一个唤醒时刻 */ i++; /* USER CODE BEGIN */ - if (DM_IMU_AutoUpdateAll(&dm_imu) == DEVICE_OK) { - } - if (i>1){ - i=0; -// 发送加速度计数据 (ID: 0x66) - 三轴压缩到一帧，每轴2字节，精度0.01g - BSP_CAN_StdDataFrame_t accl_frame = { - .id = 150, - .dlc = 8, - .data = {0} - }; - + DM_IMU_AutoUpdateAll(&dm_imu); + switch (i) { + case 0: { + // 发送加速度计数据 (ID: 0x66) - 三轴压缩到一帧，每轴2字节，精度0.01g + BSP_CAN_StdDataFrame_t accl_frame = {.id = 150, .dlc = 8, .data = {0}}; + // 转换为16位整数发送 (精度0.01g，范围±327.67g) int16_t accl_x_int = (int16_t)(dm_imu.data.accl.x * 100.0f); int16_t accl_y_int = (int16_t)(dm_imu.data.accl.y * 100.0f); int16_t accl_z_int = (int16_t)(dm_imu.data.accl.z * 100.0f); - + // 打包数据：x(2字节) + y(2字节) + z(2字节) + 2字节保留 memcpy(&accl_frame.data[0], &accl_x_int, 2); memcpy(&accl_frame.data[2], &accl_y_int, 2); memcpy(&accl_frame.data[4], &accl_z_int, 2); - + BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &accl_frame); - + + } + + break; + case 1: { // 发送陀螺仪数据 (ID: 0x67) - 三轴压缩到一帧，每轴2字节，精度0.01°/s - BSP_CAN_StdDataFrame_t gyro_frame = { - .id = 151, - .dlc = 8, - .data = {0} - }; - + BSP_CAN_StdDataFrame_t gyro_frame = {.id = 151, .dlc = 8, .data = {0}}; + // 转换为16位整数发送 (精度0.01°/s，范围±327.67°/s) - int16_t gyro_x_int = (int16_t)(dm_imu.data.gyro.x * 57.2958f * 100.0f); // 弧度/s转角度/s*100 + int16_t gyro_x_int = (int16_t)(dm_imu.data.gyro.x * 57.2958f * + 100.0f); // 弧度/s转角度/s*100 int16_t gyro_y_int = (int16_t)(dm_imu.data.gyro.y * 57.2958f * 100.0f); int16_t gyro_z_int = (int16_t)(dm_imu.data.gyro.z * 57.2958f * 100.0f); - + // 打包数据：x(2字节) + y(2字节) + z(2字节) + 2字节保留 memcpy(&gyro_frame.data[0], &gyro_x_int, 2); memcpy(&gyro_frame.data[2], &gyro_y_int, 2); memcpy(&gyro_frame.data[4], &gyro_z_int, 2); - + BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &gyro_frame); - + + break; + } + + case 2: { // 发送欧拉角数据 (ID: 0x68) - 三轴压缩到一帧，每轴2字节，精度0.01度 - BSP_CAN_StdDataFrame_t euler_frame = { - .id = 152, - .dlc = 8, - .data = {0} - }; - + BSP_CAN_StdDataFrame_t euler_frame = {.id = 152, .dlc = 8, .data = {0}}; + // 转换为16位整数发送 (精度0.01度，范围±327.67度) - int16_t yaw_int = (int16_t)(dm_imu.data.euler.yaw * 57.2958f * 100.0f); // 弧度转角度*100 - int16_t pit_int = (int16_t)(dm_imu.data.euler.pit * 57.2958f * 100.0f); // 弧度转角度*100 - int16_t rol_int = (int16_t)(dm_imu.data.euler.rol * 57.2958f * 100.0f); // 弧度转角度*100 - + int16_t yaw_int = (int16_t)(dm_imu.data.euler.yaw * 57.2958f * + 100.0f); // 弧度转角度*100 + int16_t pit_int = (int16_t)(dm_imu.data.euler.pit * 57.2958f * + 100.0f); // 弧度转角度*100 + int16_t rol_int = (int16_t)(dm_imu.data.euler.rol * 57.2958f * + 100.0f); // 弧度转角度*100 + // 打包数据：yaw(2字节) + pitch(2字节) + roll(2字节) + 2字节保留 memcpy(&euler_frame.data[0], &yaw_int, 2); memcpy(&euler_frame.data[2], &pit_int, 2); memcpy(&euler_frame.data[4], &rol_int, 2); - + BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &euler_frame); - + + } + + break; + case 3: { // 发送四元数数据 (ID: 0x69) - 四分量压缩到一帧，每分量2字节，精度0.0001 - BSP_CAN_StdDataFrame_t quat_frame = { - .id = 153, - .dlc = 8, - .data = {0} - }; - + BSP_CAN_StdDataFrame_t quat_frame = {.id = 153, .dlc = 8, .data = {0}}; + // 转换为16位整数发送 (精度0.0001，范围±3.2767) int16_t q0_int = (int16_t)(dm_imu.data.quat.q0 * 10000.0f); int16_t q1_int = (int16_t)(dm_imu.data.quat.q1 * 10000.0f); int16_t q2_int = (int16_t)(dm_imu.data.quat.q2 * 10000.0f); int16_t q3_int = (int16_t)(dm_imu.data.quat.q3 * 10000.0f); - + // 打包数据：q0(2字节) + q1(2字节) + q2(2字节) + q3(2字节) memcpy(&quat_frame.data[0], &q0_int, 2); memcpy(&quat_frame.data[2], &q1_int, 2); memcpy(&quat_frame.data[4], &q2_int, 2); memcpy(&quat_frame.data[6], &q3_int, 2); - - BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &quat_frame); + + BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &quat_frame); } + + i = 0; + break; + default: + i = 0; + break; + } + /* USER CODE END */ osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ } - } \ No newline at end of file