改发送

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); /* 运行结束，等待下一次唤醒 */
+  }
+  
+}

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));