diff --git a/User/task/1.c b/User/task/1.c new file mode 100644 index 0000000..73d130b --- /dev/null +++ b/User/task/1.c @@ -0,0 +1,95 @@ +/* + 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 c472415..2c1f583 100644 --- a/User/task/atti_esti.c +++ b/User/task/atti_esti.c @@ -144,13 +144,34 @@ void Task_atti_esti(void *argument) { osKernelUnlock(); + /* 创建修改后的数据副本用于发送到消息队列 */ + AHRS_Accl_t accl_modified; + AHRS_Gyro_t gyro_modified; + AHRS_Eulr_t eulr_modified; + + /* 加速度数据:x和y互换,y取负值,全部乘10 */ + accl_modified.x = bmi088.accl.y * 10.0f; + accl_modified.y = -bmi088.accl.x * 10.0f; + accl_modified.z = bmi088.accl.z * 10.0f; + + + /* 陀螺仪数据:x和y互换,y取负值 */ + gyro_modified.x = bmi088.gyro.y; + gyro_modified.y = -bmi088.gyro.x; + gyro_modified.z = bmi088.gyro.z; + + /* 欧拉角数据:roll和pitch互换 */ + eulr_modified.yaw = eulr_to_send.yaw; + eulr_modified.pit = eulr_to_send.rol; /* pitch = 原roll */ + eulr_modified.rol = eulr_to_send.pit; /* roll = 原pitch */ + 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.accl, &accl_modified, 0, 0); + osMessageQueuePut(task_runtime.msgq.imu.gyro, &gyro_modified, 0, 0); + osMessageQueuePut(task_runtime.msgq.imu.eulr, &eulr_modified, 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));