From aa41b87c0b6f1ecf0f2179e0578e3996eacb33c0 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sun, 11 Jan 2026 20:42:24 +0800 Subject: [PATCH] imu --- User/task/atti_esit.c | 180 ++++++++++++++++++++++++++++++++++++++++-- 1 file changed, 172 insertions(+), 8 deletions(-) diff --git a/User/task/atti_esit.c b/User/task/atti_esit.c index 19fd599..fbed008 100644 --- a/User/task/atti_esit.c +++ b/User/task/atti_esit.c @@ -1,38 +1,202 @@ /* - atti_esit Task + atti_esti Task */ /* Includes ----------------------------------------------------------------- */ +#include "cmsis_os2.h" #include "task/user_task.h" /* USER INCLUDE BEGIN */ - +#include "bsp/mm.h" +#include "bsp/pwm.h" +#include "bsp/gpio.h" +#include "bsp/flash.h" +#include "component/ahrs.h" +#include "component/pid.h" +#include "device/bmi088.h" +#include "task/user_task.h" /* USER INCLUDE END */ /* Private typedef ---------------------------------------------------------- */ /* Private define ----------------------------------------------------------- */ +/* 使用Sector 11末尾存储校准数据 */ +#define FLASH_CALI_ADDR (ADDR_FLASH_SECTOR_11 + 0x1F000) /* Sector 11末尾 */ +#define FLASH_CALI_MAGIC 0x42494D38 /* "BMI8" 魔数，用于验证数据有效性 */ +#define CALIB_SAMPLES 10000 /* 校准采样数量 */ + /* Private macro ------------------------------------------------------------ */ /* Private variables -------------------------------------------------------- */ /* USER STRUCT BEGIN */ +BMI088_t bmi088; + +/* 校准请求标识位 */ +volatile bool request_calibration = false; + +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}, /* 默认值，将从Flash加载 */ +}; + +/* Flash存储结构 */ +typedef struct { + uint32_t magic; /* 魔数，用于验证数据有效性 */ + BMI088_Cali_t cali_data; +} Flash_Cali_t; + +static const KPID_Params_t imu_temp_ctrl_pid_param = { + .k = 0.3f, + .p = 1.0f, + .i = 0.01f, + .d = 0.0f, + .i_limit = 1.0f, + .out_limit = 1.0f, +}; + +/* 校准相关变量 */ +typedef enum { + CALIB_IDLE, // 空闲状态 + CALIB_RUNNING, // 正在校准 + CALIB_DONE // 校准完成 +} CalibState_t; + +static CalibState_t calib_state = CALIB_IDLE; +static uint16_t calib_count = 0; +static struct { + float x_sum; + float y_sum; + float z_sum; +} gyro_sum= {0.0f,0.0f,0.0f}; +/* 按钮回调函数：设置校准请求标识位 */ +static void request_gyro_calibration(void) { + request_calibration = true; +} + +/* 从Flash加载校准数据 */ +static bool load_calibration_from_flash(void) { + Flash_Cali_t flash_data; + BSP_Flash_ReadBytes(FLASH_CALI_ADDR, &flash_data, sizeof(Flash_Cali_t)); + + /* 验证魔数 */ + if (flash_data.magic == FLASH_CALI_MAGIC) { + cali_bmi088 = flash_data.cali_data; + return true; + } + return false; +} + +/* 保存校准数据到Flash */ +static void save_calibration_to_flash(void) { + Flash_Cali_t flash_data; + flash_data.magic = FLASH_CALI_MAGIC; + flash_data.cali_data = cali_bmi088; + + /* 擦除扇区（注意：这会擦除整个Sector 11，如果有其他数据需要保留请修改） */ + BSP_Flash_EraseSector(FLASH_SECTOR_11); + + /* 写入数据 */ + BSP_Flash_WriteBytes(FLASH_CALI_ADDR, (uint8_t*)&flash_data, sizeof(Flash_Cali_t)); +} /* USER STRUCT END */ /* Private function --------------------------------------------------------- */ /* Exported functions ------------------------------------------------------- */ -void Task_atti_esit(void *argument) { +void Task_atti_esti(void *argument) { (void)argument; /* 未使用argument，消除警告 */ - - osDelay(ATTI_ESIT_INIT_DELAY); /* 延时一段时间再开启任务 */ - + uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ /* USER CODE INIT BEGIN */ - + /* 尝试从Flash加载校准数据 */ + if (!load_calibration_from_flash()) { + /* Flash中无有效校准数据，使用默认值 */ + cali_bmi088.gyro_offset.x = 0.0f; + cali_bmi088.gyro_offset.y = 0.0f; + cali_bmi088.gyro_offset.z = 0.0f; + } + + BMI088_Init(&bmi088, &cali_bmi088); + AHRS_Init(&gimbal_ahrs, &magn, BMI088_GetUpdateFreq(&bmi088)); + PID_Init(&imu_temp_ctrl_pid, KPID_MODE_NO_D, + 1.0f / BMI088_GetUpdateFreq(&bmi088), &imu_temp_ctrl_pid_param); + BSP_PWM_Start(BSP_PWM_IMU_HEAT_PWM); + + /* 注册按钮回调函数并启用中断 */ + BSP_GPIO_RegisterCallback(BSP_GPIO_USER_KEY, request_gyro_calibration); + BSP_GPIO_EnableIRQ(BSP_GPIO_USER_KEY); /* USER CODE INIT END */ - + while (1) { /* USER CODE BEGIN */ + BMI088_WaitNew(); + BMI088_AcclStartDmaRecv(); + BMI088_AcclWaitDmaCplt(); + + BMI088_GyroStartDmaRecv(); + BMI088_GyroWaitDmaCplt(); + + /* 锁住RTOS内核防止数据解析过程中断，造成错误 */ + osKernelLock(); + /* 接收完所有数据后，把数据从原始字节加工成方便计算的数据 */ + BMI088_ParseAccl(&bmi088); + BMI088_ParseGyro(&bmi088); + // IST8310_Parse(&ist8310); + + /* 检查校准请求标识位 */ + if (request_calibration && calib_state == CALIB_IDLE) { + calib_state = CALIB_RUNNING; + calib_count = 0; + gyro_sum.x_sum = 0.0f; + gyro_sum.y_sum = 0.0f; + gyro_sum.z_sum = 0.0f; + request_calibration = false; /* 清除标识位 */ + } + + /* 陀螺仪校准处理 */ + if (calib_state == CALIB_RUNNING) { + /* 累加陀螺仪数据用于计算零偏 */ + gyro_sum.x_sum += bmi088.gyro.x; + gyro_sum.y_sum += bmi088.gyro.y; + gyro_sum.z_sum += bmi088.gyro.z; + calib_count++; + + /* 达到采样数量后计算平均值并更新零偏 */ + if (calib_count >= CALIB_SAMPLES) { + /* 计算平均值作为零偏 */ + cali_bmi088.gyro_offset.x = gyro_sum.x_sum / CALIB_SAMPLES; + cali_bmi088.gyro_offset.y = gyro_sum.y_sum / CALIB_SAMPLES; + cali_bmi088.gyro_offset.z = gyro_sum.z_sum / CALIB_SAMPLES; + + /* 保存校准数据到Flash */ + save_calibration_to_flash(); + + /* 校准完成，重置为空闲状态以便下次校准 */ + calib_state = CALIB_IDLE; + + /* 重新初始化BMI088以应用新的校准数据 */ + BMI088_Init(&bmi088, &cali_bmi088); + AHRS_Init(&gimbal_ahrs, &magn, BMI088_GetUpdateFreq(&bmi088)); + } + } + + /* 只有在非校准状态下才进行正常的姿态解析 */ + if (calib_state != CALIB_RUNNING) { + /* 根据设备接收到的数据进行姿态解析 */ + AHRS_Update(&gimbal_ahrs, &bmi088.accl, &bmi088.gyro, &magn); + + /* 根据解析出来的四元数计算欧拉角 */ + AHRS_GetEulr(&eulr_to_send, &gimbal_ahrs); + } + + osKernelUnlock(); + + BSP_PWM_SetComp(BSP_PWM_IMU_HEAT_PWM, PID_Calc(&imu_temp_ctrl_pid, 40.0f, bmi088.temp, 0.0f, 0.0f)); /* USER CODE END */ + osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ } } \ No newline at end of file