From db7f569b5ccfd4a7c3c2eb7a7b93001787fe9792 Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Fri, 10 Oct 2025 17:31:19 +0800 Subject: [PATCH] =?UTF-8?q?=E6=94=B9=E8=BD=AE=E8=85=BF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- User/module/config.c | 1 + User/task/atti_esti.c | 91 +++++++++++++++++++++++++++++++++++++++---- User/task/rc.c | 8 ++-- 3 files changed, 88 insertions(+), 12 deletions(-) diff --git a/User/module/config.c b/User/module/config.c index 8d37c10..ca68362 100644 --- a/User/module/config.c +++ b/User/module/config.c @@ -253,6 +253,7 @@ Config_RobotParam_t robot_config = { .hip_length = 0.0f // 髋宽 (L5) (m) } }, + .lqr_gains = { .k11_coeff = { -1.498109852818409e+02f, 1.918923604951453e+02f, -1.080984818173493e+02f, -1.540703437137126e+00f }, // theta .k12_coeff = { 8.413682810667103e+00f, -6.603584762798329e+00f, -6.421063158996702e+00f, -5.450666844349098e-02f }, // d_theta diff --git a/User/task/atti_esti.c b/User/task/atti_esti.c index 2f669d7..63bb56a 100644 --- a/User/task/atti_esti.c +++ b/User/task/atti_esti.c @@ -6,11 +6,14 @@ /* Includes ----------------------------------------------------------------- */ #include "task/user_task.h" /* USER INCLUDE BEGIN */ +#include "bsp/mm.h" #include "bsp/pwm.h" +#include "bsp/gpio.h" #include "component/ahrs.h" #include "component/pid.h" -#include "device/bmi088.h" #include "module/gimbal.h" +#include "device/bmi088.h" +#include "task/user_task.h" /* USER INCLUDE END */ /* Private typedef ---------------------------------------------------------- */ @@ -29,9 +32,42 @@ KPID_t imu_temp_ctrl_pid; Gimbal_IMU_t gimbal_to_send; BMI088_Cali_t cali_bmi088= { - .gyro_offset = {0.0f,0.0f,0.0f}, + .gyro_offset = {-0.00149519544f,-0.00268901442f,0.00169837975f}, }; +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 start_gyro_calibration(void) { + if (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; + } +} +#define CALIB_SAMPLES 5000 // 校准采样数量 /* USER STRUCT END */ @@ -50,7 +86,13 @@ void Task_atti_esti(void *argument) { /* USER CODE INIT BEGIN */ 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, start_gyro_calibration); + BSP_GPIO_EnableIRQ(BSP_GPIO_USER_KEY); /* USER CODE INIT END */ while (1) { @@ -70,21 +112,54 @@ void Task_atti_esti(void *argument) { BMI088_ParseGyro(&bmi088); // IST8310_Parse(&ist8310); - /* 根据设备接收到的数据进行姿态解析 */ - AHRS_Update(&gimbal_ahrs, &bmi088.accl, &bmi088.gyro, &magn); + /* 陀螺仪校准处理 */ + 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++; - /* 根据解析出来的四元数计算欧拉角 */ - AHRS_GetEulr(&eulr_to_send, &gimbal_ahrs); + /* 达到采样数量后计算平均值并更新零偏 */ + 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; + + /* 校准完成，重置为空闲状态以便下次校准 */ + 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(); + /* 创建修改后的数据副本用于发送到消息队列 */ gimbal_to_send.eulr = eulr_to_send; gimbal_to_send.gyro = bmi088.gyro; osMessageQueueReset(task_runtime.msgq.gimbal.imu); osMessageQueuePut(task_runtime.msgq.gimbal.imu, &gimbal_to_send, 0, 0); + 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 +} + + diff --git a/User/task/rc.c b/User/task/rc.c index 075a964..8f23e7f 100644 --- a/User/task/rc.c +++ b/User/task/rc.c @@ -61,12 +61,12 @@ void Task_rc(void *argument) { cmd_for_chassis.mode = CHASSIS_MODE_RELAX; break; case DR16_SW_MID: - // cmd_for_chassis.mode = CHASSIS_MODE_RECOVER; - cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE; + cmd_for_chassis.mode = CHASSIS_MODE_RECOVER; + // cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE; break; case DR16_SW_DOWN: - // cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE; - cmd_for_chassis.mode = CHASSIS_MODE_JUMP; + cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE; + // cmd_for_chassis.mode = CHASSIS_MODE_JUMP; break; default: cmd_for_chassis.mode = CHASSIS_MODE_RELAX;