改imu

CMakeLists.txt

@@ -81,6 +81,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
 
     # User/task sources
     User/task/ai.c
+    User/task/atti_esti.c
     User/task/init.c
     User/task/rc.c
     User/task/user_task.c

Core/Inc/FreeRTOSConfig.h

@@ -68,7 +68,7 @@
 #define configTICK_RATE_HZ                       ((TickType_t)1000)
 #define configMAX_PRIORITIES                     ( 56 )
 #define configMINIMAL_STACK_SIZE                 ((uint16_t)128)
-#define configTOTAL_HEAP_SIZE                    ((size_t)15360)
+#define configTOTAL_HEAP_SIZE                    ((size_t)0x10000)
 #define configMAX_TASK_NAME_LEN                  ( 16 )
 #define configUSE_TRACE_FACILITY                 1
 #define configUSE_16_BIT_TICKS                   0

STM32H723XG_FLASH.ld

@@ -66,8 +66,8 @@ FLASH (rx)      : ORIGIN = 0x8000000, LENGTH = 1024K
 /* Highest address of the user mode stack */
 _estack = ORIGIN(DTCMRAM) + LENGTH(DTCMRAM);    /* end of RAM */
 /* Generate a link error if heap and stack don't fit into RAM */
-_Min_Heap_Size = 0x200;      /* required amount of heap  */
+_Min_Heap_Size = 0x1000;      /* required amount of heap  */
-_Min_Stack_Size = 0x800; /* required amount of stack */
+_Min_Stack_Size = 0x2000; /* required amount of stack */
 
 /* Define output sections */
 SECTIONS

User/task/atti_esti.c

@@ -0,0 +1,96 @@
+/*
+    atti_esti Task
+    
+*/
+
+/* 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 "task/user_task.h"
+/* USER INCLUDE END */
+
+/* Private typedef ---------------------------------------------------------- */
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private variables -------------------------------------------------------- */
+/* USER STRUCT BEGIN */
+static BMI088_t bmi088 __attribute__((section(".dma_buffer"))) __attribute__((aligned(32)));
+
+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.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,
+};
+
+/* 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);
+
+  /* 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);
+
+      /* 根据设备接收到的数据进行姿态解析 */
+      AHRS_Update(&gimbal_ahrs, &bmi088.accl, &bmi088.gyro, &magn);
+
+      /* 根据解析出来的四元数计算欧拉角 */
+      AHRS_GetEulr(&eulr_to_send, &gimbal_ahrs);
+    
+    osKernelUnlock();
+
+    /* USER CODE END */
+    osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
+  }
+  
+}

User/task/config.yaml

@@ -12,3 +12,10 @@
   function: Task_rc
   name: rc
   stack: 256
+- delay: 0
+  description: ''
+  freq_control: true
+  frequency: 500.0
+  function: Task_atti_esti
+  name: atti_esti
+  stack: 256

User/task/init.c

@@ -32,6 +32,7 @@ void Task_Init(void *argument) {
   /* 创建任务线程 */
   task_runtime.thread.ai = osThreadNew(Task_ai, NULL, &attr_ai);
   task_runtime.thread.rc = osThreadNew(Task_rc, NULL, &attr_rc);
+  task_runtime.thread.atti_esti = osThreadNew(Task_atti_esti, NULL, &attr_atti_esti);
 
   // 创建消息队列
   /* USER MESSAGE BEGIN */

User/task/rc.c

@@ -1,6 +1,6 @@
 /*
     rc Task
-    遥控器接收任务 - 处理DR16遥控器数据接收
+    
 */
 
 /* Includes ----------------------------------------------------------------- */

User/task/user_task.c

@@ -18,4 +18,9 @@ const osThreadAttr_t attr_rc = {
     .name = "rc",
     .priority = osPriorityNormal,
     .stack_size = 256 * 4,
+};
+const osThreadAttr_t attr_atti_esti = {
+    .name = "atti_esti",
+    .priority = osPriorityNormal,
+    .stack_size = 256 * 4,
 };

User/task/user_task.h

@@ -15,11 +15,13 @@ extern "C" {
 /* 任务运行频率 */
 #define AI_FREQ (500.0)
 #define RC_FREQ (500.0)
+#define ATTI_ESTI_FREQ (500.0)
 
 /* 任务初始化延时ms */
 #define TASK_INIT_DELAY (100u)
 #define AI_INIT_DELAY (0)
 #define RC_INIT_DELAY (0)
+#define ATTI_ESTI_INIT_DELAY (0)
 
 /* Exported defines --------------------------------------------------------- */
 /* Exported macro ----------------------------------------------------------- */
@@ -31,6 +33,7 @@ typedef struct {
     struct {
         osThreadId_t ai;
         osThreadId_t rc;
+        osThreadId_t atti_esti;
     } thread;
 
     /* USER MESSAGE BEGIN */
@@ -54,18 +57,21 @@ typedef struct {
     struct {
         UBaseType_t ai;
         UBaseType_t rc;
+        UBaseType_t atti_esti;
     } stack_water_mark;
 
     /* 各任务运行频率 */
     struct {
         float ai;
         float rc;
+        float atti_esti;
     } freq;
 
     /* 任务最近运行时间 */
     struct {
         float ai;
         float rc;
+        float atti_esti;
     } last_up_time;
 
 } Task_Runtime_t;
@@ -77,11 +83,13 @@ extern Task_Runtime_t task_runtime;
 extern const osThreadAttr_t attr_init;
 extern const osThreadAttr_t attr_ai;
 extern const osThreadAttr_t attr_rc;
+extern const osThreadAttr_t attr_atti_esti;
 
 /* 任务函数声明 */
 void Task_Init(void *argument);
 void Task_ai(void *argument);
 void Task_rc(void *argument);
+void Task_atti_esti(void *argument);
 
 #ifdef __cplusplus
 }

balance_infantry.ioc

@@ -391,8 +391,9 @@ FDCAN3.StdFiltersNbr=1
 FDCAN3.TxBuffersNbr=0
 FDCAN3.TxElmtSize=FDCAN_DATA_BYTES_64
 FDCAN3.TxFifoQueueElmtsNbr=23
-FREERTOS.IPParameters=Tasks01
+FREERTOS.IPParameters=Tasks01,configTOTAL_HEAP_SIZE
 FREERTOS.Tasks01=defaultTask,24,128,StartDefaultTask,Default,NULL,Dynamic,NULL,NULL
+FREERTOS.configTOTAL_HEAP_SIZE=0x10000
 File.Version=6
 GPIO.groupedBy=Group By Peripherals
 KeepUserPlacement=false
@@ -698,7 +699,7 @@ ProjectManager.DeviceId=STM32H723VGTx
 ProjectManager.FirmwarePackage=STM32Cube FW_H7 V1.12.1
 ProjectManager.FreePins=false
 ProjectManager.HalAssertFull=false
-ProjectManager.HeapSize=0x200
+ProjectManager.HeapSize=0x1000
 ProjectManager.KeepUserCode=true
 ProjectManager.LastFirmware=true
 ProjectManager.LibraryCopy=0
@@ -710,7 +711,7 @@ ProjectManager.ProjectFileName=balance_infantry.ioc
 ProjectManager.ProjectName=balance_infantry
 ProjectManager.ProjectStructure=
 ProjectManager.RegisterCallBack=
-ProjectManager.StackSize=0x800
+ProjectManager.StackSize=0x2000
 ProjectManager.TargetToolchain=CMake
 ProjectManager.ToolChainLocation=
 ProjectManager.UAScriptAfterPath=