diff --git a/Core/Src/can.c b/Core/Src/can.c index fb925fd..5f0741e 100644 --- a/Core/Src/can.c +++ b/Core/Src/can.c @@ -66,7 +66,7 @@ void MX_CAN1_Init(void) hcan1.Init.AutoWakeUp = DISABLE; hcan1.Init.AutoRetransmission = ENABLE; hcan1.Init.ReceiveFifoLocked = DISABLE; - hcan1.Init.TransmitFifoPriority = ENABLE; + hcan1.Init.TransmitFifoPriority = DISABLE; if (HAL_CAN_Init(&hcan1) != HAL_OK) { Error_Handler(); @@ -98,7 +98,7 @@ void MX_CAN2_Init(void) hcan2.Init.AutoWakeUp = DISABLE; hcan2.Init.AutoRetransmission = ENABLE; hcan2.Init.ReceiveFifoLocked = DISABLE; - hcan2.Init.TransmitFifoPriority = ENABLE; + hcan2.Init.TransmitFifoPriority = DISABLE; if (HAL_CAN_Init(&hcan2) != HAL_OK) { Error_Handler(); diff --git a/DevC.ioc b/DevC.ioc index 6e15eea..6ae67a2 100644 --- a/DevC.ioc +++ b/DevC.ioc @@ -15,16 +15,15 @@ ADC3.SamplingTime-0\#ChannelRegularConversion=ADC_SAMPLETIME_3CYCLES CAD.formats= CAD.pinconfig= CAD.provider= -CAN1.ABOM=DISABLE CAN1.BS1=CAN_BS1_6TQ CAN1.BS2=CAN_BS2_7TQ CAN1.CalculateBaudRate=1000000 CAN1.CalculateTimeBit=1000 CAN1.CalculateTimeQuantum=71.42857142857143 -CAN1.IPParameters=CalculateTimeQuantum,BS1,BS2,Prescaler,TXFP,ABOM,CalculateTimeBit,CalculateBaudRate,NART +CAN1.IPParameters=CalculateTimeQuantum,CalculateTimeBit,CalculateBaudRate,Prescaler,BS1,BS2,NART,TXFP CAN1.NART=ENABLE CAN1.Prescaler=3 -CAN1.TXFP=ENABLE +CAN1.TXFP=DISABLE CAN2.BS1=CAN_BS1_6TQ CAN2.BS2=CAN_BS2_7TQ CAN2.CalculateBaudRate=1000000 @@ -33,7 +32,7 @@ CAN2.CalculateTimeQuantum=71.42857142857143 CAN2.IPParameters=CalculateTimeQuantum,BS1,BS2,Prescaler,TXFP,CalculateTimeBit,CalculateBaudRate,NART CAN2.NART=ENABLE CAN2.Prescaler=3 -CAN2.TXFP=ENABLE +CAN2.TXFP=DISABLE Dma.I2C2_TX.2.Direction=DMA_MEMORY_TO_PERIPH Dma.I2C2_TX.2.FIFOMode=DMA_FIFOMODE_DISABLE Dma.I2C2_TX.2.Instance=DMA1_Stream7 @@ -285,7 +284,7 @@ NVIC.CAN2_RX1_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true MxCube.Version=6.2.0 MxDb.Version=DB.6.0.20 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\: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 @@ -608,7 +607,7 @@ ProjectManager.ProjectFileName=DevC.ioc ProjectManager.ProjectName=DevC ProjectManager.ProjectStructure= ProjectManager.RegisterCallBack= -ProjectManager.StackSize=0x1000 +ProjectManager.StackSize=0x2000 ProjectManager.TargetToolchain=CMake ProjectManager.ToolChainLocation= ProjectManager.UAScriptAfterPath= diff --git a/STM32F407XX_FLASH.ld b/STM32F407XX_FLASH.ld index 869cbeb..296c041 100644 --- a/STM32F407XX_FLASH.ld +++ b/STM32F407XX_FLASH.ld @@ -75,7 +75,7 @@ FLASH (rx) : ORIGIN = 0x8000000, LENGTH = 1024K _estack = ORIGIN(RAM) + LENGTH(RAM); /* end of RAM */ /* Generate a link error if heap and stack don't fit into RAM */ _Min_Heap_Size = 0x1000; /* required amount of heap */ -_Min_Stack_Size = 0x1000; /* required amount of stack */ +_Min_Stack_Size = 0x2000; /* required amount of stack */ >>>>>>> upper /* Define output sections */ diff --git a/User/device/dm_imu.c b/User/device/dm_imu.c new file mode 100644 index 0000000..e5380d9 --- /dev/null +++ b/User/device/dm_imu.c @@ -0,0 +1,271 @@ +/* + DM_IMU数据获取（CAN） +*/ + +/* Includes ----------------------------------------------------------------- */ +#include "dm_imu.h" + +#include "bsp/can.h" +#include "bsp/time.h" +#include "component/user_math.h" +#include + +/* Private define ----------------------------------------------------------- */ +#define DM_IMU_OFFLINE_TIMEOUT 1000 // 设备离线判定时间1000ms + +#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 TEMP_MIN (0.0f) +#define TEMP_MAX (60.0f) +#define Quaternion_MIN (-1.0f) +#define Quaternion_MAX (1.0f) + + +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +/* Private variables -------------------------------------------------------- */ +/* Private function --------------------------------------------------------- */ + +static uint8_t count = 0; // 计数器，用于判断设备是否离线 +/** + * @brief: 无符号整数转换为浮点数函数 + */ +static float uint_to_float(int x_int, float x_min, float x_max, int bits) +{ + float span = x_max - x_min; + float offset = x_min; + return ((float)x_int)*span/((float)((1<data.temp = (float)temp; + imu->data.accl.x = uint_to_float(acc_x_raw, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16); + imu->data.accl.y = uint_to_float(acc_y_raw, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16); + imu->data.accl.z = uint_to_float(acc_z_raw, ACCEL_CAN_MIN, ACCEL_CAN_MAX, 16); + return DEVICE_OK; +} +/** + * @brief 解析陀螺仪数据 + */ +static int8_t DM_IMU_ParseGyroData(DM_IMU_t *imu, uint8_t *data, uint8_t len) { + if (imu == NULL || data == NULL || len < 8) { + return DEVICE_ERR; + } + uint16_t gyro_x_raw = (data[3] << 8) | data[2]; + uint16_t gyro_y_raw = (data[5] << 8) | data[4]; + uint16_t gyro_z_raw = (data[7] << 8) | data[6]; + imu->data.gyro.x = uint_to_float(gyro_x_raw, GYRO_CAN_MIN, GYRO_CAN_MAX, 16); + imu->data.gyro.y = uint_to_float(gyro_y_raw, GYRO_CAN_MIN, GYRO_CAN_MAX, 16); + imu->data.gyro.z = uint_to_float(gyro_z_raw, GYRO_CAN_MIN, GYRO_CAN_MAX, 16); + return DEVICE_OK; +} +/** + * @brief 解析欧拉角数据 + */ +static int8_t DM_IMU_ParseEulerData(DM_IMU_t *imu, uint8_t *data, uint8_t len) { + if (imu == NULL || data == NULL || len < 8) { + return DEVICE_ERR; + } + uint16_t pit_raw = (data[3] << 8) | data[2]; + uint16_t yaw_raw = (data[5] << 8) | data[4]; + uint16_t rol_raw = (data[7] << 8) | data[6]; + imu->data.euler.pit = uint_to_float(pit_raw, PITCH_CAN_MIN, PITCH_CAN_MAX, 16) * M_DEG2RAD_MULT; + imu->data.euler.yaw = uint_to_float(yaw_raw, YAW_CAN_MIN, YAW_CAN_MAX, 16) * M_DEG2RAD_MULT; + imu->data.euler.rol = uint_to_float(rol_raw, ROLL_CAN_MIN, ROLL_CAN_MAX, 16) * M_DEG2RAD_MULT; + return DEVICE_OK; +} + +/** + * @brief 解析四元数数据 + */ +static int8_t DM_IMU_ParseQuaternionData(DM_IMU_t *imu, uint8_t *data, uint8_t len) { + if (imu == NULL || data == NULL || len < 8) { + return DEVICE_ERR; + } + int w = (data[1] << 6) | ((data[2] & 0xF8) >> 2); + int x = ((data[2] & 0x03) << 12) | (data[3] << 4) | ((data[4] & 0xF0) >> 4); + int y = ((data[4] & 0x0F) << 10) | (data[5] << 2) | ((data[6] & 0xC0) >> 6); + int z = ((data[6] & 0x3F) << 8) | data[7]; + imu->data.quat.q0 = uint_to_float(w, Quaternion_MIN, Quaternion_MAX, 14); + imu->data.quat.q1 = uint_to_float(x, Quaternion_MIN, Quaternion_MAX, 14); + imu->data.quat.q2 = uint_to_float(y, Quaternion_MIN, Quaternion_MAX, 14); + imu->data.quat.q3 = uint_to_float(z, Quaternion_MIN, Quaternion_MAX, 14); + return DEVICE_OK; +} + + +/* Exported functions ------------------------------------------------------- */ + +/** + * @brief 初始化DM IMU设备 + */ +int8_t DM_IMU_Init(DM_IMU_t *imu, DM_IMU_Param_t *param) { + if (imu == NULL || param == NULL) { + return DEVICE_ERR_NULL; + } + + // 初始化设备头部 + imu->header.online = false; + imu->header.last_online_time = 0; + + // 配置参数 + imu->param.can = param->can; + imu->param.can_id = param->can_id; + imu->param.device_id = param->device_id; + imu->param.master_id = param->master_id; + + // 清零数据 + memset(&imu->data, 0, sizeof(DM_IMU_Data_t)); + + // 注册CAN接收队列，用于接收回复报文 + int8_t result = BSP_CAN_RegisterId(imu->param.can, imu->param.master_id, 10); + if (result != BSP_OK) { + return DEVICE_ERR; + } + + return DEVICE_OK; +} + +/** + * @brief 请求IMU数据 + */ +int8_t DM_IMU_Request(DM_IMU_t *imu, DM_IMU_RID_t rid) { + if (imu == NULL) { + return DEVICE_ERR_NULL; + } + + // 构造发送数据：id_L, id_H(DM_IMU_ID), RID, 0xcc + uint8_t tx_data[4] = { + imu->param.device_id & 0xFF, // id_L + (imu->param.device_id >> 8) & 0xFF, // id_H + (uint8_t)rid, // RID + 0xCC // 固定值 + }; + + // 发送标准数据帧 + BSP_CAN_StdDataFrame_t frame = { + .id = imu->param.can_id, + .dlc = 4, + }; + memcpy(frame.data, tx_data, 4); + int8_t result = BSP_CAN_TransmitStdDataFrame(imu->param.can, &frame); + return (result == BSP_OK) ? DEVICE_OK : DEVICE_ERR; +} + +/** + * @brief 更新IMU数据（从CAN中获取所有数据并解析） + */ +int8_t DM_IMU_Update(DM_IMU_t *imu) { + if (imu == NULL) { + return DEVICE_ERR_NULL; + } + + BSP_CAN_Message_t msg; + int8_t result; + bool data_received = false; + + // 持续接收所有可用消息 + while ((result = BSP_CAN_GetMessage(imu->param.can, imu->param.master_id, &msg, BSP_CAN_TIMEOUT_IMMEDIATE)) == BSP_OK) { + // 验证回复数据格式（至少检查数据长度） + if (msg.dlc < 3) { + continue; // 跳过无效消息 + } + + // 根据数据位的第0位确定反馈报文类型 + uint8_t rid = msg.data[0] & 0x0F; // 取第0位的低4位作为RID + + // 根据RID类型解析数据 + int8_t parse_result = DEVICE_ERR; + switch (rid) { + case 0x01: // RID_ACCL + parse_result = DM_IMU_ParseAccelData(imu, msg.data, msg.dlc); + break; + case 0x02: // RID_GYRO + parse_result = DM_IMU_ParseGyroData(imu, msg.data, msg.dlc); + break; + case 0x03: // RID_EULER + parse_result = DM_IMU_ParseEulerData(imu, msg.data, msg.dlc); + break; + case 0x04: // RID_QUATERNION + parse_result = DM_IMU_ParseQuaternionData(imu, msg.data, msg.dlc); + break; + default: + continue; // 跳过未知类型的消息 + } + + // 如果解析成功，标记为收到数据 + if (parse_result == DEVICE_OK) { + data_received = true; + } + } + + // 如果收到任何有效数据，更新设备状态 + if (data_received) { + imu->header.online = true; + imu->header.last_online_time = BSP_TIME_Get_ms(); + return DEVICE_OK; + } + + return DEVICE_ERR; +} + +/** + * @brief 自动更新IMU所有数据（包括加速度计、陀螺仪、欧拉角和四元数,最高1khz） + */ +int8_t DM_IMU_AutoUpdateAll(DM_IMU_t *imu){ + if (imu == NULL) { + return DEVICE_ERR_NULL; + } + switch (count) { + case 0: + DM_IMU_Request(imu, RID_ACCL); + break; + case 1: + DM_IMU_Request(imu, RID_GYRO); + break; + case 2: + DM_IMU_Request(imu, RID_EULER); + break; + case 3: + DM_IMU_Request(imu, RID_QUATERNION); + DM_IMU_Update(imu); // 更新所有数据 + break; + } + count++; + if (count >= 4) { + count = 0; // 重置计数器 + return DEVICE_OK; + } + return DEVICE_ERR; +} + +/** + * @brief 检查设备是否在线 + */ +bool DM_IMU_IsOnline(DM_IMU_t *imu) { + if (imu == NULL) { + return false; + } + + uint32_t current_time = BSP_TIME_Get_ms(); + return imu->header.online && + (current_time - imu->header.last_online_time < DM_IMU_OFFLINE_TIMEOUT); +} diff --git a/User/device/dm_imu.h b/User/device/dm_imu.h new file mode 100644 index 0000000..3965980 --- /dev/null +++ b/User/device/dm_imu.h @@ -0,0 +1,90 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include "device/device.h" +#include "component/ahrs.h" +#include "bsp/can.h" +/* Exported constants ------------------------------------------------------- */ + +#define DM_IMU_CAN_ID_DEFAULT 0x6FF +#define DM_IMU_ID_DEFAULT 0x42 +#define DM_IMU_MST_ID_DEFAULT 0x43 + +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ + +typedef struct { + BSP_CAN_t can; // CAN总线句柄 + uint16_t can_id; // CAN通信ID + uint8_t device_id; // 设备ID + uint8_t master_id; // 主机ID +} DM_IMU_Param_t; +typedef enum { + RID_ACCL = 0x01, // 加速度计 + RID_GYRO = 0x02, // 陀螺仪 + RID_EULER = 0x03, // 欧拉角 + RID_QUATERNION = 0x04, // 四元数 +} DM_IMU_RID_t; + +typedef struct { + AHRS_Accl_t accl; // 加速度计 + AHRS_Gyro_t gyro; // 陀螺仪 + AHRS_Eulr_t euler; // 欧拉角 + AHRS_Quaternion_t quat; // 四元数 + float temp; // 温度 +} DM_IMU_Data_t; + +typedef struct { + DEVICE_Header_t header; + DM_IMU_Param_t param; // IMU参数配置 + DM_IMU_Data_t data; // IMU数据 +} DM_IMU_t; + +/* Exported functions prototypes -------------------------------------------- */ + +/** + * @brief 初始化DM IMU设备 + * @param imu DM IMU设备结构体指针 + * @param param IMU参数配置指针 + * @return DEVICE_OK 成功，其他值失败 + */ +int8_t DM_IMU_Init(DM_IMU_t *imu, DM_IMU_Param_t *param); + +/** + * @brief 请求IMU数据 + * @param imu DM IMU设备结构体指针 + * @param rid 请求的数据类型 + * @return DEVICE_OK 成功，其他值失败 + */ +int8_t DM_IMU_Request(DM_IMU_t *imu, DM_IMU_RID_t rid); + + +/** + * @brief 更新IMU数据（从CAN中获取所有数据并解析） + * @param imu DM IMU设备结构体指针 + * @return DEVICE_OK 成功，其他值失败 + */ +int8_t DM_IMU_Update(DM_IMU_t *imu); + +/** + * @brief 自动更新IMU所有数据（包括加速度计、陀螺仪、欧拉角和四元数,最高1khz，运行4次才有完整数据） + * @param imu DM IMU设备结构体指针 + * @return DEVICE_OK 成功，其他值失败 + */ +int8_t DM_IMU_AutoUpdateAll(DM_IMU_t *imu); + +/** + * @brief 检查设备是否在线 + * @param imu DM IMU设备结构体指针 + * @return true 在线，false 离线 + */ +bool DM_IMU_IsOnline(DM_IMU_t *imu); + + +#ifdef __cplusplus +} +#endif diff --git a/User/task/1.c b/User/task/1.c deleted file mode 100644 index 73d130b..0000000 --- a/User/task/1.c +++ /dev/null @@ -1,95 +0,0 @@ -/* - imu Task - -*/ - -/* Includes ----------------------------------------------------------------- */ -#include "task/user_task.h" -/* USER INCLUDE BEGIN */ -#include "bsp/pwm.h" -#include "component/ahrs.h" -#include "component/pid.h" -#include "device/bmi088.h" -/* USER INCLUDE END */ - -/* Private typedef ---------------------------------------------------------- */ -/* Private define ----------------------------------------------------------- */ -/* Private macro ------------------------------------------------------------ */ -/* Private variables -------------------------------------------------------- */ -/* USER STRUCT BEGIN */ - -BMI088_t bmi088; - -AHRS_t gimbal_ahrs; -AHRS_Magn_t magn; -AHRS_Eulr_t eulr_to_send; - -KPID_t imu_temp_ctrl_pid; - -BMI088_Cali_t cali_bmi088= { - .gyro_offset = {0.0f,0.0f,0.0f}, -}; - -/* 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 */ - BMI088_Init(&bmi088,&cali_bmi088); - AHRS_Init(&gimbal_ahrs, &magn, BMI088_GetUpdateFreq(&bmi088)); - - /* USER CODE INIT END */ - - while (1) { - tick += delay_tick; /* 计算下一个唤醒时刻 */ - /* USER CODE BEGIN */ - BMI088_WaitNew(); - BMI088_AcclStartDmaRecv(); - BMI088_AcclWaitDmaCplt(); - - BMI088_GyroStartDmaRecv(); - BMI088_GyroWaitDmaCplt(); - - /* 锁住RTOS内核防止数据解析过程中断，造成错误 */ - osKernelLock(); - /* 接收完所有数据后，把数据从原始字节加工成方便计算的数据 */ - BMI088_ParseAccl(&bmi088); - /* 扩大加速度数据10倍，并交换x和y */ - float temp_x = bmi088.accl.x * 10.0f; - float temp_y = bmi088.accl.y * 10.0f; - bmi088.accl.x = temp_y; - bmi088.accl.y = -temp_x; - bmi088.accl.z *= 10.0f; - - BMI088_ParseGyro(&bmi088); - /* 交换陀螺仪x和y */ - float temp_gyro_x = bmi088.gyro.x; - bmi088.gyro.x = bmi088.gyro.y; - bmi088.gyro.y = -temp_gyro_x; - // IST8310_Parse(&ist8310); - - /* 根据设备接收到的数据进行姿态解析 */ - AHRS_Update(&gimbal_ahrs, &bmi088.accl, &bmi088.gyro, &magn); - - /* 根据解析出来的四元数计算欧拉角 */ - AHRS_GetEulr(&eulr_to_send, &gimbal_ahrs); - /* 交换pit和rol */ - float temp_rol = eulr_to_send.rol; - eulr_to_send.rol = eulr_to_send.pit; - eulr_to_send.pit = temp_rol; - osKernelUnlock(); - /* USER CODE END */ - osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ - } - -} \ No newline at end of file diff --git a/User/task/atti_esti.c b/User/task/atti_esti.c index 65e5fa7..7a14fc3 100644 --- a/User/task/atti_esti.c +++ b/User/task/atti_esti.c @@ -4,7 +4,6 @@ */ /* Includes ----------------------------------------------------------------- */ -#include "cmsis_os2.h" #include "task/user_task.h" /* USER INCLUDE BEGIN */ #include "bsp/pwm.h" diff --git a/User/task/config.yaml b/User/task/config.yaml index 2587195..9e17a3b 100644 --- a/User/task/config.yaml +++ b/User/task/config.yaml @@ -83,3 +83,10 @@ function: Task_debug name: debug stack: 256 +- delay: 0 + description: '' + freq_control: true + frequency: 500.0 + function: Task_monitor + name: monitor + stack: 256 diff --git a/User/task/imu.c b/User/task/imu.c index e31a73c..968d271 100644 --- a/User/task/imu.c +++ b/User/task/imu.c @@ -10,6 +10,7 @@ #include "bsp/can.h" #include "bsp/time.h" #include "component/ahrs.h" +#include "component/filter.h" #include #include /* USER INCLUDE END */ @@ -50,6 +51,25 @@ 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 --------------------------------------------------------- */ @@ -66,21 +86,42 @@ void Task_imu(void *argument) { 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位编码 */ + /* 获取加速度计数据，应用滤波器后通过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字节 */ - /* 限制并映射加速度数据到16位整数 */ - float ax = CLAMP(accl.x, ACCEL_CAN_MIN, ACCEL_CAN_MAX); - float ay = CLAMP(accl.y, ACCEL_CAN_MIN, ACCEL_CAN_MAX); - float az = CLAMP(accl.z, ACCEL_CAN_MIN, ACCEL_CAN_MAX); + /* 对加速度数据应用低通滤波器 */ + 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); @@ -93,16 +134,21 @@ void Task_imu(void *argument) { BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &accl_frame); } - /* 获取陀螺仪数据并通过CAN发送 - 每轴使用16位编码 */ + /* 获取陀螺仪数据，应用滤波器后通过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字节 */ - /* 限制并映射陀螺仪数据到16位整数 */ - float gx = CLAMP(gyro.x, GYRO_CAN_MIN, GYRO_CAN_MAX); - float gy = CLAMP(gyro.y, GYRO_CAN_MIN, GYRO_CAN_MAX); - float gz = CLAMP(gyro.z, GYRO_CAN_MIN, GYRO_CAN_MAX); + /* 对陀螺仪数据应用低通滤波器 */ + 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); @@ -115,16 +161,21 @@ void Task_imu(void *argument) { BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &gyro_frame); } - /* 获取欧拉角数据并通过CAN发送 - 每角使用16位编码 */ + /* 获取欧拉角数据，应用滤波器后通过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字节 */ - /* 限制并映射欧拉角数据到16位整数 */ - float yaw = CLAMP(eulr.yaw, YAW_CAN_MIN, YAW_CAN_MAX); - float pitch = CLAMP(eulr.pit, PITCH_CAN_MIN, PITCH_CAN_MAX); - float roll = CLAMP(eulr.rol, ROLL_CAN_MIN, ROLL_CAN_MAX); + /* 对欧拉角数据应用低通滤波器 */ + 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); diff --git a/User/task/user_task.c b/User/task/user_task.c index d41f6a1..5ba951a 100644 --- a/User/task/user_task.c +++ b/User/task/user_task.c @@ -66,4 +66,9 @@ const osThreadAttr_t attr_debug = { .name = "debug", .priority = osPriorityNormal, .stack_size = 256 * 4, +}; +const osThreadAttr_t attr_monitor = { + .name = "monitor", + .priority = osPriorityNormal, + .stack_size = 256 * 4, }; \ No newline at end of file