/* imu Task */ /* Includes ----------------------------------------------------------------- */ #include "task/user_task.h" /* USER INCLUDE BEGIN */ #include "component/user_math.h" #include "bsp/can.h" #include "bsp/time.h" #include "component/ahrs.h" #include "component/filter.h" #include #include /* USER INCLUDE END */ /* Private typedef ---------------------------------------------------------- */ /* Private define ----------------------------------------------------------- */ /* AHRS数据CAN ID定义 */ #define CAN_ID_AHRS_ACCL 0x301 /* 加速度计数据 */ #define CAN_ID_AHRS_GYRO 0x302 /* 陀螺仪数据 */ #define CAN_ID_AHRS_EULR 0x303 /* 欧拉角数据 */ #define CAN_ID_AHRS_QUAT 0x304 /* 四元数数据 */ /* 数据范围定义 */ #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 (M_PI_2) /* π/2 弧度 ≈ 90° */ #define PITCH_CAN_MIN (-M_PI_2) /* -π/2 弧度 ≈ -90° */ #define ROLL_CAN_MAX (M_PI) /* π 弧度 ≈ 180° */ #define ROLL_CAN_MIN (-M_PI) /* -π 弧度 ≈ -180° */ #define YAW_CAN_MAX (M_PI) /* π 弧度 ≈ 180° */ #define YAW_CAN_MIN (-M_PI) /* -π 弧度 ≈ -180° */ #define QUATERNION_MIN (-1.0f) #define QUATERNION_MAX (1.0f) /* Private macro ------------------------------------------------------------ */ /* 数据映射宏：将浮点值映射到16位整数范围 */ #define MAP_TO_INT16(val, min, max) \ ((int16_t)(((val) - (min)) / ((max) - (min)) * 65535.0f - 32768.0f)) /* 限制值在指定范围内 */ #define CLAMP(val, min, max) \ (((val) < (min)) ? (min) : (((val) > (max)) ? (max) : (val))) /* Private variables -------------------------------------------------------- */ /* USER STRUCT BEGIN */ AHRS_Accl_t accl; AHRS_Gyro_t gyro; AHRS_Eulr_t eulr; AHRS_Quaternion_t quat; /* 低通滤波器定义 */ /* 加速度计3轴滤波器 */ LowPassFilter2p_t accl_filter_x; LowPassFilter2p_t accl_filter_y; LowPassFilter2p_t accl_filter_z; /* 陀螺仪3轴滤波器 */ LowPassFilter2p_t gyro_filter_x; LowPassFilter2p_t gyro_filter_y; LowPassFilter2p_t gyro_filter_z; /* 欧拉角3轴滤波器 */ LowPassFilter2p_t eulr_filter_yaw; LowPassFilter2p_t eulr_filter_pitch; LowPassFilter2p_t eulr_filter_roll; /* 滤波器参数定义 */ #define FILTER_CUTOFF_FREQ (30.0f) /* 截止频率50Hz */ /* USER STRUCT END */ /* Private function --------------------------------------------------------- */ /* 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(); /* 初始化低通滤波器 */ /* 加速度计滤波器初始化 */ LowPassFilter2p_Init(&accl_filter_x, IMU_FREQ, FILTER_CUTOFF_FREQ); LowPassFilter2p_Init(&accl_filter_y, IMU_FREQ, FILTER_CUTOFF_FREQ); LowPassFilter2p_Init(&accl_filter_z, IMU_FREQ, FILTER_CUTOFF_FREQ); /* 陀螺仪滤波器初始化 */ LowPassFilter2p_Init(&gyro_filter_x, IMU_FREQ, FILTER_CUTOFF_FREQ); LowPassFilter2p_Init(&gyro_filter_y, IMU_FREQ, FILTER_CUTOFF_FREQ); LowPassFilter2p_Init(&gyro_filter_z, IMU_FREQ, FILTER_CUTOFF_FREQ); /* 欧拉角滤波器初始化 */ LowPassFilter2p_Init(&eulr_filter_yaw, IMU_FREQ, FILTER_CUTOFF_FREQ); LowPassFilter2p_Init(&eulr_filter_pitch, IMU_FREQ, FILTER_CUTOFF_FREQ); LowPassFilter2p_Init(&eulr_filter_roll, IMU_FREQ, FILTER_CUTOFF_FREQ); /* USER CODE INIT END */ while (1) { tick += delay_tick; /* 计算下一个唤醒时刻 */ /* USER CODE BEGIN */ /* 获取加速度计数据，应用滤波器后通过CAN发送 - 每轴使用16位编码 */ if (osMessageQueueGet(task_runtime.msgq.imu.accl, &accl, NULL, 0) == osOK) { BSP_CAN_StdDataFrame_t accl_frame; accl_frame.id = CAN_ID_AHRS_ACCL; accl_frame.dlc = 6; /* 3轴 × 2字节 = 6字节 */ /* 对加速度数据应用低通滤波器 */ float ax_filtered = LowPassFilter2p_Apply(&accl_filter_x, accl.x); float ay_filtered = LowPassFilter2p_Apply(&accl_filter_y, accl.y); float az_filtered = LowPassFilter2p_Apply(&accl_filter_z, accl.z); /* 限制并映射滤波后的加速度数据到16位整数 */ float ax = CLAMP(ax_filtered, ACCEL_CAN_MIN, ACCEL_CAN_MAX); float ay = CLAMP(ay_filtered, ACCEL_CAN_MIN, ACCEL_CAN_MAX); float az = CLAMP(az_filtered, ACCEL_CAN_MIN, ACCEL_CAN_MAX); int16_t ax_int = MAP_TO_INT16(ax, ACCEL_CAN_MIN, ACCEL_CAN_MAX); int16_t ay_int = MAP_TO_INT16(ay, ACCEL_CAN_MIN, ACCEL_CAN_MAX); int16_t az_int = MAP_TO_INT16(az, ACCEL_CAN_MIN, ACCEL_CAN_MAX); memcpy(&accl_frame.data[0], &ax_int, sizeof(int16_t)); memcpy(&accl_frame.data[2], &ay_int, sizeof(int16_t)); memcpy(&accl_frame.data[4], &az_int, sizeof(int16_t)); BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &accl_frame); } /* 获取陀螺仪数据，应用滤波器后通过CAN发送 - 每轴使用16位编码 */ if (osMessageQueueGet(task_runtime.msgq.imu.gyro, &gyro, NULL, 0) == osOK) { BSP_CAN_StdDataFrame_t gyro_frame; gyro_frame.id = CAN_ID_AHRS_GYRO; gyro_frame.dlc = 6; /* 3轴 × 2字节 = 6字节 */ /* 对陀螺仪数据应用低通滤波器 */ float gx_filtered = LowPassFilter2p_Apply(&gyro_filter_x, gyro.x); float gy_filtered = LowPassFilter2p_Apply(&gyro_filter_y, gyro.y); float gz_filtered = LowPassFilter2p_Apply(&gyro_filter_z, gyro.z); /* 限制并映射滤波后的陀螺仪数据到16位整数 */ float gx = CLAMP(gx_filtered, GYRO_CAN_MIN, GYRO_CAN_MAX); float gy = CLAMP(gy_filtered, GYRO_CAN_MIN, GYRO_CAN_MAX); float gz = CLAMP(gz_filtered, GYRO_CAN_MIN, GYRO_CAN_MAX); int16_t gx_int = MAP_TO_INT16(gx, GYRO_CAN_MIN, GYRO_CAN_MAX); int16_t gy_int = MAP_TO_INT16(gy, GYRO_CAN_MIN, GYRO_CAN_MAX); int16_t gz_int = MAP_TO_INT16(gz, GYRO_CAN_MIN, GYRO_CAN_MAX); memcpy(&gyro_frame.data[0], &gx_int, sizeof(int16_t)); memcpy(&gyro_frame.data[2], &gy_int, sizeof(int16_t)); memcpy(&gyro_frame.data[4], &gz_int, sizeof(int16_t)); BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &gyro_frame); } /* 获取欧拉角数据，应用滤波器后通过CAN发送 - 每角使用16位编码 */ if (osMessageQueueGet(task_runtime.msgq.imu.eulr, &eulr, NULL, 0) == osOK) { BSP_CAN_StdDataFrame_t eulr_frame; eulr_frame.id = CAN_ID_AHRS_EULR; eulr_frame.dlc = 6; /* 3个角度 × 2字节 = 6字节 */ /* 对欧拉角数据应用低通滤波器 */ float yaw_filtered = LowPassFilter2p_Apply(&eulr_filter_yaw, eulr.yaw); float pitch_filtered = LowPassFilter2p_Apply(&eulr_filter_pitch, eulr.pit); float roll_filtered = LowPassFilter2p_Apply(&eulr_filter_roll, eulr.rol); /* 限制并映射滤波后的欧拉角数据到16位整数 */ float yaw = CLAMP(yaw_filtered, YAW_CAN_MIN, YAW_CAN_MAX); float pitch = CLAMP(pitch_filtered, PITCH_CAN_MIN, PITCH_CAN_MAX); float roll = CLAMP(roll_filtered, ROLL_CAN_MIN, ROLL_CAN_MAX); int16_t yaw_int = MAP_TO_INT16(yaw, YAW_CAN_MIN, YAW_CAN_MAX); int16_t pitch_int = MAP_TO_INT16(pitch, PITCH_CAN_MIN, PITCH_CAN_MAX); int16_t roll_int = MAP_TO_INT16(roll, ROLL_CAN_MIN, ROLL_CAN_MAX); memcpy(&eulr_frame.data[0], &yaw_int, sizeof(int16_t)); memcpy(&eulr_frame.data[2], &pitch_int, sizeof(int16_t)); memcpy(&eulr_frame.data[4], &roll_int, sizeof(int16_t)); BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &eulr_frame); } /* 获取四元数数据并通过CAN发送 - 每个分量使用16位编码 */ if (osMessageQueueGet(task_runtime.msgq.imu.quat, &quat, NULL, 0) == osOK) { BSP_CAN_StdDataFrame_t quat_frame; quat_frame.id = CAN_ID_AHRS_QUAT; quat_frame.dlc = 8; /* 4个四元数分量 × 2字节 = 8字节 */ /* 限制并映射四元数到16位整数 */ float q0 = CLAMP(quat.q0, QUATERNION_MIN, QUATERNION_MAX); float q1 = CLAMP(quat.q1, QUATERNION_MIN, QUATERNION_MAX); float q2 = CLAMP(quat.q2, QUATERNION_MIN, QUATERNION_MAX); float q3 = CLAMP(quat.q3, QUATERNION_MIN, QUATERNION_MAX); int16_t q0_int = MAP_TO_INT16(q0, QUATERNION_MIN, QUATERNION_MAX); int16_t q1_int = MAP_TO_INT16(q1, QUATERNION_MIN, QUATERNION_MAX); int16_t q2_int = MAP_TO_INT16(q2, QUATERNION_MIN, QUATERNION_MAX); int16_t q3_int = MAP_TO_INT16(q3, QUATERNION_MIN, QUATERNION_MAX); memcpy(&quat_frame.data[0], &q0_int, sizeof(int16_t)); memcpy(&quat_frame.data[2], &q1_int, sizeof(int16_t)); memcpy(&quat_frame.data[4], &q2_int, sizeof(int16_t)); memcpy(&quat_frame.data[6], &q3_int, sizeof(int16_t)); BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &quat_frame); } /* USER CODE END */ osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ } }