From e4b165569861bb3a383be2ce46f39fb03d1c7b0f Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Sun, 5 Oct 2025 03:25:18 +0800 Subject: [PATCH] =?UTF-8?q?imu=E5=AE=8C=E6=88=90?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeLists.txt | 1 + User/task/atti_esti.c | 162 +++++++++++++++++++++++++++++++++++ User/task/config.yaml | 7 ++ User/task/imu.c | 193 ++++++++++++++++++------------------------ User/task/init.c | 7 +- User/task/user_task.c | 5 ++ User/task/user_task.h | 14 +++ 7 files changed, 279 insertions(+), 110 deletions(-) create mode 100644 User/task/atti_esti.c diff --git a/CMakeLists.txt b/CMakeLists.txt index 4a4a3fe..b18dc79 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -69,6 +69,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE User/module/config.c # User/task sources + User/task/atti_esti.c User/task/blink.c User/task/ctrl_chassis.c User/task/imu.c diff --git a/User/task/atti_esti.c b/User/task/atti_esti.c new file mode 100644 index 0000000..c472415 --- /dev/null +++ b/User/task/atti_esti.c @@ -0,0 +1,162 @@ +/* + 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 "component/ahrs.h" +#include "component/pid.h" +#include "device/bmi088.h" +#include "task/user_task.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.00379243772f,0.00133061118f,-0.00154866849f}, +}; + +static const KPID_Params_t imu_temp_ctrl_pid_param = { + .k = 0.2f, + .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.00341676874f, 0.000505680335f, -0.00134522165f}; +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 */ + +/* Private function --------------------------------------------------------- */ +/* Exported functions ------------------------------------------------------- */ +void Task_atti_esti(void *argument) { + (void)argument; /* 未使用argument，消除警告 */ + + + /* 计算任务运行到指定频率需要等待的tick数 */ + const uint32_t delay_tick = osKernelGetTickFreq() / ATTI_ESTI_FREQ; + + osDelay(ATTI_ESTI_INIT_DELAY); /* 延时一段时间再开启任务 */ + + uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ + /* 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) { + tick += delay_tick; /* 计算下一个唤醒时刻 */ + /* 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 (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; + + /* 校准完成，重置为空闲状态以便下次校准 */ + 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(); + + osMessageQueueReset(task_runtime.msgq.imu.accl); + osMessageQueueReset(task_runtime.msgq.imu.gyro); + osMessageQueueReset(task_runtime.msgq.imu.eulr); + osMessageQueueReset(task_runtime.msgq.imu.quat); + osMessageQueuePut(task_runtime.msgq.imu.accl, &bmi088.accl, 0, 0); + osMessageQueuePut(task_runtime.msgq.imu.gyro, &bmi088.gyro, 0, 0); + osMessageQueuePut(task_runtime.msgq.imu.eulr, &eulr_to_send, 0, 0); + osMessageQueuePut(task_runtime.msgq.imu.quat, &gimbal_ahrs.quat, 0, 0); + + BSP_PWM_SetComp(BSP_PWM_IMU_HEAT_PWM, PID_Calc(&imu_temp_ctrl_pid, 50.0f, bmi088.temp, 0.0f, 0.0f)); + + /* USER CODE END */ + osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ + } + +} \ No newline at end of file diff --git a/User/task/config.yaml b/User/task/config.yaml index 66735ad..e093a96 100644 --- a/User/task/config.yaml +++ b/User/task/config.yaml @@ -19,3 +19,10 @@ function: Task_ctrl_chassis name: ctrl_chassis stack: 256 +- delay: 0 + description: '' + freq_control: true + frequency: 500.0 + function: Task_atti_esti + name: atti_esti + stack: 256 diff --git a/User/task/imu.c b/User/task/imu.c index 6d6f714..b09b5ef 100644 --- a/User/task/imu.c +++ b/User/task/imu.c @@ -6,70 +6,30 @@ /* Includes ----------------------------------------------------------------- */ #include "task/user_task.h" /* USER INCLUDE BEGIN */ -#include "bsp/mm.h" -#include "bsp/pwm.h" -#include "bsp/gpio.h" +#include "bsp/can.h" +#include "bsp/time.h" #include "component/ahrs.h" -#include "component/pid.h" -#include "device/bmi088.h" -#include "task/user_task.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 /* 四元数数据 */ /* 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}, -}; - -static const KPID_Params_t imu_temp_ctrl_pid_param = { - .k = 0.2f, - .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.00341676874f, 0.000505680335f, -0.00134522165f}; - -#define CALIB_SAMPLES 5000 // 校准采样数量 +AHRS_Accl_t accl; +AHRS_Gyro_t gyro; +AHRS_Eulr_t eulr; +AHRS_Quaternion_t quat; /* USER STRUCT END */ /* Private function --------------------------------------------------------- */ -/* 按钮回调函数：开始陀螺仪校准 */ -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; - } -} /* Exported functions ------------------------------------------------------- */ void Task_imu(void *argument) { (void)argument; /* 未使用argument，消除警告 */ @@ -82,73 +42,88 @@ void Task_imu(void *argument) { uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ /* 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); + BSP_CAN_Init(); /* 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); - BMI088_ParseGyro(&bmi088); - // IST8310_Parse(&ist8310); - - /* 陀螺仪校准处理 */ - 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; + /* 获取加速度计数据并通过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个int16_t，每个2字节，共6字节 */ - /* 校准完成，重置为空闲状态以便下次校准 */ - calib_state = CALIB_IDLE; + /* 将float转换为int16_t (乘以100，加速度一般在±20g范围内) */ + int16_t x_int = (int16_t)(accl.x * 100); + int16_t y_int = (int16_t)(accl.y * 100); + int16_t z_int = (int16_t)(accl.z * 100); - /* 重新初始化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); + memcpy(&accl_frame.data[0], &x_int, sizeof(int16_t)); + memcpy(&accl_frame.data[2], &y_int, sizeof(int16_t)); + memcpy(&accl_frame.data[4], &z_int, sizeof(int16_t)); + + BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &accl_frame); } - osKernelUnlock(); - - BSP_PWM_SetComp(BSP_PWM_IMU_HEAT_PWM, PID_Calc(&imu_temp_ctrl_pid, 50.0f, bmi088.temp, 0.0f, 0.0f)); + /* 获取陀螺仪数据并通过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个int16_t，每个2字节，共6字节 */ + + /* 将float转换为int16_t (乘以10，陀螺仪一般在±2000°/s范围内) */ + int16_t x_int = (int16_t)(gyro.x * 10); + int16_t y_int = (int16_t)(gyro.y * 10); + int16_t z_int = (int16_t)(gyro.z * 10); + + memcpy(&gyro_frame.data[0], &x_int, sizeof(int16_t)); + memcpy(&gyro_frame.data[2], &y_int, sizeof(int16_t)); + memcpy(&gyro_frame.data[4], &z_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个int16_t，每个2字节，共6字节 */ + + /* 将角度转换为int16_t (乘以100，角度范围-180~180°，精度0.01°) */ + int16_t yaw_int = (int16_t)(eulr.yaw * 100); + int16_t pit_int = (int16_t)(eulr.pit * 100); + int16_t rol_int = (int16_t)(eulr.rol * 100); + + memcpy(&eulr_frame.data[0], &yaw_int, sizeof(int16_t)); + memcpy(&eulr_frame.data[2], &pit_int, sizeof(int16_t)); + memcpy(&eulr_frame.data[4], &rol_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个int16_t，每个2字节，共8字节 */ + + /* 将四元数转换为int16_t (乘以32000，四元数范围-1~1，充分利用int16_t范围) */ + int16_t q0_int = (int16_t)(quat.q0 * 32000); + int16_t q1_int = (int16_t)(quat.q1 * 32000); + int16_t q2_int = (int16_t)(quat.q2 * 32000); + int16_t q3_int = (int16_t)(quat.q3 * 32000); + + 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); /* 运行结束，等待下一次唤醒 */ } -} +} \ No newline at end of file diff --git a/User/task/init.c b/User/task/init.c index cca7ac9..7e2eb5e 100644 --- a/User/task/init.c +++ b/User/task/init.c @@ -7,7 +7,7 @@ #include "task/user_task.h" /* USER INCLUDE BEGIN */ - +#include "component/ahrs.h" /* USER INCLUDE END */ /* Private typedef ---------------------------------------------------------- */ @@ -33,10 +33,15 @@ void Task_Init(void *argument) { task_runtime.thread.blink = osThreadNew(Task_blink, NULL, &attr_blink); task_runtime.thread.imu = osThreadNew(Task_imu, NULL, &attr_imu); task_runtime.thread.ctrl_chassis = osThreadNew(Task_ctrl_chassis, NULL, &attr_ctrl_chassis); + task_runtime.thread.atti_esti = osThreadNew(Task_atti_esti, NULL, &attr_atti_esti); // 创建消息队列 /* USER MESSAGE BEGIN */ task_runtime.msgq.user_msg= osMessageQueueNew(2u, 10, NULL); + task_runtime.msgq.imu.accl = osMessageQueueNew(2u, sizeof(AHRS_Accl_t), NULL); + task_runtime.msgq.imu.gyro = osMessageQueueNew(2u, sizeof(AHRS_Gyro_t), NULL); + task_runtime.msgq.imu.eulr = osMessageQueueNew(2u, sizeof(AHRS_Eulr_t), NULL); + task_runtime.msgq.imu.quat = osMessageQueueNew(2u, sizeof(AHRS_Quaternion_t), NULL); /* USER MESSAGE END */ osKernelUnlock(); // 解锁内核 diff --git a/User/task/user_task.c b/User/task/user_task.c index 764f790..62d7ab7 100644 --- a/User/task/user_task.c +++ b/User/task/user_task.c @@ -23,4 +23,9 @@ const osThreadAttr_t attr_ctrl_chassis = { .name = "ctrl_chassis", .priority = osPriorityNormal, .stack_size = 256 * 4, +}; +const osThreadAttr_t attr_atti_esti = { + .name = "atti_esti", + .priority = osPriorityNormal, + .stack_size = 256 * 4, }; \ No newline at end of file diff --git a/User/task/user_task.h b/User/task/user_task.h index 69d6e18..4ad3a9c 100644 --- a/User/task/user_task.h +++ b/User/task/user_task.h @@ -16,12 +16,14 @@ extern "C" { #define BLINK_FREQ (500.0) #define IMU_FREQ (500.0) #define CTRL_CHASSIS_FREQ (500.0) +#define ATTI_ESTI_FREQ (500.0) /* 任务初始化延时ms */ #define TASK_INIT_DELAY (100u) #define BLINK_INIT_DELAY (0) #define IMU_INIT_DELAY (0) #define CTRL_CHASSIS_INIT_DELAY (0) +#define ATTI_ESTI_INIT_DELAY (0) /* Exported defines --------------------------------------------------------- */ /* Exported macro ----------------------------------------------------------- */ @@ -34,11 +36,18 @@ typedef struct { osThreadId_t blink; osThreadId_t imu; osThreadId_t ctrl_chassis; + osThreadId_t atti_esti; } thread; /* USER MESSAGE BEGIN */ struct { osMessageQueueId_t user_msg; /* 用户自定义任务消息队列 */ + struct { + osMessageQueueId_t accl; + osMessageQueueId_t gyro; + osMessageQueueId_t eulr; + osMessageQueueId_t quat; + }imu; } msgq; /* USER MESSAGE END */ @@ -58,6 +67,7 @@ typedef struct { UBaseType_t blink; UBaseType_t imu; UBaseType_t ctrl_chassis; + UBaseType_t atti_esti; } stack_water_mark; /* 各任务运行频率 */ @@ -65,6 +75,7 @@ typedef struct { float blink; float imu; float ctrl_chassis; + float atti_esti; } freq; /* 任务最近运行时间 */ @@ -72,6 +83,7 @@ typedef struct { float blink; float imu; float ctrl_chassis; + float atti_esti; } last_up_time; } Task_Runtime_t; @@ -84,12 +96,14 @@ extern const osThreadAttr_t attr_init; extern const osThreadAttr_t attr_blink; extern const osThreadAttr_t attr_imu; extern const osThreadAttr_t attr_ctrl_chassis; +extern const osThreadAttr_t attr_atti_esti; /* 任务函数声明 */ void Task_Init(void *argument); void Task_blink(void *argument); void Task_imu(void *argument); void Task_ctrl_chassis(void *argument); +void Task_atti_esti(void *argument); #ifdef __cplusplus }