/* imu Task */ /* Includes ----------------------------------------------------------------- */ #include "task/user_task.h" /* USER INCLUDE BEGIN */ #include "bsp/can.h" #include "component/ahrs/ahrs.h" #include /* USER INCLUDE END */ /* Private typedef ---------------------------------------------------------- */ /* Private define ----------------------------------------------------------- */ /* CAN ID定义 - 使用标准帧 */ #define CAN_ID_IMU_ACCEL 0x100 /* 加速度计数据 */ #define CAN_ID_IMU_GYRO 0x101 /* 陀螺仪数据 */ #define CAN_ID_IMU_EULER 0x102 /* 欧拉角数据 */ #define CAN_ID_IMU_QUAT 0x103 /* 四元数数据 */ /* 数据范围定义（与DM_IMU解析一致） */ #define ACCEL_CAN_MAX (58.8f) #define ACCEL_CAN_MIN (-58.8f) #define GYRO_CAN_MAX (34.88f) #define GYRO_CAN_MIN (-34.88f) #define PITCH_CAN_MAX (90.0f) #define PITCH_CAN_MIN (-90.0f) #define ROLL_CAN_MAX (180.0f) #define ROLL_CAN_MIN (-180.0f) #define YAW_CAN_MAX (180.0f) #define YAW_CAN_MIN (-180.0f) #define QUAT_MIN (-1.0f) #define QUAT_MAX (1.0f) /* Private macro ------------------------------------------------------------ */ /* Private variables -------------------------------------------------------- */ /* USER STRUCT BEGIN */ static AHRS_All_t imu_data; /* USER STRUCT END */ /* Private function --------------------------------------------------------- */ /** * @brief 浮点数转换为无符号整数函数 */ 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 (uint32_t)((x - offset) * ((float)((1 << bits) - 1)) / span); } /* Exported functions ------------------------------------------------------- */ void Task_imu(void *argument) { (void)argument; /* 未使用argument，消除警告 */ /* 计算任务运行到指定频率需要等待的tick数 */ const uint32_t delay_tick = osKernelGetTickFreq() / IMU_FREQ; osDelay(IMU_INIT_DELAY); /* 延时一段时间再开启任务 */ uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ /* USER CODE INIT BEGIN */ BSP_CAN_Init(); /* USER CODE INIT END */ while (1) { tick += delay_tick; /* 计算下一个唤醒时刻 */ /* USER CODE BEGIN */ /* 从消息队列获取IMU数据 */ if (osMessageQueueGet(task_runtime.msgq.imu, &imu_data, NULL, 0) == osOK) { BSP_CAN_StdDataFrame_t can_frame; /* 包1: 加速度计数据 (acc_x, acc_y, acc_z) - 使用21位精度 */ can_frame.id = CAN_ID_IMU_ACCEL; can_frame.dlc = 8; 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) - 使用21位精度 */ can_frame.id = CAN_ID_IMU_GYRO; can_frame.dlc = 8; 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) - 使用21位精度 */ can_frame.id = CAN_ID_IMU_EULER; can_frame.dlc = 8; 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位精度打包 */ can_frame.id = CAN_ID_IMU_QUAT; can_frame.dlc = 8; uint16_t q0 = float_to_uint(imu_data.quat.q0, QUAT_MIN, QUAT_MAX, 14); uint16_t q1 = float_to_uint(imu_data.quat.q1, QUAT_MIN, QUAT_MAX, 14); uint16_t q2 = float_to_uint(imu_data.quat.q2, QUAT_MIN, QUAT_MAX, 14); uint16_t q3 = float_to_uint(imu_data.quat.q3, QUAT_MIN, QUAT_MAX, 14); // 打包: q0(14bit) q1(14bit) q2(14bit) q3(14bit) = 56bits can_frame.data[0] = 0; // 预留 can_frame.data[1] = (q0 >> 6) & 0xFF; can_frame.data[2] = ((q0 << 2) & 0xFC) | ((q1 >> 12) & 0x03); can_frame.data[3] = (q1 >> 4) & 0xFF; can_frame.data[4] = ((q1 << 4) & 0xF0) | ((q2 >> 10) & 0x0F); can_frame.data[5] = (q2 >> 2) & 0xFF; can_frame.data[6] = ((q2 << 6) & 0xC0) | ((q3 >> 8) & 0x3F); can_frame.data[7] = q3 & 0xFF; BSP_CAN_TransmitStdDataFrame(BSP_CAN_2, &can_frame); } /* USER CODE END */ osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ } }