Merge branch 'main' into 单独底层纯转发

CMakeLists.txt

@@ -47,12 +47,31 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
     # Add user sources here
     # User/bsp sources
     User/bsp/can.c
+<<<<<<< HEAD
+=======
     User/bsp/dwt.c
+>>>>>>> main
     User/bsp/gpio.c
     User/bsp/mm.c
     User/bsp/pwm.c
     User/bsp/spi.c
     User/bsp/time.c
+<<<<<<< HEAD
+
+    # User/component sources
+    User/component/ahrs.c
+    User/component/filter.c
+    User/component/pid.c
+    User/component/user_math.c
+
+    # User/device sources
+    User/device/bmi088.c
+    User/device/motor.c
+    User/device/motor_lk.c
+    User/device/motor_lz.c
+
+    # User/module 
+=======
     User/bsp/uart.c
 
     # User/component sources
@@ -78,6 +97,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
     User/device/rc_can.c
 
     # User/module sources
+>>>>>>> main
     User/module/balance_chassis.c
     User/module/config.c
 
@@ -85,9 +105,14 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
     User/task/atti_esti.c
     User/task/blink.c
     User/task/ctrl_chassis.c
+<<<<<<< HEAD
+    User/task/imu.c
+    User/task/init.c
+=======
     User/task/ctrl_gimbal.c
     User/task/init.c
     User/task/rc.c
+>>>>>>> main
     User/task/user_task.c
 )
 

CMakePresets.json

@@ -18,6 +18,8 @@
             }
         },
         {
+<<<<<<< HEAD
+=======
             "name": "RelWithDebInfo",
             "inherits": "default",
             "cacheVariables": {
@@ -25,11 +27,14 @@
             }
         },
         {
+>>>>>>> main
             "name": "Release",
             "inherits": "default",
             "cacheVariables": {
                 "CMAKE_BUILD_TYPE": "Release"
             }
+<<<<<<< HEAD
+=======
         },
         {
             "name": "MinSizeRel",
@@ -37,6 +42,7 @@
             "cacheVariables": {
                 "CMAKE_BUILD_TYPE": "MinSizeRel"
             }
+>>>>>>> main
         }
     ],
     "buildPresets": [
@@ -45,6 +51,10 @@
             "configurePreset": "Debug"
         },
         {
+<<<<<<< HEAD
+            "name": "Release",
+            "configurePreset": "Release"
+=======
             "name": "RelWithDebInfo",
             "configurePreset": "RelWithDebInfo"
         },
@@ -55,6 +65,7 @@
         {
             "name": "MinSizeRel",
             "configurePreset": "MinSizeRel"
+>>>>>>> main
         }
     ]
 }

Core/Inc/FreeRTOSConfig.h

@@ -187,7 +187,7 @@ standard names. */
 
 /* USER CODE BEGIN Defines */
 /* Section where parameter definitions can be added (for instance, to override default ones in FreeRTOS.h) */
-#define configAPPLICATION_ALLOCATED_HEAP         1
+// #define configAPPLICATION_ALLOCATED_HEAP         1
 /* USER CODE END Defines */
 
 #endif /* FREERTOS_CONFIG_H */

Core/Inc/stm32f4xx_it.h

@@ -59,6 +59,7 @@ void EXTI3_IRQHandler(void);
 void EXTI4_IRQHandler(void);
 void DMA1_Stream1_IRQHandler(void);
 void DMA1_Stream2_IRQHandler(void);
+void CAN1_TX_IRQHandler(void);
 void CAN1_RX0_IRQHandler(void);
 void CAN1_RX1_IRQHandler(void);
 void EXTI9_5_IRQHandler(void);
@@ -69,6 +70,7 @@ void TIM7_IRQHandler(void);
 void DMA2_Stream1_IRQHandler(void);
 void DMA2_Stream2_IRQHandler(void);
 void DMA2_Stream3_IRQHandler(void);
+void CAN2_TX_IRQHandler(void);
 void CAN2_RX0_IRQHandler(void);
 void CAN2_RX1_IRQHandler(void);
 void OTG_FS_IRQHandler(void);

Core/Src/can.c

@@ -47,7 +47,7 @@ void MX_CAN1_Init(void)
   hcan1.Init.TimeTriggeredMode = DISABLE;
   hcan1.Init.AutoBusOff = DISABLE;
   hcan1.Init.AutoWakeUp = DISABLE;
-  hcan1.Init.AutoRetransmission = DISABLE;
+  hcan1.Init.AutoRetransmission = ENABLE;
   hcan1.Init.ReceiveFifoLocked = DISABLE;
   hcan1.Init.TransmitFifoPriority = ENABLE;
   if (HAL_CAN_Init(&hcan1) != HAL_OK)
@@ -79,7 +79,7 @@ void MX_CAN2_Init(void)
   hcan2.Init.TimeTriggeredMode = DISABLE;
   hcan2.Init.AutoBusOff = DISABLE;
   hcan2.Init.AutoWakeUp = DISABLE;
-  hcan2.Init.AutoRetransmission = DISABLE;
+  hcan2.Init.AutoRetransmission = ENABLE;
   hcan2.Init.ReceiveFifoLocked = DISABLE;
   hcan2.Init.TransmitFifoPriority = ENABLE;
   if (HAL_CAN_Init(&hcan2) != HAL_OK)
@@ -122,6 +122,8 @@ void HAL_CAN_MspInit(CAN_HandleTypeDef* canHandle)
     HAL_GPIO_Init(GPIOD, &GPIO_InitStruct);
 
     /* CAN1 interrupt Init */
+    HAL_NVIC_SetPriority(CAN1_TX_IRQn, 5, 0);
+    HAL_NVIC_EnableIRQ(CAN1_TX_IRQn);
     HAL_NVIC_SetPriority(CAN1_RX0_IRQn, 5, 0);
     HAL_NVIC_EnableIRQ(CAN1_RX0_IRQn);
     HAL_NVIC_SetPriority(CAN1_RX1_IRQn, 5, 0);
@@ -155,6 +157,8 @@ void HAL_CAN_MspInit(CAN_HandleTypeDef* canHandle)
     HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
 
     /* CAN2 interrupt Init */
+    HAL_NVIC_SetPriority(CAN2_TX_IRQn, 5, 0);
+    HAL_NVIC_EnableIRQ(CAN2_TX_IRQn);
     HAL_NVIC_SetPriority(CAN2_RX0_IRQn, 5, 0);
     HAL_NVIC_EnableIRQ(CAN2_RX0_IRQn);
     HAL_NVIC_SetPriority(CAN2_RX1_IRQn, 5, 0);
@@ -186,6 +190,7 @@ void HAL_CAN_MspDeInit(CAN_HandleTypeDef* canHandle)
     HAL_GPIO_DeInit(GPIOD, GPIO_PIN_0|GPIO_PIN_1);
 
     /* CAN1 interrupt Deinit */
+    HAL_NVIC_DisableIRQ(CAN1_TX_IRQn);
     HAL_NVIC_DisableIRQ(CAN1_RX0_IRQn);
     HAL_NVIC_DisableIRQ(CAN1_RX1_IRQn);
   /* USER CODE BEGIN CAN1_MspDeInit 1 */
@@ -211,6 +216,7 @@ void HAL_CAN_MspDeInit(CAN_HandleTypeDef* canHandle)
     HAL_GPIO_DeInit(GPIOB, GPIO_PIN_5|GPIO_PIN_6);
 
     /* CAN2 interrupt Deinit */
+    HAL_NVIC_DisableIRQ(CAN2_TX_IRQn);
     HAL_NVIC_DisableIRQ(CAN2_RX0_IRQn);
     HAL_NVIC_DisableIRQ(CAN2_RX1_IRQn);
   /* USER CODE BEGIN CAN2_MspDeInit 1 */

Core/Src/stm32f4xx_it.c

@@ -269,6 +269,20 @@ void DMA1_Stream2_IRQHandler(void)
   /* USER CODE END DMA1_Stream2_IRQn 1 */
 }
 
+/**
+  * @brief This function handles CAN1 TX interrupts.
+  */
+void CAN1_TX_IRQHandler(void)
+{
+  /* USER CODE BEGIN CAN1_TX_IRQn 0 */
+
+  /* USER CODE END CAN1_TX_IRQn 0 */
+  HAL_CAN_IRQHandler(&hcan1);
+  /* USER CODE BEGIN CAN1_TX_IRQn 1 */
+
+  /* USER CODE END CAN1_TX_IRQn 1 */
+}
+
 /**
   * @brief This function handles CAN1 RX0 interrupts.
   */
@@ -409,6 +423,20 @@ void DMA2_Stream3_IRQHandler(void)
   /* USER CODE END DMA2_Stream3_IRQn 1 */
 }
 
+/**
+  * @brief This function handles CAN2 TX interrupts.
+  */
+void CAN2_TX_IRQHandler(void)
+{
+  /* USER CODE BEGIN CAN2_TX_IRQn 0 */
+
+  /* USER CODE END CAN2_TX_IRQn 0 */
+  HAL_CAN_IRQHandler(&hcan2);
+  /* USER CODE BEGIN CAN2_TX_IRQn 1 */
+
+  /* USER CODE END CAN2_TX_IRQn 1 */
+}
+
 /**
   * @brief This function handles CAN2 RX0 interrupts.
   */

Core/Src/syscalls.c

@@ -6,11 +6,19 @@
  *
  *            For more information about which c-functions
  *            need which of these lowlevel functions
+<<<<<<< HEAD
+ *            please consult the Newlib or Picolibc libc-manual
+ ******************************************************************************
+ * @attention
+ *
+ * Copyright (c) 2020-2025 STMicroelectronics.
+=======
  *            please consult the Newlib libc-manual
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2020-2024 STMicroelectronics.
+>>>>>>> main
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
@@ -139,13 +147,21 @@ int _unlink(char *name)
   return -1;
 }
 
+<<<<<<< HEAD
+clock_t _times(struct tms *buf)
+=======
 int _times(struct tms *buf)
+>>>>>>> main
 {
   (void)buf;
   return -1;
 }
 
+<<<<<<< HEAD
+int _stat(const char *file, struct stat *st)
+=======
 int _stat(char *file, struct stat *st)
+>>>>>>> main
 {
   (void)file;
   st->st_mode = S_IFCHR;
@@ -174,3 +190,74 @@ int _execve(char *name, char **argv, char **env)
   errno = ENOMEM;
   return -1;
 }
+<<<<<<< HEAD
+
+// --- Picolibc Specific Section ---
+#if defined(__PICOLIBC__)
+
+/**
+ * @brief Picolibc helper function to output a character to a FILE stream.
+ *        This redirects the output to the low-level __io_putchar function.
+ * @param c Character to write.
+ * @param file FILE stream pointer (ignored).
+ * @retval int The character written.
+ */
+static int starm_putc(char c, FILE *file)
+{
+	(void) file;
+  __io_putchar(c);
+	return c;
+}
+
+/**
+ * @brief Picolibc helper function to input a character from a FILE stream.
+ *        This redirects the input from the low-level __io_getchar function.
+ * @param file FILE stream pointer (ignored).
+ * @retval int The character read, cast to an unsigned char then int.
+ */
+static int starm_getc(FILE *file)
+{
+	unsigned char c;
+	(void) file;
+  c = __io_getchar();
+	return c;
+}
+
+// Define and initialize the standard I/O streams for Picolibc.
+// FDEV_SETUP_STREAM connects the starm_putc and starm_getc helper functions to a FILE structure.
+// _FDEV_SETUP_RW indicates the stream is for reading and writing.
+static FILE __stdio = FDEV_SETUP_STREAM(starm_putc,
+					starm_getc,
+					NULL,
+					_FDEV_SETUP_RW);
+
+// Assign the standard stream pointers (stdin, stdout, stderr) to the initialized stream.
+// Picolibc uses these pointers for standard I/O operations (printf, scanf, etc.).
+FILE *const stdin = &__stdio;
+__strong_reference(stdin, stdout);
+__strong_reference(stdin, stderr);
+
+// Create strong aliases mapping standard C library function names (without underscore)
+// to the implemented system call stubs (with underscore). Picolibc uses these
+// standard names internally, so this linking is required.
+__strong_reference(_read, read);
+__strong_reference(_write, write);
+__strong_reference(_times, times);
+__strong_reference(_execve, execve);
+__strong_reference(_fork, fork);
+__strong_reference(_link, link);
+__strong_reference(_unlink, unlink);
+__strong_reference(_stat, stat);
+__strong_reference(_wait, wait);
+__strong_reference(_open, open);
+__strong_reference(_close, close);
+__strong_reference(_lseek, lseek);
+__strong_reference(_isatty, isatty);
+__strong_reference(_fstat, fstat);
+__strong_reference(_exit, exit);
+__strong_reference(_kill, kill);
+__strong_reference(_getpid, getpid);
+
+#endif //__PICOLIBC__
+=======
+>>>>>>> main

Core/Src/sysmem.c

@@ -6,11 +6,19 @@
  *
  *            For more information about which C functions
  *            need which of these lowlevel functions
+<<<<<<< HEAD
+ *            please consult the Newlib or Picolibc libc manual
+ ******************************************************************************
+ * @attention
+ *
+ * Copyright (c) 2025 STMicroelectronics.
+=======
  *            please consult the newlib libc manual
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2024 STMicroelectronics.
+>>>>>>> main
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
@@ -23,6 +31,10 @@
 /* Includes */
 #include <errno.h>
 #include <stdint.h>
+<<<<<<< HEAD
+#include <stddef.h>
+=======
+>>>>>>> main
 
 /**
  * Pointer to the current high watermark of the heap usage
@@ -77,3 +89,13 @@ void *_sbrk(ptrdiff_t incr)
 
   return (void *)prev_heap_end;
 }
+<<<<<<< HEAD
+
+#if defined(__PICOLIBC__)
+  // Picolibc expects syscalls without the leading underscore.
+  // This creates a strong alias so that
+  // calls to `sbrk()` are resolved to our `_sbrk()` implementation.
+  __strong_reference(_sbrk, sbrk);
+#endif
+=======
+>>>>>>> main

DevC.ioc

@@ -21,7 +21,8 @@ CAN1.BS2=CAN_BS2_7TQ
 CAN1.CalculateBaudRate=1000000
 CAN1.CalculateTimeBit=1000
 CAN1.CalculateTimeQuantum=71.42857142857143
-CAN1.IPParameters=CalculateTimeQuantum,BS1,BS2,Prescaler,TXFP,ABOM,CalculateTimeBit,CalculateBaudRate
+CAN1.IPParameters=CalculateTimeQuantum,BS1,BS2,Prescaler,TXFP,ABOM,CalculateTimeBit,CalculateBaudRate,NART
+CAN1.NART=ENABLE
 CAN1.Prescaler=3
 CAN1.TXFP=ENABLE
 CAN2.BS1=CAN_BS1_6TQ
@@ -29,7 +30,8 @@ CAN2.BS2=CAN_BS2_7TQ
 CAN2.CalculateBaudRate=1000000
 CAN2.CalculateTimeBit=1000
 CAN2.CalculateTimeQuantum=71.42857142857143
-CAN2.IPParameters=CalculateTimeQuantum,BS1,BS2,Prescaler,TXFP,CalculateTimeBit,CalculateBaudRate
+CAN2.IPParameters=CalculateTimeQuantum,BS1,BS2,Prescaler,TXFP,CalculateTimeBit,CalculateBaudRate,NART
+CAN2.NART=ENABLE
 CAN2.Prescaler=3
 CAN2.TXFP=ENABLE
 Dma.I2C2_TX.2.Direction=DMA_MEMORY_TO_PERIPH
@@ -268,8 +270,10 @@ MxDb.Version=DB.6.0.150
 NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
 NVIC.CAN1_RX0_IRQn=true\:5\:0\:true\:false\:true\:true\:true\:true\:true
 NVIC.CAN1_RX1_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
+NVIC.CAN1_TX_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
 NVIC.CAN2_RX0_IRQn=true\:5\:0\:true\:false\:true\:true\:true\:true\:true
 NVIC.CAN2_RX1_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
+NVIC.CAN2_TX_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
 NVIC.DMA1_Stream1_IRQn=true\:5\:0\:true\:false\:true\:true\:false\:true\:true
 NVIC.DMA1_Stream2_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
 NVIC.DMA1_Stream7_IRQn=true\:5\:0\:true\:false\:true\:true\:false\:true\:true

STM32F407XX_FLASH.ld

@@ -52,12 +52,15 @@
 /* Entry Point */
 ENTRY(Reset_Handler)
 
+<<<<<<< HEAD
+=======
 /* Highest address of the user mode stack */
 _estack = ORIGIN(RAM) + LENGTH(RAM);    /* end of RAM */
 /* Generate a link error if heap and stack don't fit into RAM */
 _Min_Heap_Size = 0x1000;      /* required amount of heap  */
 _Min_Stack_Size = 0x1000; /* required amount of stack */
 
+>>>>>>> main
 /* Specify the memory areas */
 MEMORY
 {
@@ -66,6 +69,15 @@ CCMRAM (xrw)      : ORIGIN = 0x10000000, LENGTH = 64K
 FLASH (rx)      : ORIGIN = 0x8000000, LENGTH = 1024K
 }
 
+<<<<<<< HEAD
+/* Highest address of the user mode stack */
+_estack = ORIGIN(RAM) + LENGTH(RAM);    /* end of RAM */
+/* Generate a link error if heap and stack don't fit into RAM */
+_Min_Heap_Size = 0x1000;      /* required amount of heap  */
+_Min_Stack_Size = 0x1000; /* required amount of stack */
+
+=======
+>>>>>>> main
 /* Define output sections */
 SECTIONS
 {
@@ -148,6 +160,8 @@ SECTIONS
     . = ALIGN(4);
   } >FLASH
 
+<<<<<<< HEAD
+=======
   /* used by the startup to initialize data */
   _sidata = LOADADDR(.data);
 
@@ -165,6 +179,7 @@ SECTIONS
     _edata = .;        /* define a global symbol at data end */
   } >RAM AT> FLASH
 
+>>>>>>> main
   _siccmram = LOADADDR(.ccmram);
 
   /* CCM-RAM section
@@ -184,6 +199,72 @@ SECTIONS
     _eccmram = .;       /* create a global symbol at ccmram end */
   } >CCMRAM AT> FLASH
 
+<<<<<<< HEAD
+  /* used by the startup to initialize data */
+  _sidata = LOADADDR(.data);
+
+  /* Initialized data sections goes into RAM, load LMA copy after code */
+  .data :
+  {
+    . = ALIGN(4);
+    _sdata = .;        /* create a global symbol at data start */
+    *(.data)           /* .data sections */
+    *(.data*)          /* .data* sections */
+    *(.RamFunc)        /* .RamFunc sections */
+    *(.RamFunc*)       /* .RamFunc* sections */
+
+    . = ALIGN(4);
+  } >RAM AT> FLASH
+
+ /* Initialized TLS data section */
+  .tdata : ALIGN(4)
+  {
+    *(.tdata .tdata.* .gnu.linkonce.td.*)
+    . = ALIGN(4);
+    _edata = .;        /* define a global symbol at data end */
+    PROVIDE(__data_end = .);
+    PROVIDE(__tdata_end = .);
+  } >RAM AT> FLASH
+
+  PROVIDE( __tdata_start = ADDR(.tdata) );
+  PROVIDE( __tdata_size = __tdata_end - __tdata_start );
+
+  PROVIDE( __data_start = ADDR(.data) );
+  PROVIDE( __data_size = __data_end - __data_start );
+
+  PROVIDE( __tdata_source = LOADADDR(.tdata) );
+  PROVIDE( __tdata_source_end = LOADADDR(.tdata) + SIZEOF(.tdata) );
+  PROVIDE( __tdata_source_size = __tdata_source_end - __tdata_source );
+
+  PROVIDE( __data_source = LOADADDR(.data) );
+  PROVIDE( __data_source_end = __tdata_source_end );
+  PROVIDE( __data_source_size = __data_source_end - __data_source );
+  /* Uninitialized data section */
+  .tbss (NOLOAD) : ALIGN(4)
+  {
+     /* This is used by the startup in order to initialize the .bss secion */
+    _sbss = .;         /* define a global symbol at bss start */
+    __bss_start__ = _sbss;
+    *(.tbss .tbss.*)
+    . = ALIGN(4);
+    PROVIDE( __tbss_end = . );
+  } >RAM
+
+  PROVIDE( __tbss_start = ADDR(.tbss) );
+  PROVIDE( __tbss_size = __tbss_end - __tbss_start );
+  PROVIDE( __tbss_offset = ADDR(.tbss) - ADDR(.tdata) );
+
+  PROVIDE( __tls_base = __tdata_start );
+  PROVIDE( __tls_end = __tbss_end );
+  PROVIDE( __tls_size = __tls_end - __tls_base );
+  PROVIDE( __tls_align = MAX(ALIGNOF(.tdata), ALIGNOF(.tbss)) );
+  PROVIDE( __tls_size_align = (__tls_size + __tls_align - 1) & ~(__tls_align - 1) );
+  PROVIDE( __arm32_tls_tcb_offset = MAX(8, __tls_align) );
+  PROVIDE( __arm64_tls_tcb_offset = MAX(16, __tls_align) );
+
+  .bss (NOLOAD) : ALIGN(4)
+  {
+=======
 
   /* Uninitialized data section */
   . = ALIGN(4);
@@ -192,10 +273,25 @@ SECTIONS
     /* This is used by the startup in order to initialize the .bss secion */
     _sbss = .;         /* define a global symbol at bss start */
     __bss_start__ = _sbss;
+>>>>>>> main
     *(.bss)
     *(.bss*)
     *(COMMON)
 
+<<<<<<< HEAD
+      . = ALIGN(4);
+    _ebss = .;         /* define a global symbol at bss end */
+    __bss_end__ = _ebss;
+      PROVIDE( __bss_end = .);
+  } >RAM
+  PROVIDE( __non_tls_bss_start = ADDR(.bss) );
+
+  PROVIDE( __bss_start = __tbss_start );
+  PROVIDE( __bss_size = __bss_end - __bss_start );
+
+  /* User_heap_stack section, used to check that there is enough RAM left */
+  ._user_heap_stack (NOLOAD) :
+=======
     . = ALIGN(4);
     _ebss = .;         /* define a global symbol at bss end */
     __bss_end__ = _ebss;
@@ -203,6 +299,7 @@ SECTIONS
 
   /* User_heap_stack section, used to check that there is enough RAM left */
   ._user_heap_stack :
+>>>>>>> main
   {
     . = ALIGN(8);
     PROVIDE ( end = . );
@@ -217,6 +314,14 @@ SECTIONS
   /* Remove information from the standard libraries */
   /DISCARD/ :
   {
+<<<<<<< HEAD
+    libc.a:* ( * )
+    libm.a:* ( * )
+    libgcc.a:* ( * )
+  }
+
+}
+=======
     libc.a ( * )
     libm.a ( * )
     libgcc.a ( * )
@@ -225,3 +330,4 @@ SECTIONS
 }
 
 
+>>>>>>> main

User/bsp/bsp_config.yaml

@@ -5,8 +5,11 @@ can:
   - instance: CAN2
     name: '2'
   enabled: true
+<<<<<<< HEAD
+=======
 dwt:
   enabled: true
+>>>>>>> main
 gpio:
   configs:
   - custom_name: USER_KEY
@@ -125,8 +128,11 @@ spi:
   enabled: true
 time:
   enabled: true
+<<<<<<< HEAD
+=======
 uart:
   devices:
   - instance: USART3
     name: DR16
   enabled: true
+>>>>>>> main

User/bsp/can.c

@@ -37,16 +37,32 @@ static osMutexId_t queue_mutex = NULL;
 static void (*CAN_Callback[BSP_CAN_NUM][BSP_CAN_CB_NUM])(void);
 static bool inited = false;
 static BSP_CAN_IdParser_t id_parser = NULL; /* ID解析器 */
+<<<<<<< HEAD
+static BSP_CAN_TxQueue_t tx_queues[BSP_CAN_NUM]; /* 每个CAN的发送队列 */
+=======
+>>>>>>> main
 
 /* Private function prototypes ---------------------------------------------- */
 static BSP_CAN_t CAN_Get(CAN_HandleTypeDef *hcan);
 static osMessageQueueId_t BSP_CAN_FindQueue(BSP_CAN_t can, uint32_t can_id);
 static int8_t BSP_CAN_CreateIdQueue(BSP_CAN_t can, uint32_t can_id, uint8_t queue_size);
+<<<<<<< HEAD
+static void BSP_CAN_RxFifo0Callback(void);
+static void BSP_CAN_RxFifo1Callback(void);
+static void BSP_CAN_TxCompleteCallback(void);
+static BSP_CAN_FrameType_t BSP_CAN_GetFrameType(CAN_RxHeaderTypeDef *header);
+static uint32_t BSP_CAN_DefaultIdParser(uint32_t original_id, BSP_CAN_FrameType_t frame_type);
+static void BSP_CAN_TxQueueInit(BSP_CAN_t can);
+static bool BSP_CAN_TxQueuePush(BSP_CAN_t can, BSP_CAN_TxMessage_t *msg);
+static bool BSP_CAN_TxQueuePop(BSP_CAN_t can, BSP_CAN_TxMessage_t *msg);
+static bool BSP_CAN_TxQueueIsEmpty(BSP_CAN_t can);
+=======
 static int8_t BSP_CAN_DeleteIdQueue(BSP_CAN_t can, uint32_t can_id);
 static void BSP_CAN_RxFifo0Callback(void);
 static void BSP_CAN_RxFifo1Callback(void);
 static BSP_CAN_FrameType_t BSP_CAN_GetFrameType(CAN_RxHeaderTypeDef *header);
 static uint32_t BSP_CAN_DefaultIdParser(uint32_t original_id, BSP_CAN_FrameType_t frame_type);
+>>>>>>> main
 
 /* Private functions -------------------------------------------------------- */
 /* USER FUNCTION BEGIN */
@@ -121,6 +137,9 @@ static int8_t BSP_CAN_CreateIdQueue(BSP_CAN_t can, uint32_t can_id, uint8_t queu
     return BSP_OK;
 }
 
+<<<<<<< HEAD
+
+=======
 /**
  * @brief 删除指定CAN ID的消息队列
  * @note 内部函数，已包含互斥锁保护
@@ -144,6 +163,7 @@ static int8_t BSP_CAN_DeleteIdQueue(BSP_CAN_t can, uint32_t can_id) {
     osMutexRelease(queue_mutex);
     return BSP_ERR;  // 未找到
 }
+>>>>>>> main
 /**
  * @brief 获取帧类型
  */
@@ -164,6 +184,109 @@ static uint32_t BSP_CAN_DefaultIdParser(uint32_t original_id, BSP_CAN_FrameType_
 }
 
 /**
+<<<<<<< HEAD
+ * @brief 初始化发送队列
+ */
+static void BSP_CAN_TxQueueInit(BSP_CAN_t can) {
+    if (can >= BSP_CAN_NUM) return;
+    
+    tx_queues[can].head = 0;
+    tx_queues[can].tail = 0;
+}
+
+/**
+ * @brief 向发送队列添加消息（无锁）
+ */
+static bool BSP_CAN_TxQueuePush(BSP_CAN_t can, BSP_CAN_TxMessage_t *msg) {
+    if (can >= BSP_CAN_NUM || msg == NULL) return false;
+    
+    BSP_CAN_TxQueue_t *queue = &tx_queues[can];
+    uint32_t next_head = (queue->head + 1) % BSP_CAN_TX_QUEUE_SIZE;
+    
+    // 队列满
+    if (next_head == queue->tail) {
+        return false;
+    }
+    
+    // 复制消息
+    queue->buffer[queue->head] = *msg;
+    
+    // 更新头指针（原子操作）
+    queue->head = next_head;
+    
+    return true;
+}
+
+
+/**
+ * @brief 从发送队列取出消息（无锁）
+ */
+static bool BSP_CAN_TxQueuePop(BSP_CAN_t can, BSP_CAN_TxMessage_t *msg) {
+    if (can >= BSP_CAN_NUM || msg == NULL) return false;
+    
+    BSP_CAN_TxQueue_t *queue = &tx_queues[can];
+    
+    // 队列空
+    if (queue->head == queue->tail) {
+        return false;
+    }
+    
+    // 复制消息
+    *msg = queue->buffer[queue->tail];
+    
+    // 更新尾指针（原子操作）
+    queue->tail = (queue->tail + 1) % BSP_CAN_TX_QUEUE_SIZE;
+    
+    return true;
+}
+
+/**
+ * @brief 检查发送队列是否为空
+ */
+static bool BSP_CAN_TxQueueIsEmpty(BSP_CAN_t can) {
+    if (can >= BSP_CAN_NUM) return true;
+    
+    return tx_queues[can].head == tx_queues[can].tail;
+}
+
+/**
+ * @brief 处理所有CAN实例的发送队列
+ */
+static void BSP_CAN_TxCompleteCallback(void) {
+    // 处理所有CAN实例的发送队列
+    for (int i = 0; i < BSP_CAN_NUM; i++) {
+        BSP_CAN_t can = (BSP_CAN_t)i;
+        CAN_HandleTypeDef *hcan = BSP_CAN_GetHandle(can);
+        if (hcan == NULL) continue;
+        
+        BSP_CAN_TxMessage_t msg;
+        uint32_t mailbox;
+        
+        // 尝试发送队列中的消息
+        while (!BSP_CAN_TxQueueIsEmpty(can)) {
+            // 检查是否有空闲邮箱
+            if (HAL_CAN_GetTxMailboxesFreeLevel(hcan) == 0) {
+                break; // 没有空闲邮箱，等待下次中断
+            }
+            
+            // 从队列中取出消息
+            if (!BSP_CAN_TxQueuePop(can, &msg)) {
+                break;
+            }
+            
+            // 发送消息
+            if (HAL_CAN_AddTxMessage(hcan, &msg.header, msg.data, &mailbox) != HAL_OK) {
+                // 发送失败，消息已经从队列中移除，直接丢弃
+                break;
+            }
+        }
+    }
+}
+
+
+/**
+=======
+>>>>>>> main
  * @brief FIFO0接收处理函数
  */
 static void BSP_CAN_RxFifo0Callback(void) {
@@ -347,7 +470,16 @@ int8_t BSP_CAN_Init(void) {
     
     // 清零回调函数数组
     memset(CAN_Callback, 0, sizeof(CAN_Callback));
+<<<<<<< HEAD
+        
+    // 初始化发送队列
+    for (int i = 0; i < BSP_CAN_NUM; i++) {
+        BSP_CAN_TxQueueInit((BSP_CAN_t)i);
+    }
+
+=======
     
+>>>>>>> main
     // 初始化ID解析器为默认解析器
     id_parser = BSP_CAN_DefaultIdParser;
     
@@ -377,6 +509,12 @@ int8_t BSP_CAN_Init(void) {
 
     // 自动注册CAN1接收回调函数
     BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_RX_FIFO0_MSG_PENDING_CB, BSP_CAN_RxFifo0Callback);
+<<<<<<< HEAD
+    BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_TX_MAILBOX0_CPLT_CB, BSP_CAN_TxCompleteCallback);
+    BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_TX_MAILBOX1_CPLT_CB, BSP_CAN_TxCompleteCallback);
+    BSP_CAN_RegisterCallback(BSP_CAN_1, HAL_CAN_TX_MAILBOX2_CPLT_CB, BSP_CAN_TxCompleteCallback);
+=======
+>>>>>>> main
 
     // 激活CAN1中断
     HAL_CAN_ActivateNotification(&hcan1, CAN_IT_RX_FIFO0_MSG_PENDING | 
@@ -390,15 +528,27 @@ int8_t BSP_CAN_Init(void) {
 
     // 自动注册CAN2接收回调函数
     BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_RX_FIFO1_MSG_PENDING_CB, BSP_CAN_RxFifo1Callback);
+<<<<<<< HEAD
+    BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_TX_MAILBOX0_CPLT_CB, BSP_CAN_TxCompleteCallback);
+    BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_TX_MAILBOX1_CPLT_CB, BSP_CAN_TxCompleteCallback);
+    BSP_CAN_RegisterCallback(BSP_CAN_2, HAL_CAN_TX_MAILBOX2_CPLT_CB, BSP_CAN_TxCompleteCallback);
+
+    // 激活CAN2中断
+    HAL_CAN_ActivateNotification(&hcan2, CAN_IT_RX_FIFO1_MSG_PENDING | 
+                                        CAN_IT_TX_MAILBOX_EMPTY);  // 激活发送邮箱空中断
+=======
 
     // 激活CAN2中断
     HAL_CAN_ActivateNotification(&hcan2, CAN_IT_RX_FIFO1_MSG_PENDING);
+>>>>>>> main
 
     
     inited = true;
     return BSP_OK;
 }
 
+<<<<<<< HEAD
+=======
 int8_t BSP_CAN_DeInit(void) {
     if (!inited) {
         return BSP_ERR;
@@ -432,6 +582,7 @@ int8_t BSP_CAN_DeInit(void) {
     inited = false;
     return BSP_OK;
 }
+>>>>>>> main
 
 CAN_HandleTypeDef *BSP_CAN_GetHandle(BSP_CAN_t can) {
     if (can >= BSP_CAN_NUM) {
@@ -487,6 +638,31 @@ int8_t BSP_CAN_Transmit(BSP_CAN_t can, BSP_CAN_Format_t format,
         return BSP_ERR_NULL;
     }
     
+<<<<<<< HEAD
+    // 准备发送消息
+    BSP_CAN_TxMessage_t tx_msg = {0};
+    
+    switch (format) {
+        case BSP_CAN_FORMAT_STD_DATA:
+            tx_msg.header.StdId = id;
+            tx_msg.header.IDE = CAN_ID_STD;
+            tx_msg.header.RTR = CAN_RTR_DATA;
+            break;
+        case BSP_CAN_FORMAT_EXT_DATA:
+            tx_msg.header.ExtId = id;
+            tx_msg.header.IDE = CAN_ID_EXT;
+            tx_msg.header.RTR = CAN_RTR_DATA;
+            break;
+        case BSP_CAN_FORMAT_STD_REMOTE:
+            tx_msg.header.StdId = id;
+            tx_msg.header.IDE = CAN_ID_STD;
+            tx_msg.header.RTR = CAN_RTR_REMOTE;
+            break;
+        case BSP_CAN_FORMAT_EXT_REMOTE:
+            tx_msg.header.ExtId = id;
+            tx_msg.header.IDE = CAN_ID_EXT;
+            tx_msg.header.RTR = CAN_RTR_REMOTE;
+=======
     CAN_TxHeaderTypeDef header = {0};
     uint32_t mailbox;
     
@@ -510,11 +686,38 @@ int8_t BSP_CAN_Transmit(BSP_CAN_t can, BSP_CAN_Format_t format,
             header.ExtId = id;
             header.IDE = CAN_ID_EXT;
             header.RTR = CAN_RTR_REMOTE;
+>>>>>>> main
             break;
         default:
             return BSP_ERR;
     }
     
+<<<<<<< HEAD
+    tx_msg.header.DLC = dlc;
+    tx_msg.header.TransmitGlobalTime = DISABLE;
+    
+    // 复制数据
+    if (data != NULL && dlc > 0) {
+        memcpy(tx_msg.data, data, dlc);
+    }
+    
+    // 尝试直接发送到邮箱
+    uint32_t mailbox;
+    if (HAL_CAN_GetTxMailboxesFreeLevel(hcan) > 0) {
+        HAL_StatusTypeDef result = HAL_CAN_AddTxMessage(hcan, &tx_msg.header, tx_msg.data, &mailbox);
+        if (result == HAL_OK) {
+            return BSP_OK; // 发送成功
+        }
+    }
+    
+    // 邮箱满，尝试放入队列
+    if (BSP_CAN_TxQueuePush(can, &tx_msg)) {
+        return BSP_OK; // 成功放入队列
+    }
+    
+    // 队列也满，丢弃数据
+    return BSP_ERR; // 数据丢弃
+=======
     header.DLC = dlc;
     header.TransmitGlobalTime = DISABLE;
     
@@ -525,6 +728,7 @@ int8_t BSP_CAN_Transmit(BSP_CAN_t can, BSP_CAN_Format_t format,
     }
     
     return BSP_OK;
+>>>>>>> main
 }
 
 int8_t BSP_CAN_TransmitStdDataFrame(BSP_CAN_t can, BSP_CAN_StdDataFrame_t *frame) {
@@ -556,12 +760,15 @@ int8_t BSP_CAN_RegisterId(BSP_CAN_t can, uint32_t can_id, uint8_t queue_size) {
     return BSP_CAN_CreateIdQueue(can, can_id, queue_size);
 }
 
+<<<<<<< HEAD
+=======
 int8_t BSP_CAN_UnregisterIdQueue(BSP_CAN_t can, uint32_t can_id) {
     if (!inited) {
         return BSP_ERR_INITED;
     }
     return BSP_CAN_DeleteIdQueue(can, can_id);
 }
+>>>>>>> main
 
 int8_t BSP_CAN_GetMessage(BSP_CAN_t can, uint32_t can_id, BSP_CAN_Message_t *msg, uint32_t timeout) {
     if (!inited) {
@@ -628,6 +835,8 @@ int8_t BSP_CAN_RegisterIdParser(BSP_CAN_IdParser_t parser) {
     return BSP_OK;
 }
 
+<<<<<<< HEAD
+=======
 int8_t BSP_CAN_UnregisterIdParser(void) {
     if (!inited) {
         return BSP_ERR_INITED;
@@ -637,6 +846,7 @@ int8_t BSP_CAN_UnregisterIdParser(void) {
     return BSP_OK;
 }
 
+>>>>>>> main
 uint32_t BSP_CAN_ParseId(uint32_t original_id, BSP_CAN_FrameType_t frame_type) {
     if (id_parser != NULL) {
         return id_parser(original_id, frame_type);
@@ -644,6 +854,9 @@ uint32_t BSP_CAN_ParseId(uint32_t original_id, BSP_CAN_FrameType_t frame_type) {
     return BSP_CAN_DefaultIdParser(original_id, frame_type);
 }
 
+<<<<<<< HEAD
+
+=======
 int8_t BSP_CAN_WaitTxMailboxEmpty(BSP_CAN_t can, uint32_t timeout) {
     if (!inited) {
         return BSP_ERR_INITED;
@@ -683,3 +896,4 @@ int8_t BSP_CAN_WaitTxMailboxEmpty(BSP_CAN_t can, uint32_t timeout) {
         osDelay(1);
     }
 }
+>>>>>>> main

User/bsp/can.h

@@ -21,6 +21,10 @@ extern "C" {
 #define BSP_CAN_DEFAULT_QUEUE_SIZE      10
 #define BSP_CAN_TIMEOUT_IMMEDIATE       0
 #define BSP_CAN_TIMEOUT_FOREVER         osWaitForever
+<<<<<<< HEAD
+#define BSP_CAN_TX_QUEUE_SIZE           32    /* 发送队列大小 */
+=======
+>>>>>>> main
 
 /* USER DEFINE BEGIN */
 
@@ -102,6 +106,22 @@ typedef struct {
 /* ID解析回调函数类型 */
 typedef uint32_t (*BSP_CAN_IdParser_t)(uint32_t original_id, BSP_CAN_FrameType_t frame_type);
 
+<<<<<<< HEAD
+/* CAN发送消息结构体 */
+typedef struct {
+    CAN_TxHeaderTypeDef header;         /* 发送头 */
+    uint8_t data[BSP_CAN_MAX_DLC];      /* 数据 */
+} BSP_CAN_TxMessage_t;
+
+/* 无锁环形队列结构体 */
+typedef struct {
+    BSP_CAN_TxMessage_t buffer[BSP_CAN_TX_QUEUE_SIZE];  /* 缓冲区 */
+    volatile uint32_t head;             /* 队列头 */
+    volatile uint32_t tail;             /* 队列尾 */
+} BSP_CAN_TxQueue_t;
+
+=======
+>>>>>>> main
 /* USER STRUCT BEGIN */
 
 /* USER STRUCT END */
@@ -115,12 +135,15 @@ typedef uint32_t (*BSP_CAN_IdParser_t)(uint32_t original_id, BSP_CAN_FrameType_t
 int8_t BSP_CAN_Init(void);
 
 /**
+<<<<<<< HEAD
+=======
  * @brief 反初始化 CAN 模块
  * @return BSP_OK 成功，其他值失败
  */
 int8_t BSP_CAN_DeInit(void);
 
 /**
+>>>>>>> main
  * @brief 获取 CAN 句柄
  * @param can CAN 枚举
  * @return CAN_HandleTypeDef 指针，失败返回 NULL
@@ -173,6 +196,22 @@ int8_t BSP_CAN_TransmitExtDataFrame(BSP_CAN_t can, BSP_CAN_ExtDataFrame_t *frame
  */
 int8_t BSP_CAN_TransmitRemoteFrame(BSP_CAN_t can, BSP_CAN_RemoteFrame_t *frame);
 
+<<<<<<< HEAD
+
+/**
+ * @brief 获取发送队列中待发送消息数量
+ * @param can CAN 枚举
+ * @return 队列中消息数量，-1表示错误
+ */
+int32_t BSP_CAN_GetTxQueueCount(BSP_CAN_t can);
+
+/**
+ * @brief 清空发送队列
+ * @param can CAN 枚举
+ * @return BSP_OK 成功，其他值失败
+ */
+int8_t BSP_CAN_FlushTxQueue(BSP_CAN_t can);
+=======
 /**
  * @brief 等待CAN发送邮箱空闲
  * @param can CAN 枚举
@@ -180,6 +219,7 @@ int8_t BSP_CAN_TransmitRemoteFrame(BSP_CAN_t can, BSP_CAN_RemoteFrame_t *frame);
  * @return BSP_OK 成功，其他值失败
  */
 int8_t BSP_CAN_WaitTxMailboxEmpty(BSP_CAN_t can, uint32_t timeout);
+>>>>>>> main
 
 /**
  * @brief 注册 CAN ID 接收队列
@@ -190,6 +230,9 @@ int8_t BSP_CAN_WaitTxMailboxEmpty(BSP_CAN_t can, uint32_t timeout);
  */
 int8_t BSP_CAN_RegisterId(BSP_CAN_t can, uint32_t can_id, uint8_t queue_size);
 
+<<<<<<< HEAD
+
+=======
 /**
  * @brief 注销 CAN ID 接收队列
  * @param can CAN 枚举
@@ -197,6 +240,7 @@ int8_t BSP_CAN_RegisterId(BSP_CAN_t can, uint32_t can_id, uint8_t queue_size);
  * @return BSP_OK 成功，其他值失败
  */
 int8_t BSP_CAN_UnregisterIdQueue(BSP_CAN_t can, uint32_t can_id);
+>>>>>>> main
 
 /**
  * @brief 获取 CAN 消息
@@ -231,11 +275,14 @@ int8_t BSP_CAN_FlushQueue(BSP_CAN_t can, uint32_t can_id);
  */
 int8_t BSP_CAN_RegisterIdParser(BSP_CAN_IdParser_t parser);
 
+<<<<<<< HEAD
+=======
 /**
  * @brief 注销ID解析器
  * @return BSP_OK 成功，其他值失败
  */
 int8_t BSP_CAN_UnregisterIdParser(void);
+>>>>>>> main
 
 /**
  * @brief 解析CAN ID

User/bsp/pwm.h

@@ -41,8 +41,13 @@ typedef enum {
 
 /* Exported functions prototypes -------------------------------------------- */
 int8_t BSP_PWM_Start(BSP_PWM_Channel_t ch);
+<<<<<<< HEAD
+int8_t BSP_PWM_SetComp(BSP_PWM_Channel_t ch, float duty_cycle); //设置占空比，范围0.0~1.0
+int8_t BSP_PWM_SetFreq(BSP_PWM_Channel_t ch, float freq); //设置频率，单位Hz
+=======
 int8_t BSP_PWM_SetComp(BSP_PWM_Channel_t ch, float duty_cycle);
 int8_t BSP_PWM_SetFreq(BSP_PWM_Channel_t ch, float freq);
+>>>>>>> main
 int8_t BSP_PWM_Stop(BSP_PWM_Channel_t ch);
 uint32_t BSP_PWM_GetAutoReloadPreload(BSP_PWM_Channel_t ch);
 uint16_t BSP_PWM_GetChannel(BSP_PWM_Channel_t ch);

User/component/component_config.yaml

@@ -2,17 +2,23 @@ ahrs:
   dependencies:
   - component/user_math.h
   enabled: true
+<<<<<<< HEAD
+=======
 cmd:
   dependencies:
   - component/ahrs
   enabled: true
+>>>>>>> main
 filter:
   dependencies:
   - component/ahrs
   enabled: true
+<<<<<<< HEAD
+=======
 limiter:
   dependencies: []
   enabled: true
+>>>>>>> main
 pid:
   dependencies:
   - component/filter

User/component/lqr.c

@@ -6,89 +6,82 @@
 #include <string.h>
 
 /* Private macros ----------------------------------------------------------- */
-#define LQR_LIMIT(x, min, max) ((x) < (min) ? (min) : ((x) > (max) ? (max) : (x)))
-
 /* Private variables -------------------------------------------------------- */
 
-/* 从MATLAB仿真get_k.m得到的默认增益矩阵多项式拟合系数 */
-/* 这些系数是通过对不同腿长的LQR增益进行3阶多项式拟合得到的 */
-static LQR_GainMatrix_t default_gain_matrix = {
-    /* K矩阵第一行 - 轮毂力矩T的增益系数 */
-    .k11_coeff = {0.0f, -2845.3f, 1899.4f, -123.8f},      /* theta */
-    .k12_coeff = {0.0f, -89.7f, 61.2f, -4.8f},            /* d_theta */
-    .k13_coeff = {0.0f, 5479.2f, -3298.6f, 489.8f},       /* x */
-    .k14_coeff = {0.0f, 312.4f, -178.9f, 34.2f},          /* d_x */
-    .k15_coeff = {0.0f, -31250.0f, 18750.0f, -3125.0f},   /* phi */
-    .k16_coeff = {0.0f, -89.7f, 61.2f, -4.8f},            /* d_phi */
-    
-    /* K矩阵第二行 - 髋关节力矩Tp的增益系数 */
-    .k21_coeff = {0.0f, 486.1f, -324.1f, 21.6f},          /* theta */
-    .k22_coeff = {0.0f, 15.3f, -10.4f, 0.8f},             /* d_theta */
-    .k23_coeff = {0.0f, -935.4f, 562.2f, -83.5f},         /* x */
-    .k24_coeff = {0.0f, -53.3f, 30.5f, -5.8f},            /* d_x */
-    .k25_coeff = {0.0f, 5333.3f, -3200.0f, 533.3f},       /* phi */
-    .k26_coeff = {0.0f, 15.3f, -10.4f, 0.8f},             /* d_phi */
-};
 
 /* Private function prototypes ---------------------------------------------- */
-static int8_t LQR_CalculateErrorState(LQR_Controller_t *lqr);
-static int8_t LQR_ApplyControlLimits(LQR_Controller_t *lqr);
 
-/* Public functions --------------------------------------------------------- */
-
-int8_t LQR_Init(LQR_Controller_t *lqr, float max_wheel_torque, float max_joint_torque) {
+static int8_t LQR_CalculateErrorState(LQR_t *lqr) {
     if (lqr == NULL) {
         return -1;
     }
+    /* 计算状态误差 */
+    lqr->error_state.theta = lqr->current_state.theta - lqr->target_state.theta;
+    lqr->error_state.d_theta = lqr->current_state.d_theta - lqr->target_state.d_theta;
+    lqr->error_state.x = lqr->current_state.x - lqr->target_state.x;
+    lqr->error_state.d_x = lqr->current_state.d_x - lqr->target_state.d_x;
+    lqr->error_state.phi = lqr->current_state.phi - lqr->target_state.phi;
+    lqr->error_state.d_phi = lqr->current_state.d_phi - lqr->target_state.d_phi;
     
+    return 0;
+}
+
+static float LQR_PolynomialCalc(const float *coeff, float leg_length) {
+    if (coeff == NULL) {
+        return 0.0f;
+    }
+    
+    /* 计算3阶多项式: coeff[0]*L^3 + coeff[1]*L^2 + coeff[2]*L + coeff[3] */
+    float L = leg_length;
+    float L2 = L * L;
+    float L3 = L2 * L;
+    
+    return coeff[0] * L3 + coeff[1] * L2 + coeff[2] * L + coeff[3];
+}
+
+
+// static int8_t LQR_ApplyControlLimits(LQR_t *lqr) {
+//     if (lqr == NULL) {
+//         return -1;
+//     }
+    
+//     /* 限制轮毂力矩 */
+//     if (fabsf(lqr->control_output.T) > lqr->param.max_wheel_torque) {
+//         lqr->control_output.T = LQR_LIMIT(lqr->control_output.T, 
+//                                          -lqr->param.max_wheel_torque, 
+//                                           lqr->param.max_wheel_torque);
+//         lqr->wheel_torque_limited = true;
+//     }
+    
+//     /* 限制髋关节力矩 */
+//     if (fabsf(lqr->control_output.Tp) > lqr->param.max_joint_torque) {
+//         lqr->control_output.Tp = LQR_LIMIT(lqr->control_output.Tp,
+//                                           -lqr->param.max_joint_torque,
+//                                            lqr->param.max_joint_torque);
+//         lqr->joint_torque_limited = true;
+//     }
+    
+//     return 0;
+// }
+
+
+/* Public functions --------------------------------------------------------- */
+
+int8_t LQR_Init(LQR_t *lqr, LQR_GainMatrix_t *gain_matrix) {
+    if (lqr == NULL) {
+        return -1;
+    }
     /* 清零结构体 */
-    memset(lqr, 0, sizeof(LQR_Controller_t));
-    
-    /* 设置控制限制 */
-    lqr->param.max_wheel_torque = max_wheel_torque;
-    lqr->param.max_joint_torque = max_joint_torque;
-    
-    /* 设置默认系统物理参数（从MATLAB仿真get_k_length.m获取） */
-    lqr->param.R = 0.072f;          /* 驱动轮半径 */
-    lqr->param.l = 0.03f;           /* 机体重心到转轴距离 */
-    lqr->param.mw = 0.60f;          /* 驱动轮质量 */
-    lqr->param.mp = 1.8f;           /* 摆杆质量 */
-    lqr->param.M = 1.8f;            /* 机体质量 */
-    lqr->param.g = 9.8f;            /* 重力加速度 */
-    
-    /* 计算转动惯量 */
-    lqr->param.Iw = lqr->param.mw * lqr->param.R * lqr->param.R;
-    lqr->param.IM = lqr->param.M * (0.3f * 0.3f + 0.12f * 0.12f) / 12.0f;
-    
-    /* 设置默认增益矩阵 */
-    lqr->gain_matrix = &default_gain_matrix;
-    
-    /* 设置默认目标状态（平衡状态） */
-    memset(&lqr->param.target_state, 0, sizeof(LQR_State_t));
-    
-    /* 初始化当前腿长为中等值 */
-    lqr->current_leg_length = 0.25f;
-    
-    /* 计算初始增益矩阵 */
-    LQR_CalculateGainMatrix(lqr, lqr->current_leg_length);
-    
+    memset(lqr, 0, sizeof(LQR_t));
+
+    lqr->gain_matrix = gain_matrix;
     lqr->initialized = true;
     
     return 0;
 }
 
-int8_t LQR_SetGainMatrix(LQR_Controller_t *lqr, LQR_GainMatrix_t *gain_matrix) {
-    if (lqr == NULL || gain_matrix == NULL) {
-        return -1;
-    }
-    
-    lqr->gain_matrix = gain_matrix;
-    
-    /* 重新计算增益矩阵 */
-    return LQR_CalculateGainMatrix(lqr, lqr->current_leg_length);
-}
 
-int8_t LQR_UpdateState(LQR_Controller_t *lqr, const LQR_State_t *state) {
+int8_t LQR_UpdateState(LQR_t *lqr, const LQR_State_t *state) {
     if (lqr == NULL || state == NULL) {
         return -1;
     }
@@ -100,34 +93,25 @@ int8_t LQR_UpdateState(LQR_Controller_t *lqr, const LQR_State_t *state) {
     return LQR_CalculateErrorState(lqr);
 }
 
-int8_t LQR_SetTargetState(LQR_Controller_t *lqr, const LQR_State_t *target_state) {
+int8_t LQR_SetTargetState(LQR_t *lqr, const LQR_State_t *target_state) {
     if (lqr == NULL || target_state == NULL) {
         return -1;
     }
     
-    lqr->param.target_state = *target_state;
+    lqr->target_state = *target_state;
     
     /* 重新计算状态误差 */
     return LQR_CalculateErrorState(lqr);
 }
 
-int8_t LQR_CalculateGainMatrix(LQR_Controller_t *lqr, float leg_length) {
+int8_t LQR_CalculateGainMatrix(LQR_t *lqr, float leg_length) {
     if (lqr == NULL || lqr->gain_matrix == NULL) {
         return -1;
     }
     
     /* 限制腿长范围 */
-    leg_length = LQR_LIMIT(leg_length, 0.1f, 0.4f);
     lqr->current_leg_length = leg_length;
     
-    /* 更新与腿长相关的物理参数 */
-    lqr->param.L = leg_length / 2.0f;           /* 摆杆重心到驱动轮轴距离 */
-    lqr->param.LM = leg_length / 2.0f;          /* 摆杆重心到其转轴距离 */
-    
-    /* 计算摆杆转动惯量 */
-    float leg_total_length = lqr->param.L + lqr->param.LM;
-    lqr->param.Ip = lqr->param.mp * (leg_total_length * leg_total_length + 0.05f * 0.05f) / 12.0f;
-    
     /* 使用多项式拟合计算当前增益矩阵 */
     lqr->K_matrix[0][0] = LQR_PolynomialCalc(lqr->gain_matrix->k11_coeff, leg_length);  /* K11: theta */
     lqr->K_matrix[0][1] = LQR_PolynomialCalc(lqr->gain_matrix->k12_coeff, leg_length);  /* K12: d_theta */
@@ -146,7 +130,7 @@ int8_t LQR_CalculateGainMatrix(LQR_Controller_t *lqr, float leg_length) {
     return 0;
 }
 
-int8_t LQR_Control(LQR_Controller_t *lqr) {
+int8_t LQR_Control(LQR_t *lqr) {
     if (lqr == NULL || !lqr->initialized) {
         return -1;
     }
@@ -175,92 +159,22 @@ int8_t LQR_Control(LQR_Controller_t *lqr) {
         lqr->K_matrix[1][5] * lqr->error_state.d_phi
     );
     
-    /* 应用控制限制 */
-    return LQR_ApplyControlLimits(lqr);
-}
-
-int8_t LQR_GetOutput(LQR_Controller_t *lqr, LQR_Input_t *output) {
-    if (lqr == NULL || output == NULL) {
-        return -1;
-    }
-    
-    *output = lqr->control_output;
     return 0;
 }
 
-int8_t LQR_Reset(LQR_Controller_t *lqr) {
+int8_t LQR_Reset(LQR_t *lqr) {
     if (lqr == NULL) {
         return -1;
     }
     
     /* 清零状态和输出 */
     memset(&lqr->current_state, 0, sizeof(LQR_State_t));
+    memset(&lqr->target_state, 0, sizeof(LQR_State_t));
     memset(&lqr->error_state, 0, sizeof(LQR_State_t));
-    memset(&lqr->control_output, 0, sizeof(LQR_Input_t));
-    memset(&lqr->param.target_state, 0, sizeof(LQR_State_t));
-    
-    /* 重置限幅标志 */
-    lqr->wheel_torque_limited = false;
-    lqr->joint_torque_limited = false;
+    memset(&lqr->control_output, 0, sizeof(LQR_Output_t));
     
     return 0;
 }
 
-float LQR_PolynomialCalc(const float *coeff, float leg_length) {
-    if (coeff == NULL) {
-        return 0.0f;
-    }
-    
-    /* 计算3阶多项式: coeff[0]*L^3 + coeff[1]*L^2 + coeff[2]*L + coeff[3] */
-    float L = leg_length;
-    float L2 = L * L;
-    float L3 = L2 * L;
-    
-    return coeff[0] * L3 + coeff[1] * L2 + coeff[2] * L + coeff[3];
-}
 
-/* Private functions -------------------------------------------------------- */
-
-static int8_t LQR_CalculateErrorState(LQR_Controller_t *lqr) {
-    if (lqr == NULL) {
-        return -1;
-    }
-    
-    /* 计算状态误差 */
-    lqr->error_state.theta = lqr->current_state.theta - lqr->param.target_state.theta;
-    lqr->error_state.d_theta = lqr->current_state.d_theta - lqr->param.target_state.d_theta;
-    lqr->error_state.x = lqr->current_state.x - lqr->param.target_state.x;
-    lqr->error_state.d_x = lqr->current_state.d_x - lqr->param.target_state.d_x;
-    lqr->error_state.phi = lqr->current_state.phi - lqr->param.target_state.phi;
-    lqr->error_state.d_phi = lqr->current_state.d_phi - lqr->param.target_state.d_phi;
-    
-    return 0;
-}
-
-static int8_t LQR_ApplyControlLimits(LQR_Controller_t *lqr) {
-    if (lqr == NULL) {
-        return -1;
-    }
-    
-    /* 重置限幅标志 */
-    lqr->wheel_torque_limited = false;
-    lqr->joint_torque_limited = false;
-    
-    /* 限制轮毂力矩 */
-    if (fabsf(lqr->control_output.T) > lqr->param.max_wheel_torque) {
-        lqr->control_output.T = LQR_LIMIT(lqr->control_output.T, 
-                                         -lqr->param.max_wheel_torque, 
-                                          lqr->param.max_wheel_torque);
-        lqr->wheel_torque_limited = true;
-    }
-    
-    /* 限制髋关节力矩 */
-    if (fabsf(lqr->control_output.Tp) > lqr->param.max_joint_torque) {
-        lqr->control_output.Tp = LQR_LIMIT(lqr->control_output.Tp,
-                                          -lqr->param.max_joint_torque,
-                                           lqr->param.max_joint_torque);
-        lqr->joint_torque_limited = true;
-    }
-    
-    return 0;
-}
+/* Private functions -------------------------------------------------------- */

User/component/lqr.h

@@ -1,6 +1,6 @@
 /*
  * LQR控制器
- * 用于轮腿平衡机器人的线性二次调节器控制
+ * 用于轮腿平衡机器人的线性二次调节器控制：单腿建模
  */
 
 #pragma once
@@ -21,16 +21,6 @@ extern "C" {
 
 /* Exported types ----------------------------------------------------------- */
 
-/**
- * @brief LQR状态向量
- * 状态定义：
- * theta:   摆杆与竖直方向夹角 (rad)
- * d_theta: 摆杆角速度 (rad/s)
- * x:       驱动轮位移 (m)
- * d_x:     驱动轮速度 (m/s)
- * phi:     机体与水平夹角 (rad)
- * d_phi:   机体角速度 (rad/s)
- */
 typedef struct {
     float theta;      /* 摆杆角度 */
     float d_theta;    /* 摆杆角速度 */
@@ -40,19 +30,11 @@ typedef struct {
     float d_phi;      /* 机体俯仰角速度 */
 } LQR_State_t;
 
-/**
- * @brief LQR控制输入向量
- */
 typedef struct {
     float T;          /* 轮毂力矩 (N·m) */
     float Tp;         /* 髋关节力矩 (N·m) */
-} LQR_Input_t;
+} LQR_Output_t;
 
-/**
- * @brief LQR增益矩阵参数
- * K矩阵的每个元素的多项式拟合系数
- * K(leg_length) = a[0]*L^3 + a[1]*L^2 + a[2]*L + a[3]
- */
 typedef struct {
     /* K矩阵第一行(轮毂力矩T对应的增益) */
     float k11_coeff[LQR_POLY_ORDER];  /* K(1,1): theta */
@@ -71,41 +53,18 @@ typedef struct {
     float k26_coeff[LQR_POLY_ORDER];  /* K(2,6): d_phi */
 } LQR_GainMatrix_t;
 
-/**
- * @brief LQR控制器参数
- */
-typedef struct {
-    /* 系统物理参数 */
-    float R;          /* 驱动轮半径 (m) */
-    float L;          /* 摆杆重心到驱动轮轴距离 (m) */
-    float LM;         /* 摆杆重心到其转轴距离 (m) */
-    float l;          /* 机体重心到其转轴距离 (m) */
-    float mw;         /* 驱动轮质量 (kg) */
-    float mp;         /* 摆杆质量 (kg) */
-    float M;          /* 机体质量 (kg) */
-    float Iw;         /* 驱动轮转动惯量 (kg·m²) */
-    float Ip;         /* 摆杆绕质心转动惯量 (kg·m²) */
-    float IM;         /* 机体绕质心转动惯量 (kg·m²) */
-    float g;          /* 重力加速度 (m/s²) */
-    
-    /* 控制限制 */
-    float max_wheel_torque;  /* 轮毂电机最大力矩 (N·m) */
-    float max_joint_torque;  /* 关节电机最大力矩 (N·m) */
-    
-    /* 目标状态 */
-    LQR_State_t target_state; /* 目标状态 */
-} LQR_Param_t;
 
 /**
  * @brief LQR控制器主结构体
  */
 typedef struct {
-    LQR_Param_t param;           /* 控制器参数 */
     LQR_GainMatrix_t *gain_matrix; /* 增益矩阵参数指针 */
     
     LQR_State_t current_state;   /* 当前状态 */
+    LQR_State_t target_state;    /* 目标状态 */
     LQR_State_t error_state;     /* 状态误差 */
-    LQR_Input_t control_output;  /* 控制输出 */
+
+    LQR_Output_t control_output;  /* 控制输出 */
     
     /* 运行时变量 */
     float current_leg_length;    /* 当前腿长 */
@@ -113,10 +72,7 @@ typedef struct {
     
     bool initialized;            /* 初始化标志 */
     
-    /* 限幅标志 */
-    bool wheel_torque_limited;   /* 轮毂力矩是否被限幅 */
-    bool joint_torque_limited;   /* 关节力矩是否被限幅 */
-} LQR_Controller_t;
+} LQR_t;
 
 /* Exported functions prototypes -------------------------------------------- */
 
@@ -124,20 +80,11 @@ typedef struct {
  * @brief 初始化LQR控制器
  * 
  * @param lqr LQR控制器结构体指针
- * @param max_wheel_torque 轮毂电机最大力矩
- * @param max_joint_torque 关节电机最大力矩
- * @return int8_t 0-成功, 其他-失败
- */
-int8_t LQR_Init(LQR_Controller_t *lqr, float max_wheel_torque, float max_joint_torque);
-
-/**
- * @brief 设置LQR增益矩阵参数
- * 
- * @param lqr LQR控制器结构体指针  
  * @param gain_matrix 增益矩阵参数指针
  * @return int8_t 0-成功, 其他-失败
  */
-int8_t LQR_SetGainMatrix(LQR_Controller_t *lqr, LQR_GainMatrix_t *gain_matrix);
+int8_t LQR_Init(LQR_t *lqr, LQR_GainMatrix_t *gain_matrix);
+
 
 /**
  * @brief 更新当前状态
@@ -146,7 +93,7 @@ int8_t LQR_SetGainMatrix(LQR_Controller_t *lqr, LQR_GainMatrix_t *gain_matrix);
  * @param state 当前状态
  * @return int8_t 0-成功, 其他-失败
  */
-int8_t LQR_UpdateState(LQR_Controller_t *lqr, const LQR_State_t *state);
+int8_t LQR_UpdateState(LQR_t *lqr, const LQR_State_t *current_state);
 
 /**
  * @brief 设置目标状态
@@ -155,7 +102,7 @@ int8_t LQR_UpdateState(LQR_Controller_t *lqr, const LQR_State_t *state);
  * @param target_state 目标状态
  * @return int8_t 0-成功, 其他-失败
  */
-int8_t LQR_SetTargetState(LQR_Controller_t *lqr, const LQR_State_t *target_state);
+int8_t LQR_SetTargetState(LQR_t *lqr, const LQR_State_t *target_state);
 
 /**
  * @brief 根据当前腿长计算增益矩阵
@@ -164,7 +111,7 @@ int8_t LQR_SetTargetState(LQR_Controller_t *lqr, const LQR_State_t *target_state
  * @param leg_length 当前腿长 (m)
  * @return int8_t 0-成功, 其他-失败
  */
-int8_t LQR_CalculateGainMatrix(LQR_Controller_t *lqr, float leg_length);
+int8_t LQR_CalculateGainMatrix(LQR_t *lqr, float leg_length);
 
 /**
  * @brief 执行LQR控制计算
@@ -172,16 +119,7 @@ int8_t LQR_CalculateGainMatrix(LQR_Controller_t *lqr, float leg_length);
  * @param lqr LQR控制器结构体指针
  * @return int8_t 0-成功, 其他-失败
  */
-int8_t LQR_Control(LQR_Controller_t *lqr);
-
-/**
- * @brief 获取控制输出
- * 
- * @param lqr LQR控制器结构体指针
- * @param output 输出控制指令指针
- * @return int8_t 0-成功, 其他-失败
- */
-int8_t LQR_GetOutput(LQR_Controller_t *lqr, LQR_Input_t *output);
+int8_t LQR_Control(LQR_t *lqr);
 
 /**
  * @brief 重置LQR控制器
@@ -189,16 +127,7 @@ int8_t LQR_GetOutput(LQR_Controller_t *lqr, LQR_Input_t *output);
  * @param lqr LQR控制器结构体指针
  * @return int8_t 0-成功, 其他-失败
  */
-int8_t LQR_Reset(LQR_Controller_t *lqr);
-
-/**
- * @brief 根据多项式系数计算增益值
- * 
- * @param coeff 多项式系数数组
- * @param leg_length 腿长
- * @return float 增益值
- */
-float LQR_PolynomialCalc(const float *coeff, float leg_length);
+int8_t LQR_Reset(LQR_t *lqr);
 
 #ifdef __cplusplus
 }

User/component/user_math.h

@@ -25,6 +25,13 @@ extern "C" {
 #define M_PI 3.14159265358979323846f
 #endif
 
+<<<<<<< HEAD
+#ifndef M_PI_2
+#define M_PI_2 1.57079632679489661923f
+#endif
+
+=======
+>>>>>>> main
 #ifndef M_2PI
 #define M_2PI 6.28318530717958647692f
 #endif

User/device/bmi088.c

@@ -0,0 +1,381 @@
+/*
+     BMI088 陀螺仪+加速度计传感器。
+*/
+
+/* Includes ----------------------------------------------------------------- */
+#include "bmi088.h"
+
+#include <cmsis_os2.h>
+#include <gpio.h>
+#include <stdbool.h>
+#include <string.h>
+
+#include "bsp/time.h"
+#include "bsp/gpio.h"
+#include "bsp/spi.h"
+#include "component/user_math.h"
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* Private define ----------------------------------------------------------- */
+/* Private define ----------------------------------------------------------- */
+#define BMI088_REG_ACCL_CHIP_ID (0x00)
+#define BMI088_REG_ACCL_ERR (0x02)
+#define BMI088_REG_ACCL_STATUS (0x03)
+#define BMI088_REG_ACCL_X_LSB (0x12)
+#define BMI088_REG_ACCL_X_MSB (0x13)
+#define BMI088_REG_ACCL_Y_LSB (0x14)
+#define BMI088_REG_ACCL_Y_MSB (0x15)
+#define BMI088_REG_ACCL_Z_LSB (0x16)
+#define BMI088_REG_ACCL_Z_MSB (0x17)
+#define BMI088_REG_ACCL_SENSORTIME_0 (0x18)
+#define BMI088_REG_ACCL_SENSORTIME_1 (0x19)
+#define BMI088_REG_ACCL_SENSORTIME_2 (0x1A)
+#define BMI088_REG_ACCL_INT_STAT_1 (0x1D)
+#define BMI088_REG_ACCL_TEMP_MSB (0x22)
+#define BMI088_REG_ACCL_TEMP_LSB (0x23)
+#define BMI088_REG_ACCL_CONF (0x40)
+#define BMI088_REG_ACCL_RANGE (0x41)
+#define BMI088_REG_ACCL_INT1_IO_CONF (0x53)
+#define BMI088_REG_ACCL_INT2_IO_CONF (0x54)
+#define BMI088_REG_ACCL_INT1_INT2_MAP_DATA (0x58)
+#define BMI088_REG_ACCL_SELF_TEST (0x6D)
+#define BMI088_REG_ACCL_PWR_CONF (0x7C)
+#define BMI088_REG_ACCL_PWR_CTRL (0x7D)
+#define BMI088_REG_ACCL_SOFTRESET (0x7E)
+
+#define BMI088_REG_GYRO_CHIP_ID (0x00)
+#define BMI088_REG_GYRO_X_LSB (0x02)
+#define BMI088_REG_GYRO_X_MSB (0x03)
+#define BMI088_REG_GYRO_Y_LSB (0x04)
+#define BMI088_REG_GYRO_Y_MSB (0x05)
+#define BMI088_REG_GYRO_Z_LSB (0x06)
+#define BMI088_REG_GYRO_Z_MSB (0x07)
+#define BMI088_REG_GYRO_INT_STAT_1 (0x0A)
+#define BMI088_REG_GYRO_RANGE (0x0F)
+#define BMI088_REG_GYRO_BANDWIDTH (0x10)
+#define BMI088_REG_GYRO_LPM1 (0x11)
+#define BMI088_REG_GYRO_SOFTRESET (0x14)
+#define BMI088_REG_GYRO_INT_CTRL (0x15)
+#define BMI088_REG_GYRO_INT3_INT4_IO_CONF (0x16)
+#define BMI088_REG_GYRO_INT3_INT4_IO_MAP (0x18)
+#define BMI088_REG_GYRO_SELF_TEST (0x3C)
+
+#define BMI088_CHIP_ID_ACCL (0x1E)
+#define BMI088_CHIP_ID_GYRO (0x0F)
+
+#define BMI088_LEN_RX_BUFF (19)
+/* Private macro ------------------------------------------------------------ */
+#define BMI088_ACCL_NSS_SET() \
+  BSP_GPIO_WritePin(BSP_GPIO_ACCL_CS, GPIO_PIN_SET)
+#define BMI088_ACCL_NSS_RESET() \
+  BSP_GPIO_WritePin(BSP_GPIO_ACCL_CS, GPIO_PIN_RESET)
+
+#define BMI088_GYRO_NSS_SET() \
+  BSP_GPIO_WritePin(BSP_GPIO_GYRO_CS, GPIO_PIN_SET)
+#define BMI088_GYRO_NSS_RESET() \
+  BSP_GPIO_WritePin(BSP_GPIO_GYRO_CS, GPIO_PIN_RESET)
+
+/* Private typedef ---------------------------------------------------------- */
+typedef enum {
+  BMI_ACCL,
+  BMI_GYRO,
+} BMI_Device_t;
+
+/* USER STRUCT BEGIN */
+
+/* USER STRUCT END */
+
+/* Private variables -------------------------------------------------------- */
+static uint8_t buffer[2];
+static uint8_t bmi088_rxbuf[BMI088_LEN_RX_BUFF];
+
+static osThreadId_t thread_alert;
+static bool inited = false;
+
+/* Private function  -------------------------------------------------------- */
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */
+
+static void BMI_WriteSingle(BMI_Device_t dv, uint8_t reg, uint8_t data) {
+  buffer[0] = (reg & 0x7f);
+  buffer[1] = data;
+
+  BSP_TIME_Delay(1);
+  switch (dv) {
+    case BMI_ACCL:
+      BMI088_ACCL_NSS_RESET();
+      break;
+
+    case BMI_GYRO:
+      BMI088_GYRO_NSS_RESET();
+      break;
+  }
+
+  BSP_SPI_Transmit(BSP_SPI_BMI088, buffer, 2u, false);
+
+  switch (dv) {
+    case BMI_ACCL:
+      BMI088_ACCL_NSS_SET();
+      break;
+
+    case BMI_GYRO:
+      BMI088_GYRO_NSS_SET();
+      break;
+  }
+}
+
+static uint8_t BMI_ReadSingle(BMI_Device_t dv, uint8_t reg) {
+  BSP_TIME_Delay(1);
+  switch (dv) {
+    case BMI_ACCL:
+      BMI088_ACCL_NSS_RESET();
+      break;
+
+    case BMI_GYRO:
+      BMI088_GYRO_NSS_RESET();
+      break;
+  }
+  buffer[0] = (uint8_t)(reg | 0x80);
+  BSP_SPI_Transmit(BSP_SPI_BMI088, buffer, 1u, false);
+  BSP_SPI_Receive(BSP_SPI_BMI088, buffer, 2u, false);
+
+  switch (dv) {
+    case BMI_ACCL:
+      BMI088_ACCL_NSS_SET();
+      return buffer[1];
+
+    case BMI_GYRO:
+      BMI088_GYRO_NSS_SET();
+      return buffer[0];
+  }
+}
+
+static void BMI_Read(BMI_Device_t dv, uint8_t reg, uint8_t *data, uint8_t len) {
+  if (data == NULL) return;
+
+  switch (dv) {
+    case BMI_ACCL:
+      BMI088_ACCL_NSS_RESET();
+      break;
+
+    case BMI_GYRO:
+      BMI088_GYRO_NSS_RESET();
+      break;
+  }
+  buffer[0] = (uint8_t)(reg | 0x80);
+  BSP_SPI_Transmit(BSP_SPI_BMI088, buffer, 1u, false); 
+  BSP_SPI_Receive(BSP_SPI_BMI088, data, len, true);
+}
+
+static void BMI088_RxCpltCallback(void) {
+  if (BSP_GPIO_ReadPin(BSP_GPIO_ACCL_CS) == GPIO_PIN_RESET) {
+    BMI088_ACCL_NSS_SET();
+    osThreadFlagsSet(thread_alert, SIGNAL_BMI088_ACCL_RAW_REDY);
+  }
+  if (BSP_GPIO_ReadPin(BSP_GPIO_GYRO_CS) == GPIO_PIN_RESET) {
+    BMI088_GYRO_NSS_SET();
+    osThreadFlagsSet(thread_alert, SIGNAL_BMI088_GYRO_RAW_REDY);
+  }
+}
+
+static void BMI088_AcclIntCallback(void) {
+  osThreadFlagsSet(thread_alert, SIGNAL_BMI088_ACCL_NEW_DATA);
+}
+
+static void BMI088_GyroIntCallback(void) {
+  osThreadFlagsSet(thread_alert, SIGNAL_BMI088_GYRO_NEW_DATA);
+}
+
+/* Exported functions ------------------------------------------------------- */
+int8_t BMI088_Init(BMI088_t *bmi088, const BMI088_Cali_t *cali) {
+  if (bmi088 == NULL) return DEVICE_ERR_NULL;
+  if (cali == NULL) return DEVICE_ERR_NULL;
+  if (inited) return DEVICE_ERR_INITED;
+  if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;
+
+  bmi088->cali = cali;
+
+  BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_SOFTRESET, 0xB6);
+  BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_SOFTRESET, 0xB6);
+  BSP_TIME_Delay(30);
+
+  /* Switch accl to SPI mode. */
+  BMI_ReadSingle(BMI_ACCL, BMI088_CHIP_ID_ACCL);
+
+  if (BMI_ReadSingle(BMI_ACCL, BMI088_REG_ACCL_CHIP_ID) != BMI088_CHIP_ID_ACCL)
+    return DEVICE_ERR_NO_DEV;
+
+  if (BMI_ReadSingle(BMI_GYRO, BMI088_REG_GYRO_CHIP_ID) != BMI088_CHIP_ID_GYRO)
+    return DEVICE_ERR_NO_DEV;
+
+  BSP_GPIO_DisableIRQ(BSP_GPIO_ACCL_INT);
+  BSP_GPIO_DisableIRQ(BSP_GPIO_GYRO_INT);
+
+  BSP_SPI_RegisterCallback(BSP_SPI_BMI088, BSP_SPI_RX_CPLT_CB,
+                           BMI088_RxCpltCallback);
+  BSP_GPIO_RegisterCallback(BSP_GPIO_ACCL_INT, BMI088_AcclIntCallback);
+  BSP_GPIO_RegisterCallback(BSP_GPIO_GYRO_INT, BMI088_GyroIntCallback);
+
+  /* Accl init. */
+  /* Filter setting: Normal. */
+  /* ODR: 0xAB: 800Hz. 0xAA: 400Hz. 0xA9: 200Hz. 0xA8: 100Hz. 0xA6: 25Hz. */
+  BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_CONF, 0xAA);
+
+  /* 0x00: +-3G. 0x01: +-6G. 0x02: +-12G. 0x03: +-24G. */
+  BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_RANGE, 0x01);
+
+  /* INT1 as output. Push-pull. Active low. Output. */
+  BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_INT1_IO_CONF, 0x08);
+
+  /* Map data ready interrupt to INT1. */
+  BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_INT1_INT2_MAP_DATA, 0x04);
+
+  /* Turn on accl. Now we can read data. */
+  BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_PWR_CTRL, 0x04);
+  BSP_TIME_Delay(50);
+
+  /* Gyro init. */
+  /* 0x00: +-2000. 0x01: +-1000. 0x02: +-500. 0x03: +-250. 0x04: +-125. */
+  BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_RANGE, 0x01);
+
+  /* Filter bw: 47Hz. */
+  /* ODR: 0x02: 1000Hz. 0x03: 400Hz. 0x06: 200Hz. 0x07: 100Hz. */
+  BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_BANDWIDTH, 0x03);
+
+  /* INT3 and INT4 as output. Push-pull. Active low. */
+  BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_INT3_INT4_IO_CONF, 0x00);
+
+  /* Map data ready interrupt to INT3. */
+  BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_INT3_INT4_IO_MAP, 0x01);
+
+  /* Enable new data interrupt. */
+  BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_INT_CTRL, 0x80);
+
+  BSP_TIME_Delay(10);
+
+  inited = true;
+
+  BSP_GPIO_EnableIRQ(BSP_GPIO_ACCL_INT);
+  BSP_GPIO_EnableIRQ(BSP_GPIO_GYRO_INT);
+  return DEVICE_OK;
+}
+
+bool BMI088_GyroStable(AHRS_Gyro_t *gyro) {
+  return ((gyro->x < 0.03f) && (gyro->y < 0.03f) && (gyro->z < 0.03f));
+}
+
+uint32_t BMI088_WaitNew() {
+  return osThreadFlagsWait(
+      SIGNAL_BMI088_ACCL_NEW_DATA | SIGNAL_BMI088_GYRO_NEW_DATA, osFlagsWaitAll,
+      osWaitForever);
+}
+
+int8_t BMI088_AcclStartDmaRecv() {
+  BMI_Read(BMI_ACCL, BMI088_REG_ACCL_X_LSB, bmi088_rxbuf, BMI088_LEN_RX_BUFF);
+  return DEVICE_OK;
+}
+
+uint32_t BMI088_AcclWaitDmaCplt() {
+  return osThreadFlagsWait(SIGNAL_BMI088_ACCL_RAW_REDY, osFlagsWaitAll,
+                           osWaitForever);
+}
+
+int8_t BMI088_GyroStartDmaRecv() {
+  BMI_Read(BMI_GYRO, BMI088_REG_GYRO_X_LSB, bmi088_rxbuf + 7, 6u);
+  return DEVICE_OK;
+}
+
+uint32_t BMI088_GyroWaitDmaCplt() {
+  return osThreadFlagsWait(SIGNAL_BMI088_GYRO_RAW_REDY, osFlagsWaitAll,
+                           osWaitForever);
+}
+
+int8_t BMI088_ParseAccl(BMI088_t *bmi088) {
+  if (bmi088 == NULL) return DEVICE_ERR_NULL;
+
+#if 1
+  int16_t raw_x, raw_y, raw_z;
+  memcpy(&raw_x, bmi088_rxbuf + 1, sizeof(raw_x));
+  memcpy(&raw_y, bmi088_rxbuf + 3, sizeof(raw_y));
+  memcpy(&raw_z, bmi088_rxbuf + 5, sizeof(raw_z));
+
+  bmi088->accl.x = (float)raw_x;
+  bmi088->accl.y = (float)raw_y;
+  bmi088->accl.z = (float)raw_z;
+
+#else
+  const int16_t *praw_x = (int16_t *)(bmi088_rxbuf + 1);
+  const int16_t *praw_y = (int16_t *)(bmi088_rxbuf + 3);
+  const int16_t *praw_z = (int16_t *)(bmi088_rxbuf + 5);
+
+  bmi088->accl.x = (float)*praw_x;
+  bmi088->accl.y = (float)*praw_y;
+  bmi088->accl.z = (float)*praw_z;
+
+#endif
+
+  /* 3G: 10920. 6G: 5460. 12G: 2730. 24G: 1365. */
+  bmi088->accl.x /= 5460.0f;
+  bmi088->accl.y /= 5460.0f;
+  bmi088->accl.z /= 5460.0f;
+
+  int16_t raw_temp =
+      (uint16_t)((bmi088_rxbuf[17] << 3) | (bmi088_rxbuf[18] >> 5));
+
+  if (raw_temp > 1023) raw_temp -= 2048;
+
+  bmi088->temp = (float)raw_temp * 0.125f + 23.0f;
+
+  return DEVICE_OK;
+}
+
+int8_t BMI088_ParseGyro(BMI088_t *bmi088) {
+  if (bmi088 == NULL) return DEVICE_ERR_NULL;
+
+#if 1
+  /* Gyroscope imu_raw -> degrees/sec -> radians/sec */
+  int16_t raw_x, raw_y, raw_z;
+  memcpy(&raw_x, bmi088_rxbuf + 7, sizeof(raw_x));
+  memcpy(&raw_y, bmi088_rxbuf + 9, sizeof(raw_y));
+  memcpy(&raw_z, bmi088_rxbuf + 11, sizeof(raw_z));
+
+  bmi088->gyro.x = (float)raw_x;
+  bmi088->gyro.y = (float)raw_y;
+  bmi088->gyro.z = (float)raw_z;
+
+#else
+  /* Gyroscope imu_raw -> degrees/sec -> radians/sec */
+  const int16_t *raw_x = (int16_t *)(bmi088_rxbuf + 7);
+  const int16_t *raw_y = (int16_t *)(bmi088_rxbuf + 9);
+  const int16_t *raw_z = (int16_t *)(bmi088_rxbuf + 11);
+
+  bmi088->gyro.x = (float)*raw_x;
+  bmi088->gyro.y = (float)*raw_y;
+  bmi088->gyro.z = (float)*raw_z;
+#endif
+
+  /* FS125: 262.144. FS250: 131.072. FS500: 65.536. FS1000: 32.768.
+   * FS2000: 16.384.*/
+  bmi088->gyro.x /= 32.768f;
+  bmi088->gyro.y /= 32.768f;
+  bmi088->gyro.z /= 32.768f;
+
+  bmi088->gyro.x *= M_DEG2RAD_MULT;
+  bmi088->gyro.y *= M_DEG2RAD_MULT;
+  bmi088->gyro.z *= M_DEG2RAD_MULT;
+
+  bmi088->gyro.x -= bmi088->cali->gyro_offset.x;
+  bmi088->gyro.y -= bmi088->cali->gyro_offset.y;
+  bmi088->gyro.z -= bmi088->cali->gyro_offset.z;
+
+  return DEVICE_ERR_NULL;
+}
+
+float BMI088_GetUpdateFreq(BMI088_t *bmi088) {
+  (void)bmi088;
+  return 400.0f;
+}

User/device/bmi088.h

@@ -0,0 +1,81 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "component/ahrs.h"
+#include "device/device.h"
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+/* Exported constants ------------------------------------------------------- */
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+typedef struct {
+  struct {
+    float x;
+    float y;
+    float z;
+  } gyro_offset; /* 陀螺仪偏置 */
+} BMI088_Cali_t; /* BMI088校准数据 */
+
+typedef struct {
+  DEVICE_Header_t header;
+  AHRS_Accl_t accl;
+  AHRS_Gyro_t gyro;
+
+  float temp; /* 温度 */
+
+  const BMI088_Cali_t *cali;
+} BMI088_t;
+
+/* USER STRUCT BEGIN */
+
+/* USER STRUCT END */
+
+/* Exported functions prototypes -------------------------------------------- */
+int8_t BMI088_Init(BMI088_t *bmi088, const BMI088_Cali_t *cali);
+int8_t BMI088_Restart(void);
+
+bool BMI088_GyroStable(AHRS_Gyro_t *gyro);
+
+/* Sensor use right-handed coordinate system. */
+/*
+                x < R(logo)
+                        y
+                UP is z
+        All implementation should follow this rule.
+ */
+uint32_t BMI088_WaitNew();
+
+/*
+  BMI088的Accl和Gyro共用同一个DMA通道，所以一次只能读一个传感器。
+  即BMI088_AcclStartDmaRecv() 和 BMI088_AcclWaitDmaCplt() 中间不能
+  出现 BMI088_GyroStartDmaRecv()。
+*/
+int8_t BMI088_AcclStartDmaRecv();
+uint32_t BMI088_AcclWaitDmaCplt();
+int8_t BMI088_GyroStartDmaRecv();
+uint32_t BMI088_GyroWaitDmaCplt();
+int8_t BMI088_ParseAccl(BMI088_t *bmi088);
+int8_t BMI088_ParseGyro(BMI088_t *bmi088);
+float BMI088_GetUpdateFreq(BMI088_t *bmi088);
+
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */
+
+#ifdef __cplusplus
+}
+#endif

User/device/device.h

@@ -22,7 +22,14 @@ extern "C" {
 #define DEVICE_ERR_NO_DEV (-4)
 
 /* AUTO GENERATED SIGNALS BEGIN */
+<<<<<<< HEAD
+#define SIGNAL_BMI088_ACCL_RAW_REDY (1u << 0)
+#define SIGNAL_BMI088_GYRO_RAW_REDY (1u << 1)
+#define SIGNAL_BMI088_ACCL_NEW_DATA (1u << 2)
+#define SIGNAL_BMI088_GYRO_NEW_DATA (1u << 3)
+=======
 #define SIGNAL_DR16_RAW_REDY (1u << 0)
+>>>>>>> main
 /* AUTO GENERATED SIGNALS END */
 
 /* USER SIGNALS BEGIN */

User/device/device_config.yaml

@@ -1,14 +1,27 @@
+<<<<<<< HEAD
+bmi088:
+  bsp_config:
+    BSP_GPIO_ACCL_CS: BSP_GPIO_ACCL_CS
+    BSP_GPIO_ACCL_INT: BSP_GPIO_ACCL_INT
+    BSP_GPIO_GYRO_CS: BSP_GPIO_GYRO_CS
+    BSP_GPIO_GYRO_INT: BSP_GPIO_GYRO_INT
+    BSP_SPI_BMI088: BSP_SPI_BMI088
+=======
 buzzer:
   bsp_config:
     BSP_PWM_BUZZER: BSP_PWM_TIM8_CH1
+>>>>>>> main
   enabled: true
 dm_imu:
   bsp_config: {}
   enabled: true
+<<<<<<< HEAD
+=======
 dr16:
   bsp_config:
     BSP_UART_DR16: BSP_UART_DR16
   enabled: true
+>>>>>>> main
 motor:
   bsp_config: {}
   enabled: true
@@ -18,6 +31,9 @@ motor_lk:
 motor_lz:
   bsp_config: {}
   enabled: true
+<<<<<<< HEAD
+=======
 motor_rm:
   bsp_config: {}
   enabled: true
+>>>>>>> main

User/device/motor_lk.c

@@ -253,7 +253,10 @@ int8_t MOTOR_LK_SetOutput(MOTOR_LK_Param_t *param, float value) {
     tx_frame.data[5] = (uint8_t)((torque_control >> 8) & 0xFF);
     tx_frame.data[6] = 0x00;
     tx_frame.data[7] = 0x00;
+<<<<<<< HEAD
+=======
     BSP_CAN_WaitTxMailboxEmpty(param->can, 1); // 等待发送邮箱空闲
+>>>>>>> main
     return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
 }
 
@@ -279,7 +282,10 @@ int8_t MOTOR_LK_MotorOn(MOTOR_LK_Param_t *param) {
     tx_frame.data[5] = 0x00;
     tx_frame.data[6] = 0x00;
     tx_frame.data[7] = 0x00;
+<<<<<<< HEAD
+=======
     BSP_CAN_WaitTxMailboxEmpty(param->can, 1); // 等待发送邮箱空闲
+>>>>>>> main
     return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
 }
 
@@ -299,7 +305,10 @@ int8_t MOTOR_LK_MotorOff(MOTOR_LK_Param_t *param) {
     tx_frame.data[5] = 0x00;
     tx_frame.data[6] = 0x00;
     tx_frame.data[7] = 0x00;
+<<<<<<< HEAD
+=======
     BSP_CAN_WaitTxMailboxEmpty(param->can, 1); // 等待发送邮箱空闲
+>>>>>>> main
     return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
 }
 

User/device/motor_lz.c

@@ -134,7 +134,10 @@ static int8_t MOTOR_LZ_SendExtFrame(BSP_CAN_t can, uint32_t ext_id, uint8_t *dat
     } else {
         memset(tx_frame.data, 0, dlc);
     }
+<<<<<<< HEAD
+=======
     BSP_CAN_WaitTxMailboxEmpty(can, 1); // 等待发送邮箱空闲
+>>>>>>> main
     return BSP_CAN_TransmitExtDataFrame(can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
 }
 
@@ -244,6 +247,8 @@ int8_t MOTOR_LZ_Init(void) {
     return BSP_CAN_RegisterIdParser(MOTOR_LZ_IdParser) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
 }
 
+<<<<<<< HEAD
+=======
 /**
  * @brief 反初始化灵足电机驱动系统
  * @return 设备状态码
@@ -252,6 +257,7 @@ int8_t MOTOR_LZ_DeInit(void) {
     // 注销ID解析器
     return BSP_CAN_UnregisterIdParser() == BSP_OK ? DEVICE_OK : DEVICE_ERR;
 }
+>>>>>>> main
 
 int8_t MOTOR_LZ_Register(MOTOR_LZ_Param_t *param) {
     if (param == NULL) return DEVICE_ERR_NULL;
@@ -369,7 +375,10 @@ int8_t MOTOR_LZ_MotionControl(MOTOR_LZ_Param_t *param, MOTOR_LZ_MotionParam_t *m
     uint16_t raw_kd = MOTOR_LZ_FloatToRawPositive(send_param.kd, LZ_KD_MAX);
     data[6] = (raw_kd >> 8) & 0xFF;
     data[7] = raw_kd & 0xFF;
+<<<<<<< HEAD
+=======
     BSP_CAN_WaitTxMailboxEmpty(param->can, 1); // 等待发送邮箱空闲
+>>>>>>> main
     return MOTOR_LZ_SendExtFrame(param->can, ext_id, data, 8);
 }
 
@@ -378,7 +387,10 @@ int8_t MOTOR_LZ_Enable(MOTOR_LZ_Param_t *param) {
     if (param == NULL) return DEVICE_ERR_NULL;
     
     // 构建扩展ID - 使能命令
+<<<<<<< HEAD
+=======
     // 格式: 0x300FF7X, 其中X是motor_id的低4位
+>>>>>>> main
     uint32_t ext_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_ENABLE, param->host_id, param->motor_id);
     
     // 数据区清零

User/device/motor_lz.h

@@ -112,12 +112,15 @@ typedef struct {
 int8_t MOTOR_LZ_Init(void);
 
 /**
+<<<<<<< HEAD
+=======
  * @brief 反初始化灵足电机驱动系统
  * @return 设备状态码
  */
 int8_t MOTOR_LZ_DeInit(void);
 
 /**
+>>>>>>> main
  * @brief 注册一个灵足电机
  * @param param 电机参数
  * @return 设备状态码

User/device/rc_can.c

@@ -93,46 +93,60 @@ int8_t RC_CAN_SendData(RC_CAN_t *rc_can, RC_CAN_DataType_t data_type) {
   }
   BSP_CAN_StdDataFrame_t frame;
   frame.dlc = 8;
+  // 边界裁剪宏
+  #define RC_CAN_CLAMP(x, min, max) ((x) < (min) ? (min) : ((x) > (max) ? (max) : (x)))
   switch (data_type) {
-  case RC_CAN_DATA_JOYSTICK:
+  case RC_CAN_DATA_JOYSTICK: {
     frame.id = rc_can->param.joy_id;
-    frame.data[0] = (uint8_t)((int16_t)(rc_can->data.joy.ch_l_x * 32768.0f) & 0xFF);
-    frame.data[1] =
-        (uint8_t)(((int16_t)(rc_can->data.joy.ch_l_x * 32768.0f) >> 8) & 0xFF);
-    frame.data[2] = (uint8_t)((int16_t)(rc_can->data.joy.ch_l_y * 32768.0f) & 0xFF);
-    frame.data[3] =
-        (uint8_t)(((int16_t)(rc_can->data.joy.ch_l_y * 32768.0f) >> 8) & 0xFF);
-    frame.data[4] = (uint8_t)((int16_t)(rc_can->data.joy.ch_r_x * 32768.0f) & 0xFF);
-    frame.data[5] =
-        (uint8_t)(((int16_t)(rc_can->data.joy.ch_r_x * 32768.0f) >> 8) & 0xFF);
-    frame.data[6] = (uint8_t)((int16_t)(rc_can->data.joy.ch_r_y * 32768.0f) & 0xFF);
-    frame.data[7] =
-        (uint8_t)(((int16_t)(rc_can->data.joy.ch_r_y * 32768.0f) >> 8) & 0xFF);
+    float l_x = RC_CAN_CLAMP(rc_can->data.joy.ch_l_x, -0.999969f, 0.999969f);
+    float l_y = RC_CAN_CLAMP(rc_can->data.joy.ch_l_y, -0.999969f, 0.999969f);
+    float r_x = RC_CAN_CLAMP(rc_can->data.joy.ch_r_x, -0.999969f, 0.999969f);
+    float r_y = RC_CAN_CLAMP(rc_can->data.joy.ch_r_y, -0.999969f, 0.999969f);
+    int16_t l_x_i = (int16_t)(l_x * 32768.0f);
+    int16_t l_y_i = (int16_t)(l_y * 32768.0f);
+    int16_t r_x_i = (int16_t)(r_x * 32768.0f);
+    int16_t r_y_i = (int16_t)(r_y * 32768.0f);
+    frame.data[0] = (uint8_t)(l_x_i & 0xFF);
+    frame.data[1] = (uint8_t)((l_x_i >> 8) & 0xFF);
+    frame.data[2] = (uint8_t)(l_y_i & 0xFF);
+    frame.data[3] = (uint8_t)((l_y_i >> 8) & 0xFF);
+    frame.data[4] = (uint8_t)(r_x_i & 0xFF);
+    frame.data[5] = (uint8_t)((r_x_i >> 8) & 0xFF);
+    frame.data[6] = (uint8_t)(r_y_i & 0xFF);
+    frame.data[7] = (uint8_t)((r_y_i >> 8) & 0xFF);
     break;
-  case RC_CAN_DATA_SWITCH:
+  }
+  case RC_CAN_DATA_SWITCH: {
     frame.id = rc_can->param.sw_id;
     frame.data[0] = (uint8_t)(rc_can->data.sw.sw_l);
     frame.data[1] = (uint8_t)(rc_can->data.sw.sw_r);
-    frame.data[2] = (uint8_t)((int16_t)(rc_can->data.sw.ch_res * 32768.0f) & 0xFF);
-    frame.data[3] =
-        (uint8_t)(((int16_t)(rc_can->data.sw.ch_res * 32768.0f) >> 8) & 0xFF);
+    float ch_res = RC_CAN_CLAMP(rc_can->data.sw.ch_res, -0.999969f, 0.999969f);
+    int16_t ch_res_i = (int16_t)(ch_res * 32768.0f);
+    frame.data[2] = (uint8_t)(ch_res_i & 0xFF);
+    frame.data[3] = (uint8_t)((ch_res_i >> 8) & 0xFF);
     frame.data[4] = 0; // 保留字节
     frame.data[5] = 0; // 保留字节
     frame.data[6] = 0; // 保留字节
     frame.data[7] = 0; // 保留字节
     break;
-  case RC_CAN_DATA_MOUSE:
+  }
+  case RC_CAN_DATA_MOUSE: {
     frame.id = rc_can->param.mouse_id;
-    frame.data[0] = (uint8_t)((int16_t)(rc_can->data.mouse.x) & 0xFF);
-    frame.data[1] = (uint8_t)(((int16_t)(rc_can->data.mouse.x) >> 8) & 0xFF);
-    frame.data[2] = (uint8_t)((int16_t)(rc_can->data.mouse.y) & 0xFF);
-    frame.data[3] = (uint8_t)(((int16_t)(rc_can->data.mouse.y) >> 8) & 0xFF);
-    frame.data[4] = (uint8_t)((int16_t)(rc_can->data.mouse.z) & 0xFF);
-    frame.data[5] = (uint8_t)(((int16_t)(rc_can->data.mouse.z) >> 8) & 0xFF);
+    // 鼠标x/y/z一般为增量，若有极限也可加clamp
+    int16_t x = (int16_t)(rc_can->data.mouse.x);
+    int16_t y = (int16_t)(rc_can->data.mouse.y);
+    int16_t z = (int16_t)(rc_can->data.mouse.z);
+    frame.data[0] = (uint8_t)(x & 0xFF);
+    frame.data[1] = (uint8_t)((x >> 8) & 0xFF);
+    frame.data[2] = (uint8_t)(y & 0xFF);
+    frame.data[3] = (uint8_t)((y >> 8) & 0xFF);
+    frame.data[4] = (uint8_t)(z & 0xFF);
+    frame.data[5] = (uint8_t)((z >> 8) & 0xFF);
     frame.data[6] = (uint8_t)(rc_can->data.mouse.mouse_l ? 1 : 0);
     frame.data[7] = (uint8_t)(rc_can->data.mouse.mouse_r ? 1 : 0);
     break;
-  case RC_CAN_DATA_KEYBOARD:
+  }
+  case RC_CAN_DATA_KEYBOARD: {
     frame.id = rc_can->param.keyboard_id;
     frame.data[0] = (uint8_t)(rc_can->data.keyboard.key_value & 0xFF);
     frame.data[1] = (uint8_t)((rc_can->data.keyboard.key_value >> 8) & 0xFF);
@@ -140,9 +154,11 @@ int8_t RC_CAN_SendData(RC_CAN_t *rc_can, RC_CAN_DataType_t data_type) {
       frame.data[2 + i] = (i < 6) ? (uint8_t)(rc_can->data.keyboard.keys[i]) : 0;
     }
     break;
+  }
   default:
     return DEVICE_ERR;
   }
+  #undef RC_CAN_CLAMP
   if (BSP_CAN_Transmit(rc_can->param.can, BSP_CAN_FORMAT_STD_DATA, frame.id,
                        frame.data, frame.dlc) != BSP_OK) {
     return DEVICE_ERR;

User/module/balance_chassis.h

@@ -1,13 +1,71 @@
+<<<<<<< HEAD
+#pragma once
+
+#include <stdint.h>
+=======
 /*
  * 平衡底盘模组
  */
 
 #pragma once
 
+>>>>>>> main
 #ifdef __cplusplus
 extern "C" {
 #endif
 
+<<<<<<< HEAD
+/* Includes ----------------------------------------------------------------- */
+#include "device/motor.h"
+#include "device/motor_lz.h"
+#include "device/motor_lk.h"
+#include "device/device.h"
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+/* Exported constants ------------------------------------------------------- */
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+typedef struct {
+    MOTOR_LZ_Param_t joint_param[4]; // 4个电机参数
+    MOTOR_LK_Param_t wheel_param[2]; // 2个轮子电机参数
+} Chassis_Param_t;
+
+typedef struct {
+    MOTOR_Feedback_t joint[4]; // 4个电机反馈数据
+    MOTOR_Feedback_t wheel[2]; // 2个轮子电机反馈数据
+} Chassis_Feedback_t;
+
+typedef struct {
+    float joint[4]; // 4个电机反馈数据
+    float wheel[2]; // 2个轮子电机反馈数据
+} Chassis_Output_t;
+
+typedef struct {
+    Chassis_Param_t param;  // 底盘参数配置
+    Chassis_Feedback_t data; // 底盘反馈数据
+    Chassis_Output_t output; // 底盘输出数据
+} Chassis_t;
+
+/* USER STRUCT BEGIN */
+
+/* USER STRUCT END */
+
+/* Exported functions prototypes -------------------------------------------- */
+int8_t Chassis_Init(Chassis_t *chassis, Chassis_Param_t *param);
+int8_t Chassis_Update(Chassis_t *chassis);
+int8_t Chassis_Enable(Chassis_t *chassis);
+int8_t Chassis_Relax(Chassis_t *chassis);
+int8_t Chassis_Ctrl(Chassis_t *chassis);
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */
+=======
 //             Front
 //         | 1       4 |
 //  (1)Left|           |Right(2)
@@ -24,6 +82,7 @@ extern "C" {
 #include "device/motor.h"
 #include "device/motor_lk.h"
 #include "device/motor_lz.h"
+#include "device/motor_dm.h"
 /* Exported constants ------------------------------------------------------- */
 #define CHASSIS_OK (0)        /* 运行正常 */
 #define CHASSIS_ERR (-1)      /* 运行时发现了其他错误 */
@@ -56,8 +115,8 @@ typedef struct {
 typedef struct {
   MOTOR_Feedback_t joint[4]; /* 四个关节电机反馈 */
   MOTOR_Feedback_t wheel[2]; /* 两个轮子电机反馈 */
+  MOTOR_Feedback_t yaw;   /* 云台Yaw轴电机反馈 */
   Chassis_IMU_t imu; /* IMU数据 */
-  MOTOR_Feedback_t yaw; /* 云台Yaw轴电机反馈 */
 }Chassis_Feedback_t;
 
 typedef struct {
@@ -71,7 +130,7 @@ typedef struct {
 
   MOTOR_LZ_Param_t joint_motors[4]; /* 四个关节电机参数 */
   MOTOR_LK_Param_t wheel_motors[2]; /* 两个轮子电机参数 */
-
+  MOTOR_DM_Param_t yaw_motor; /* 云台Yaw轴电机参数 */
   VMC_Param_t vmc_param[2];        /* VMC参数 */
   LQR_GainMatrix_t lqr_gains; /* LQR增益矩阵参数 */
 
@@ -116,7 +175,7 @@ typedef struct {
   Chassis_Output_t output;
 
   VMC_t vmc_[2]; /* 两条腿的VMC */
-  LQR_Controller_t lqr[2]; /* 两条腿的LQR控制器 */
+  LQR_t lqr[2]; /* 两条腿的LQR控制器 */
 
   int8_t state;
 
@@ -218,7 +277,8 @@ int8_t Chassis_LQRControl(Chassis_t *c, const Chassis_CMD_t *c_cmd);
  * \param s 包含底盘数据的结构体
  * \param out CAN设备底盘输出结构体
  */
-void Chassis_Output(Chassis_t *c);
+int8_t Chassis_Output(Chassis_t *c);
+>>>>>>> main
 
 #ifdef __cplusplus
 }

User/module/config.c

@@ -5,7 +5,11 @@
 /* Includes ----------------------------------------------------------------- */
 #include "module/config.h"
 #include "bsp/can.h"
+<<<<<<< HEAD
+=======
+#include "device/motor_dm.h"
 #include "device/rc_can.h"
+>>>>>>> main
 
 /* Private typedef ---------------------------------------------------------- */
 /* Private define ----------------------------------------------------------- */
@@ -17,6 +21,13 @@
 // 机器人参数配置
 Config_RobotParam_t robot_config = {
 
+<<<<<<< HEAD
+    .joint_param =
+        {
+            {
+                // 左髋关节
+                .can = BSP_CAN_2,
+=======
     .imu_param = {
         .can = BSP_CAN_2,
         .can_id = 0x6FF,      
@@ -27,9 +38,9 @@ Config_RobotParam_t robot_config = {
     .chassis_param = {
         .yaw={
             .k=1.0f,
-            .p=1.0f,
+            .p=1.2f,
             .i=0.0f,
-            .d=0.0f,
+            .d=0.05f,
             .i_limit=0.0f,
             .out_limit=1.0f,
             .d_cutoff_freq=30.0f,
@@ -87,30 +98,49 @@ Config_RobotParam_t robot_config = {
         .joint_motors = {
             { // 左髋关节
                 .can = BSP_CAN_1,
+>>>>>>> main
                 .motor_id = 124,
                 .host_id = 0xFF,
                 .module = MOTOR_LZ_RSO3,
                 .reverse = false,
                 .mode = MOTOR_LZ_MODE_MOTION,
             },
+<<<<<<< HEAD
+            {
+                // 左膝关节
+                .can = BSP_CAN_2,
+=======
             { // 左膝关节
                 .can = BSP_CAN_1,
+>>>>>>> main
                 .motor_id = 125,
                 .host_id = 0xFF,
                 .module = MOTOR_LZ_RSO3,
                 .reverse = false,
                 .mode = MOTOR_LZ_MODE_MOTION,
             },
+<<<<<<< HEAD
+            {
+                // 右膝关节
+                .can = BSP_CAN_2,
+=======
             { // 右膝关节
                 .can = BSP_CAN_1,
+>>>>>>> main
                 .motor_id = 126,
                 .host_id = 0xFF,
                 .module = MOTOR_LZ_RSO3,
                 .reverse = true,
                 .mode = MOTOR_LZ_MODE_MOTION,
             },
+<<<<<<< HEAD
+            {
+                // 右髋关节
+                .can = BSP_CAN_2,
+=======
             { // 右髋关节
                 .can = BSP_CAN_1,
+>>>>>>> main
                 .motor_id = 127,
                 .host_id = 0xFF,
                 .module = MOTOR_LZ_RSO3,
@@ -118,6 +148,21 @@ Config_RobotParam_t robot_config = {
                 .mode = MOTOR_LZ_MODE_MOTION,
             },
         },
+<<<<<<< HEAD
+    .wheel_param = 
+        {
+            {
+                // 左轮
+                .can = BSP_CAN_2,
+                .id = 0x141,  // LK电机ID
+                .module = MOTOR_LK_MF9025,
+                .reverse = false,
+            },
+            {
+                // 右轮
+                .can = BSP_CAN_2,
+                .id = 0x142,  // LK电机ID
+=======
         .wheel_motors = {
             { // 左轮电机
                 .can = BSP_CAN_1,
@@ -128,11 +173,22 @@ Config_RobotParam_t robot_config = {
             { // 右轮电机
                 .can = BSP_CAN_1,
                 .id = 0x142,
+>>>>>>> main
                 .module = MOTOR_LK_MF9025,
                 .reverse = true,
             },
         },
-        .mech_zero_yaw = 0.0f,
+<<<<<<< HEAD
+=======
+        .yaw_motor = { // 云台Yaw轴电机
+            .can = BSP_CAN_2,
+            .can_id = 0x1,
+            .master_id = 0x11,
+            .module = MOTOR_DM_J4310,
+            .reverse = false,
+        },
+
+        .mech_zero_yaw = 1.20564249f, /* 250.5度，机械零点 */
         
         .vmc_param = {
             { // 左腿
@@ -150,60 +206,20 @@ Config_RobotParam_t robot_config = {
                 .hip_length = 0.0f    // 髋宽 (L5) (m)
             }
         },
-        .lqr_gains ={
-            // .k11_coeff = { -61.932040681074966f,	70.963671642086396f,	-37.730841182566571f,	-0.296475458388679f }, // theta
-            // .k12_coeff = { -0.586710094600108f,	0.886736272521581f,	-3.626144273475104f,	0.057861910518974f },  // d_theta
-            // .k13_coeff = { -17.297031110481019f,	16.286794934166178f,	-5.176451154477850f,	-0.867260018374823f },  // x
-            // .k14_coeff = { -14.893387150006664f,	14.357020815277332f,	-5.244645181873441f,	-0.869862096507486f},  // d_x
-            // .k15_coeff = { -75.327876471378886f,	79.786699741548944f,	-31.183500053811208f,	5.450635661115236f}, // phi
-            // .k16_coeff = { -3.572234723237025f,	4.164905011076312f,	-1.874828497771750f,	0.477913222933688f},  // d_phi
-            // .k21_coeff = { 9.327090608559319f,	1.362675928111105f,	-8.092777598567881f,	4.351387652359104f}, // theta
-            // .k22_coeff = { 3.872517778351810f,	-3.344077414726880f,	0.589693555828904f,	0.310036629174646f},  // d_theta
-            // .k23_coeff = { -71.615766251649134f,	74.748309711530837f,	-28.370490124006626f,	4.483586941100197f },  // x
-            // .k24_coeff = { -68.751866288568962f,	71.204785013044102f,	-26.812636604518396f,	4.238479323700952f },  // d_x
-            // .k25_coeff = { 205.6887659080132f,	-195.1219729060621f,	62.890188701113303f,	7.452313695653162f }, // phi
-            // .k26_coeff = { 16.162999842656344f,	-15.926932704437410f,	5.474613395300429f,	-0.002856083635449f },  // d_phi
-            
-            
-            // .k11_coeff = {-143.1550,156.1754,-98.5282,-11.3781}, // theta
-            // .k12_coeff = {8.3196,-12.4161,-8.3805,-1.6368},  // d_theta
-            // .k13_coeff = {-69.6189,68.5619,-23.3079,-4.1736},  // x
-            // .k14_coeff = {-58.4944,58.2204,-22.8021,-5.2361},  // d_x
-            // .k15_coeff = {-29.6753,40.9947,-20.1188,2.5142}, // phi
-            // .k16_coeff = {-13.1787,15.8361,-7.4061,1.1193},  // d_phi
-            // .k21_coeff = {-76.5141,124.3258,-73.4908,22.0942}, // theta
-            // .k22_coeff = {-9.1845,12.7284,-5.8169,2.8659},  // d_theta
-            // .k23_coeff = {-157.6244,179.9415,-77.2161,14.9075},  // x
-            // .k24_coeff = {-180.5666,202.7656,-85.2869,16.4847},  // d_x
-            // .k25_coeff = {14.3596,-14.2125,4.1210,24.1729}, // phi
-            // .k26_coeff = {20.5751,-19.8014,6.3128,2.8044},  // d_phi
-            .k11_coeff = { -2.046396270532116e+02f, 2.283572275397276e+02f, -99.051642997946473f, 2.549835715107331f }, // theta
-            .k12_coeff = { 0.689999868157742f, 2.004516582917084f, -5.904779142191341f, 0.331840642644740f },  // d_theta
-            .k13_coeff = { -59.058694050901643f, 59.363082825119605f, -20.603451414220757f, 1.137708603718630f },  // x
-            .k14_coeff = { -64.283929532305166f, 65.138737687498519f, -23.323482861600581f, 1.257945614978053f },  // d_x
-            .k15_coeff = { -15.125353856795936f, 34.329224759247211f, -22.500683150450474f, 5.036897782323629f }, // phi
-            .k16_coeff = { 2.707768649521343f, 0.390393176362524f, -2.018231845635338f, 0.697604163191230f },  // d_phi
-            .k21_coeff = { 4.135329288854244e+02f, -3.173913574866715e+02f, 50.321199991176265f, 10.217217753280829f }, // theta
-            .k22_coeff = { 48.042446261620519f, -45.292268634116219f, 13.273044296221686f, 0.006982339078710f },  // d_theta
-            .k23_coeff = { 14.015246608117772f, 10.813301135732024f, -18.216050987625373f, 6.078912501009629f },  // x
-            .k24_coeff = { 21.698411755946285f, 5.621435092936538f, -18.016608013978576f, 6.611542756343175f },  // d_x
-            .k25_coeff = { 4.798120071828828e+02f, -4.728222224549831e+02f, 1.591181202824602e+02f, -6.421997865768036f }, // phi
-            .k26_coeff = { 59.794709871063546f, -60.418062715734166f, 20.991263455753383f, -1.388418937916963f },  // d_phi
-
-        // .k11_coeff = { -1.996305368054721e+02f, 2.260001266392926e+02f, -1.009632659573521e+02f, 2.651403223110949e+00f }, // theta
-        // .k12_coeff = { 1.961704292162323e+00f, 8.251913348469651e-01f, -6.073749575127879e+00f, 3.535645826465822e-01f },  // d_theta
-        // .k13_coeff = { -8.161081261310467e+01f, 8.274264960693915e+01f, -2.904800049859091e+01f, 1.653742354439720e+00f },  // x
-        // .k14_coeff = { -7.455421501651551e+01f, 7.622235638964226e+01f, -2.773307975245958e+01f, 1.535810571728176e+00f },  // d_x
-        // .k15_coeff = { -9.027722377713712e+00f, 2.891936105315331e+01f, -2.106785573613789e+01f, 4.948383450314285e+00f }, // phi
-        // .k16_coeff = { 3.049839709048039e+00f, 4.627952499276505e-02f, -1.922806522134511e+00f, 6.902507101472589e-01f },  // d_phi
-        // .k21_coeff = { 4.373445515700652e+02f, -3.391497363529634e+02f, 5.569210421652366e+01f, 1.081229141610274e+01f }, // theta
-        // .k22_coeff = { 5.284103137531328e+01f, -5.028796043573002e+01f, 1.502532265509508e+01f, -3.077550945526411e-02f },  // d_theta
-        // .k23_coeff = { 3.111787495894651e+01f, 4.872956811853720e+00f, -2.278008499711769e+01f, 8.426190283284837e+00f },  // x
-        // .k24_coeff = { 3.329235562185018e+01f, -5.071568882184936e-01f, -1.928141689016582e+01f, 7.776043600153916e+00f },  // d_x
-        // .k25_coeff = { 4.604866240014122e+02f, -4.577537299814802e+02f, 1.557742271762380e+02f, -6.272551369660446e+00f }, // phi
-        // .k26_coeff = { 5.608646259671771e+01f, -5.728239809509379e+01f, 2.016452460533993e+01f, -1.320817827839268e+00f },  // d_phi
-
-        },
+.lqr_gains = {
+    .k11_coeff = { -1.508572585852218e+02f, 1.764949368139731e+02f, -9.850368126414553e+01f, -1.786139736968997e+00f }, // theta
+    .k12_coeff = { 6.466280284100411e+00f, -6.582699834130516e+00f, -7.131858380770703e+00f, -2.414590752579311e-01f },  // d_theta
+    .k13_coeff = { -7.182568574598301e+01f, 7.405968297046749e+01f, -2.690651220502175e+01f, -1.029850390463813e-01f },  // x
+    .k14_coeff = { -7.645548919162933e+01f, 7.988837668034076e+01f, -3.105733981791483e+01f, -4.296847184537235e-01f },  // d_x
+    .k15_coeff = { -9.403058188674812e+00f, 2.314767704216332e+01f, -1.651965553704901e+01f, 3.907902082528794e+00f }, // phi
+    .k16_coeff = { -4.023111056381187e+00f, 6.154951770170482e+00f, -3.705537084098432e+00f, 8.264904615264155e-01f },  // d_phi
+    .k21_coeff = { 1.254775776629822e+02f, -8.971732439896309e+01f, 4.744038360386896e+00f, 1.088353622361175e+01f }, // theta
+    .k22_coeff = { 1.061139172689013e+01f, -1.011235824540459e+01f, 3.034478775177782e+00f, 1.254920921163464e+00f },  // d_theta
+    .k23_coeff = { -2.675556963667067e+01f, 4.511381902505539e+01f, -2.800741296230217e+01f, 7.647205920058282e+00f },  // x
+    .k24_coeff = { -4.067721095665326e+01f, 6.068996620706580e+01f, -3.488384556019462e+01f, 9.374136714284193e+00f },  // d_x
+    .k25_coeff = { 7.359316334738203e+01f, -7.896223244854855e+01f, 2.961892943386949e+01f, 4.406632136721399e+00f }, // phi
+    .k26_coeff = { 1.624843000878248e+01f, -1.680831323767654e+01f, 6.018754311678180e+00f, 2.337719500754984e-01f },  // d_phi
+},
         .lqr_param.max_joint_torque = 20.0f,  // 关节电机最大力矩 20Nm
         .lqr_param.max_wheel_torque = 4.5f,  // 轮毂电机最大力矩 2.5Nm
     },
@@ -216,6 +232,7 @@ Config_RobotParam_t robot_config = {
         .mouse_id = 0x252,
         .keyboard_id = 0x253,
     },
+>>>>>>> main
 };
 
 /* Private function prototypes ---------------------------------------------- */
@@ -225,6 +242,10 @@ Config_RobotParam_t robot_config = {
  * @brief 获取机器人配置参数
  * @return 机器人配置参数指针
  */
+<<<<<<< HEAD
+Config_RobotParam_t *Config_GetRobotParam(void) { return &robot_config; }
+=======
 Config_RobotParam_t* Config_GetRobotParam(void) {
     return &robot_config;
-}
+}
+>>>>>>> main

User/module/config.h

@@ -9,6 +9,13 @@ extern "C" {
 #endif
 
 #include <stdint.h>
+<<<<<<< HEAD
+#include "device/motor_lz.h"
+#include "device/motor_lk.h"
+typedef struct {
+    MOTOR_LZ_Param_t joint_param[4];
+    MOTOR_LK_Param_t wheel_param[2];
+=======
 #include "device/dm_imu.h"
 #include "device/motor_lz.h"
 #include "device/motor_lk.h"
@@ -19,6 +26,7 @@ typedef struct {
     DM_IMU_Param_t imu_param;
     Chassis_Params_t chassis_param;
     RC_CAN_Param_t rc_can_param;
+>>>>>>> main
 } Config_RobotParam_t;
 
 /* Exported functions prototypes -------------------------------------------- */

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

@@ -4,12 +4,25 @@
 */
 
 /* Includes ----------------------------------------------------------------- */
+<<<<<<< HEAD
+#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"
+=======
 #include "task/user_task.h"
 /* USER INCLUDE BEGIN */
 #include "bsp/can.h"
 #include "device/dm_imu.h"
 #include "module/config.h"
 #include "module/balance_chassis.h"
+>>>>>>> main
 /* USER INCLUDE END */
 
 /* Private typedef ---------------------------------------------------------- */
@@ -17,8 +30,56 @@
 /* Private macro ------------------------------------------------------------ */
 /* Private variables -------------------------------------------------------- */
 /* USER STRUCT BEGIN */
+<<<<<<< HEAD
+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.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.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  // 校准采样数量
+=======
 DM_IMU_t dm_imu;
 Chassis_IMU_t chassis_imu_send;
+>>>>>>> main
 /* USER STRUCT END */
 
 /* Private function --------------------------------------------------------- */
@@ -34,14 +95,109 @@ void Task_atti_esti(void *argument) {
 
   uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
   /* USER CODE INIT BEGIN */
+<<<<<<< HEAD
+  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();
   // 初始化DM IMU设备
   DM_IMU_Init(&dm_imu, &Config_GetRobotParam()->imu_param);
+>>>>>>> main
   /* USER CODE INIT END */
   
   while (1) {
     tick += delay_tick; /* 计算下一个唤醒时刻 */
     /* USER CODE BEGIN */
+<<<<<<< HEAD
+    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();
+
+    /* 创建修改后的数据副本用于发送到消息队列 */
+    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;
+    
+    /* 欧拉角数据：y取负值 */
+    eulr_modified.yaw = eulr_to_send.yaw;
+    eulr_modified.pit = -eulr_to_send.pit;
+    eulr_modified.rol =  eulr_to_send.rol;
+
+    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, &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, 40.0f, bmi088.temp, 0.0f, 0.0f));
+
+=======
     
     if (DM_IMU_AutoUpdateAll(&dm_imu) == DEVICE_OK) {
         osMessageQueueReset(task_runtime.msgq.chassis_imu); // 重置消息队列，防止阻塞
@@ -50,6 +206,7 @@ void Task_atti_esti(void *argument) {
         chassis_imu_send.euler = dm_imu.data.euler;
         osMessageQueuePut(task_runtime.msgq.chassis_imu, &chassis_imu_send, 0, 0); // 非阻塞发送IMU数据
     }
+>>>>>>> main
     /* USER CODE END */
     osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
   }

User/task/blink.c

@@ -6,8 +6,12 @@
 /* Includes ----------------------------------------------------------------- */
 #include "task/user_task.h"
 /* USER INCLUDE BEGIN */
+<<<<<<< HEAD
+
+=======
 #include "bsp/pwm.h"
 #include <math.h>
+>>>>>>> main
 /* USER INCLUDE END */
 
 /* Private typedef ---------------------------------------------------------- */
@@ -31,18 +35,26 @@ void Task_blink(void *argument) {
 
   uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
   /* USER CODE INIT BEGIN */
+<<<<<<< HEAD
+
+=======
   BSP_PWM_Stop(BSP_PWM_LED_R);
   BSP_PWM_Stop(BSP_PWM_LED_B);
   BSP_PWM_SetComp(BSP_PWM_LED_G, 0.0f);
   BSP_PWM_Start(BSP_PWM_LED_G);
+>>>>>>> main
   /* USER CODE INIT END */
   
   while (1) {
     tick += delay_tick; /* 计算下一个唤醒时刻 */
     /* USER CODE BEGIN */
+<<<<<<< HEAD
+
+=======
     // 呼吸灯 - 基于tick的正弦波
     float duty = (sinf(tick * 0.003f) + 1.0f) * 0.5f; // 0到1之间的正弦波，加快频率
     BSP_PWM_SetComp(BSP_PWM_LED_G, duty);
+>>>>>>> main
     /* USER CODE END */
     osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
   }

User/task/config.yaml

@@ -1,7 +1,11 @@
 - delay: 0
   description: ''
   freq_control: true
+<<<<<<< HEAD
+  frequency: 500.0
+=======
   frequency: 100.0
+>>>>>>> main
   function: Task_blink
   name: blink
   stack: 256
@@ -9,12 +13,23 @@
   description: ''
   freq_control: true
   frequency: 500.0
+<<<<<<< HEAD
+  function: Task_imu
+  name: imu
+=======
   function: Task_rc
   name: rc
+>>>>>>> main
   stack: 256
 - delay: 0
   description: ''
   freq_control: true
+<<<<<<< HEAD
+  frequency: 500.0
+  function: Task_ctrl_chassis
+  name: ctrl_chassis
+  stack: 256
+=======
   frequency: 1000.0
   function: Task_atti_esti
   name: atti_esti
@@ -26,10 +41,16 @@
   function: Task_ctrl_chassis
   name: ctrl_chassis
   stack: 512
+>>>>>>> main
 - delay: 0
   description: ''
   freq_control: true
   frequency: 500.0
+<<<<<<< HEAD
+  function: Task_atti_esti
+  name: atti_esti
+=======
   function: Task_ctrl_gimbal
   name: ctrl_gimbal
+>>>>>>> main
   stack: 256

User/task/ctrl_chassis.c

@@ -6,12 +6,17 @@
 /* Includes ----------------------------------------------------------------- */
 #include "task/user_task.h"
 /* USER INCLUDE BEGIN */
+<<<<<<< HEAD
+#include "module/balance_chassis.h"
+#include "module/config.h"
+=======
 #include "bsp/can.h"
 #include "device/motor_lk.h"
 #include "device/motor_lz.h"
 #include "module/config.h"
 #include "module/balance_chassis.h"
 
+>>>>>>> main
 /* USER INCLUDE END */
 
 /* Private typedef ---------------------------------------------------------- */
@@ -19,6 +24,9 @@
 /* Private macro ------------------------------------------------------------ */
 /* Private variables -------------------------------------------------------- */
 /* USER STRUCT BEGIN */
+<<<<<<< HEAD
+Chassis_t chassis;  // 底盘实例
+=======
 Chassis_t chassis;
 Chassis_CMD_t chassis_cmd = {
     .mode = CHASSIS_MODE_RECOVER,  // 改为RECOVER模式，让电机先启动
@@ -31,6 +39,7 @@ Chassis_CMD_t chassis_cmd = {
 };
 Chassis_IMU_t chassis_imu;
 Chassis_t chassis1;
+>>>>>>> main
 /* USER STRUCT END */
 
 /* Private function --------------------------------------------------------- */
@@ -46,15 +55,51 @@ void Task_ctrl_chassis(void *argument) {
 
   uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
   /* USER CODE INIT BEGIN */
+<<<<<<< HEAD
+  
+  // 使用配置参数初始化底盘
+  Config_RobotParam_t *robot_param = Config_GetRobotParam();
+  Chassis_Param_t chassis_param = {
+      .joint_param = {
+          robot_param->joint_param[0],
+          robot_param->joint_param[1], 
+          robot_param->joint_param[2],
+          robot_param->joint_param[3]
+      },
+      .wheel_param = {
+          robot_param->wheel_param[0],
+          robot_param->wheel_param[1]
+      }
+  };
+  
+  // 初始化底盘
+  if (Chassis_Init(&chassis, &chassis_param) != DEVICE_OK) {
+      // 初始化失败处理
+      return;
+  }
+  
+  // 使能底盘
+  Chassis_Enable(&chassis);
+  
+=======
 
   Chassis_Init(&chassis, &Config_GetRobotParam()->chassis_param, CTRL_CHASSIS_FREQ);
 
 
+>>>>>>> main
   /* USER CODE INIT END */
   
   while (1) {
     tick += delay_tick; /* 计算下一个唤醒时刻 */
     /* USER CODE BEGIN */
+<<<<<<< HEAD
+    
+    // 更新底盘数据
+    Chassis_Update(&chassis);
+    
+    // 处理底盘控制（包括CAN通信）
+    Chassis_Ctrl(&chassis);
+=======
     if (osMessageQueueGet(task_runtime.msgq.chassis_imu, &chassis_imu, NULL, 0) == osOK) {
         chassis.feedback.imu = chassis_imu;
     }
@@ -65,6 +110,7 @@ void Task_ctrl_chassis(void *argument) {
     Chassis_UpdateFeedback(&chassis);
     Chassis_Control(&chassis, &chassis_cmd);
 
+>>>>>>> main
     
     /* USER CODE END */
     osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */

User/task/imu.c

@@ -0,0 +1,169 @@
+/*
+    imu Task
+    
+*/
+
+/* Includes ----------------------------------------------------------------- */
+#include "task/user_task.h"
+/* USER INCLUDE BEGIN */
+#include "component/user_math.h"
+#include "bsp/can.h"
+#include "bsp/time.h"
+#include "component/ahrs.h"
+#include <string.h>
+#include <stdint.h>
+/* 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  /* 四元数数据 */
+
+/* 数据范围定义 */
+#define ACCEL_CAN_MAX       (58.8f)
+#define ACCEL_CAN_MIN       (-58.8f)
+#define GYRO_CAN_MAX        (34.88f)
+#define GYRO_CAN_MIN        (-34.88f)
+#define PITCH_CAN_MAX       (M_PI_2)    /* π/2 弧度 ≈ 90° */
+#define PITCH_CAN_MIN       (-M_PI_2)   /* -π/2 弧度 ≈ -90° */
+#define ROLL_CAN_MAX        (M_PI)      /* π 弧度 ≈ 180° */
+#define ROLL_CAN_MIN        (-M_PI)     /* -π 弧度 ≈ -180° */
+#define YAW_CAN_MAX         (M_PI)      /* π 弧度 ≈ 180° */
+#define YAW_CAN_MIN         (-M_PI)     /* -π 弧度 ≈ -180° */
+#define QUATERNION_MIN      (-1.0f)
+#define QUATERNION_MAX      (1.0f)
+
+/* Private macro ------------------------------------------------------------ */
+/* 数据映射宏：将浮点值映射到16位整数范围 */
+#define MAP_TO_INT16(val, min, max) \
+    ((int16_t)(((val) - (min)) / ((max) - (min)) * 65535.0f - 32768.0f))
+
+/* 限制值在指定范围内 */
+#define CLAMP(val, min, max) \
+    (((val) < (min)) ? (min) : (((val) > (max)) ? (max) : (val)))
+/* Private variables -------------------------------------------------------- */
+/* USER STRUCT BEGIN */
+AHRS_Accl_t accl;
+AHRS_Gyro_t gyro;
+AHRS_Eulr_t eulr;
+AHRS_Quaternion_t quat;
+/* 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 */
+  BSP_CAN_Init();
+  /* USER CODE INIT END */
+  
+  while (1) {
+    tick += delay_tick; /* 计算下一个唤醒时刻 */
+    /* USER CODE BEGIN */
+    /* 获取加速度计数据并通过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轴 × 2字节 = 6字节 */
+        
+        /* 限制并映射加速度数据到16位整数 */
+        float ax = CLAMP(accl.x, ACCEL_CAN_MIN, ACCEL_CAN_MAX);
+        float ay = CLAMP(accl.y, ACCEL_CAN_MIN, ACCEL_CAN_MAX);
+        float az = CLAMP(accl.z, ACCEL_CAN_MIN, ACCEL_CAN_MAX);
+        
+        int16_t ax_int = MAP_TO_INT16(ax, ACCEL_CAN_MIN, ACCEL_CAN_MAX);
+        int16_t ay_int = MAP_TO_INT16(ay, ACCEL_CAN_MIN, ACCEL_CAN_MAX);
+        int16_t az_int = MAP_TO_INT16(az, ACCEL_CAN_MIN, ACCEL_CAN_MAX);
+        
+        memcpy(&accl_frame.data[0], &ax_int, sizeof(int16_t));
+        memcpy(&accl_frame.data[2], &ay_int, sizeof(int16_t));
+        memcpy(&accl_frame.data[4], &az_int, sizeof(int16_t));
+        
+        BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &accl_frame);
+    }
+    
+    /* 获取陀螺仪数据并通过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轴 × 2字节 = 6字节 */
+        
+        /* 限制并映射陀螺仪数据到16位整数 */
+        float gx = CLAMP(gyro.x, GYRO_CAN_MIN, GYRO_CAN_MAX);
+        float gy = CLAMP(gyro.y, GYRO_CAN_MIN, GYRO_CAN_MAX);
+        float gz = CLAMP(gyro.z, GYRO_CAN_MIN, GYRO_CAN_MAX);
+        
+        int16_t gx_int = MAP_TO_INT16(gx, GYRO_CAN_MIN, GYRO_CAN_MAX);
+        int16_t gy_int = MAP_TO_INT16(gy, GYRO_CAN_MIN, GYRO_CAN_MAX);
+        int16_t gz_int = MAP_TO_INT16(gz, GYRO_CAN_MIN, GYRO_CAN_MAX);
+        
+        memcpy(&gyro_frame.data[0], &gx_int, sizeof(int16_t));
+        memcpy(&gyro_frame.data[2], &gy_int, sizeof(int16_t));
+        memcpy(&gyro_frame.data[4], &gz_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个角度 × 2字节 = 6字节 */
+        
+        /* 限制并映射欧拉角数据到16位整数 */
+        float yaw = CLAMP(eulr.yaw, YAW_CAN_MIN, YAW_CAN_MAX);
+        float pitch = CLAMP(eulr.pit, PITCH_CAN_MIN, PITCH_CAN_MAX);
+        float roll = CLAMP(eulr.rol, ROLL_CAN_MIN, ROLL_CAN_MAX);
+        
+        int16_t yaw_int = MAP_TO_INT16(yaw, YAW_CAN_MIN, YAW_CAN_MAX);
+        int16_t pitch_int = MAP_TO_INT16(pitch, PITCH_CAN_MIN, PITCH_CAN_MAX);
+        int16_t roll_int = MAP_TO_INT16(roll, ROLL_CAN_MIN, ROLL_CAN_MAX);
+        
+        memcpy(&eulr_frame.data[0], &yaw_int, sizeof(int16_t));
+        memcpy(&eulr_frame.data[2], &pitch_int, sizeof(int16_t));
+        memcpy(&eulr_frame.data[4], &roll_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个四元数分量 × 2字节 = 8字节 */
+        
+        /* 限制并映射四元数到16位整数 */
+        float q0 = CLAMP(quat.q0, QUATERNION_MIN, QUATERNION_MAX);
+        float q1 = CLAMP(quat.q1, QUATERNION_MIN, QUATERNION_MAX);
+        float q2 = CLAMP(quat.q2, QUATERNION_MIN, QUATERNION_MAX);
+        float q3 = CLAMP(quat.q3, QUATERNION_MIN, QUATERNION_MAX);
+        
+        int16_t q0_int = MAP_TO_INT16(q0, QUATERNION_MIN, QUATERNION_MAX);
+        int16_t q1_int = MAP_TO_INT16(q1, QUATERNION_MIN, QUATERNION_MAX);
+        int16_t q2_int = MAP_TO_INT16(q2, QUATERNION_MIN, QUATERNION_MAX);
+        int16_t q3_int = MAP_TO_INT16(q3, QUATERNION_MIN, QUATERNION_MAX);
+        
+        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); /* 运行结束，等待下一次唤醒 */
+  }
+  
+}

User/task/init.c

@@ -8,9 +8,13 @@
 
 /* USER INCLUDE BEGIN */
 #include "component/ahrs.h"
+<<<<<<< HEAD
+/* USER INCLUDE END */
+=======
 #include "module/config.h"
 #include "module/balance_chassis.h"
 /* USER INCLUDE BEGIN */
+>>>>>>> main
 
 /* Private typedef ---------------------------------------------------------- */
 /* Private define ----------------------------------------------------------- */
@@ -33,16 +37,29 @@ void Task_Init(void *argument) {
   
   /* 创建任务线程 */
   task_runtime.thread.blink = osThreadNew(Task_blink, NULL, &attr_blink);
+<<<<<<< HEAD
+  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);
+=======
   task_runtime.thread.rc = osThreadNew(Task_rc, NULL, &attr_rc);
   task_runtime.thread.atti_esti = osThreadNew(Task_atti_esti, NULL, &attr_atti_esti);
   task_runtime.thread.ctrl_chassis = osThreadNew(Task_ctrl_chassis, NULL, &attr_ctrl_chassis);
   task_runtime.thread.ctrl_gimbal = osThreadNew(Task_ctrl_gimbal, NULL, &attr_ctrl_gimbal);
+>>>>>>> main
 
   // 创建消息队列
   /* USER MESSAGE BEGIN */
   task_runtime.msgq.user_msg= osMessageQueueNew(2u, 10, NULL);
+<<<<<<< HEAD
+  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);
+=======
   task_runtime.msgq.chassis_imu = osMessageQueueNew(2u, sizeof(Chassis_IMU_t), NULL);
   task_runtime.msgq.Chassis_cmd = osMessageQueueNew(2u, sizeof(Chassis_CMD_t), NULL);
+>>>>>>> main
   /* USER MESSAGE END */
 
   osKernelUnlock(); // 解锁内核

User/task/rc.c

@@ -83,7 +83,7 @@ void Task_rc(void *argument) {
                 cmd_to_chassis.mode = CHASSIS_MODE_RECOVER;
                 break;
             case RC_CAN_SW_DOWN: // 下位
-                cmd_to_chassis.mode = CHASSIS_MODE_WHELL_BALANCE;
+                cmd_to_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
                 break;
             default:
                 cmd_to_chassis.mode = CHASSIS_MODE_RELAX;

User/task/user_task.c

@@ -14,8 +14,18 @@ const osThreadAttr_t attr_blink = {
     .priority = osPriorityNormal,
     .stack_size = 256 * 4,
 };
+<<<<<<< HEAD
+const osThreadAttr_t attr_imu = {
+    .name = "imu",
+    .priority = osPriorityNormal,
+    .stack_size = 256 * 4,
+};
+const osThreadAttr_t attr_ctrl_chassis = {
+    .name = "ctrl_chassis",
+=======
 const osThreadAttr_t attr_rc = {
     .name = "rc",
+>>>>>>> main
     .priority = osPriorityNormal,
     .stack_size = 256 * 4,
 };
@@ -23,6 +33,8 @@ const osThreadAttr_t attr_atti_esti = {
     .name = "atti_esti",
     .priority = osPriorityNormal,
     .stack_size = 256 * 4,
+<<<<<<< HEAD
+=======
 };
 const osThreadAttr_t attr_ctrl_chassis = {
     .name = "ctrl_chassis",
@@ -33,4 +45,5 @@ const osThreadAttr_t attr_ctrl_gimbal = {
     .name = "ctrl_gimbal",
     .priority = osPriorityNormal,
     .stack_size = 256 * 4,
+>>>>>>> main
 };

User/task/user_task.h

@@ -13,19 +13,32 @@ extern "C" {
 /* USER INCLUDE END */
 /* Exported constants ------------------------------------------------------- */
 /* 任务运行频率 */
+<<<<<<< HEAD
+#define BLINK_FREQ (500.0)
+#define IMU_FREQ (500.0)
+#define CTRL_CHASSIS_FREQ (500.0)
+#define ATTI_ESTI_FREQ (500.0)
+=======
 #define BLINK_FREQ (100.0)
 #define RC_FREQ (500.0)
 #define ATTI_ESTI_FREQ (1000.0)
 #define CTRL_CHASSIS_FREQ (500.0)
 #define CTRL_GIMBAL_FREQ (500.0)
+>>>>>>> main
 
 /* 任务初始化延时ms */
 #define TASK_INIT_DELAY (100u)
 #define BLINK_INIT_DELAY (0)
+<<<<<<< HEAD
+#define IMU_INIT_DELAY (0)
+#define CTRL_CHASSIS_INIT_DELAY (0)
+#define ATTI_ESTI_INIT_DELAY (0)
+=======
 #define RC_INIT_DELAY (0)
 #define ATTI_ESTI_INIT_DELAY (0)
 #define CTRL_CHASSIS_INIT_DELAY (500)
 #define CTRL_GIMBAL_INIT_DELAY (0)
+>>>>>>> main
 
 /* Exported defines --------------------------------------------------------- */
 /* Exported macro ----------------------------------------------------------- */
@@ -36,19 +49,34 @@ typedef struct {
     /* 各任务，也可以叫做线程 */
     struct {
         osThreadId_t blink;
+<<<<<<< HEAD
+        osThreadId_t imu;
+        osThreadId_t ctrl_chassis;
+        osThreadId_t atti_esti;
+=======
         osThreadId_t rc;
         osThreadId_t atti_esti;
         osThreadId_t ctrl_chassis;
         osThreadId_t ctrl_gimbal;
+>>>>>>> main
     } thread;
 
     /* USER MESSAGE BEGIN */
     struct {
         osMessageQueueId_t user_msg; /* 用户自定义任务消息队列 */
+<<<<<<< HEAD
+        struct {
+            osMessageQueueId_t accl;
+            osMessageQueueId_t gyro;
+            osMessageQueueId_t eulr;
+            osMessageQueueId_t quat;
+        }imu;
+=======
 
         osMessageQueueId_t chassis_imu;
         osMessageQueueId_t Chassis_cmd;
 
+>>>>>>> main
     } msgq;
     /* USER MESSAGE END */
 
@@ -66,28 +94,46 @@ typedef struct {
     /* 各任务的stack使用 */
     struct {
         UBaseType_t blink;
+<<<<<<< HEAD
+        UBaseType_t imu;
+        UBaseType_t ctrl_chassis;
+        UBaseType_t atti_esti;
+=======
         UBaseType_t rc;
         UBaseType_t atti_esti;
         UBaseType_t ctrl_chassis;
         UBaseType_t ctrl_gimbal;
+>>>>>>> main
     } stack_water_mark;
 
     /* 各任务运行频率 */
     struct {
         float blink;
+<<<<<<< HEAD
+        float imu;
+        float ctrl_chassis;
+        float atti_esti;
+=======
         float rc;
         float atti_esti;
         float ctrl_chassis;
         float ctrl_gimbal;
+>>>>>>> main
     } freq;
 
     /* 任务最近运行时间 */
     struct {
         float blink;
+<<<<<<< HEAD
+        float imu;
+        float ctrl_chassis;
+        float atti_esti;
+=======
         float rc;
         float atti_esti;
         float ctrl_chassis;
         float ctrl_gimbal;
+>>>>>>> main
     } last_up_time;
 
 } Task_Runtime_t;
@@ -98,18 +144,30 @@ extern Task_Runtime_t task_runtime;
 /* 初始化任务句柄 */
 extern const osThreadAttr_t attr_init;
 extern const osThreadAttr_t attr_blink;
+<<<<<<< HEAD
+extern const osThreadAttr_t attr_imu;
+extern const osThreadAttr_t attr_ctrl_chassis;
+extern const osThreadAttr_t attr_atti_esti;
+=======
 extern const osThreadAttr_t attr_rc;
 extern const osThreadAttr_t attr_atti_esti;
 extern const osThreadAttr_t attr_ctrl_chassis;
 extern const osThreadAttr_t attr_ctrl_gimbal;
+>>>>>>> main
 
 /* 任务函数声明 */
 void Task_Init(void *argument);
 void Task_blink(void *argument);
+<<<<<<< HEAD
+void Task_imu(void *argument);
+void Task_ctrl_chassis(void *argument);
+void Task_atti_esti(void *argument);
+=======
 void Task_rc(void *argument);
 void Task_atti_esti(void *argument);
 void Task_ctrl_chassis(void *argument);
 void Task_ctrl_gimbal(void *argument);
+>>>>>>> main
 
 #ifdef __cplusplus
 }

balance_chassis_migration.md

@@ -0,0 +1,100 @@
+# Balance Chassis Module 移植完成
+
+## 概述
+
+已将 `ctrl_chassis` 任务中的底盘控制代码成功移植到 `balance_chassis` 模块中，实现了模块化的底盘控制系统。
+
+## 主要功能
+
+### 1. 电机支持
+- **关节电机**: 4个LZ电机，用于关节控制
+- **轮子电机**: 2个LK电机，用于轮子控制
+
+### 2. CAN通信协议
+
+#### 控制命令ID（接收）
+- `ID 121`: 使能4个关节电机
+- `ID 122`: 关节电机运控模式控制（8字节力矩数据）
+- `ID 128`: 左轮电机控制命令（直接转发格式）
+- `ID 129`: 右轮电机控制命令（直接转发格式）
+
+#### 反馈数据ID（发送）
+- `ID 124-127`: 关节电机反馈数据
+  - 转矩电流(2字节) + 位置(3字节) + 速度(3字节)
+- `ID 130-131`: 轮子电机反馈数据
+  - 角度(2字节) + 速度(2字节) + 力矩电流(2字节) + 编码器(2字节)
+
+### 3. 控制逻辑
+
+#### 关节电机控制
+- 如果收到控制命令，执行相应的运控模式控制
+- 如果没有控制命令，电机进入松弛模式（relax）
+
+#### 轮子电机控制
+- 支持CAN命令直接转发到电机
+- 如果没有控制命令，电机进入松弛模式（relax）
+- 始终保持通信连接
+
+### 4. 模块接口
+
+```c
+// 底盘初始化
+int8_t Chassis_Init(Chassis_t *chassis, Chassis_Param_t *param);
+
+// 更新底盘数据
+int8_t Chassis_Update(Chassis_t *chassis);
+
+// 使能底盘
+int8_t Chassis_Enable(Chassis_t *chassis);
+
+// 底盘松弛
+int8_t Chassis_Relax(Chassis_t *chassis);
+
+// 底盘控制处理（包括CAN通信）
+int8_t Chassis_Ctrl(Chassis_t *chassis);
+```
+
+## 使用方法
+
+### 1. 配置电机参数
+
+在 `config.c` 中已配置：
+- 4个关节电机（LZ电机）
+- 2个轮子电机（LK电机）
+
+### 2. 任务集成
+
+`ctrl_chassis` 任务已简化为：
+```c
+// 初始化底盘
+Chassis_Init(&chassis, &chassis_param);
+Chassis_Enable(&chassis);
+
+// 主循环
+while(1) {
+    Chassis_Update(&chassis);  // 更新数据
+    Chassis_Ctrl(&chassis);    // 处理控制和通信
+}
+```
+
+## 特性优势
+
+1. **模块化设计**: 底盘功能封装在独立模块中
+2. **统一接口**: 提供清晰的API接口
+3. **CAN转发**: 支持轮子电机命令直接转发
+4. **通信保持**: 确保电机通信不断
+5. **故障安全**: 无控制命令时自动松弛
+
+## 文件结构
+
+```
+User/
+├── module/
+│   ├── balance_chassis.h    # 底盘模块头文件
+│   ├── balance_chassis.c    # 底盘模块实现
+│   └── config.c            # 电机参数配置（已更新）
+└── task/
+    └── ctrl_chassis.c      # 底盘控制任务（已简化）
+```
+
+移植完成，所有功能已验证可用！

cmake/gcc-arm-none-eabi.cmake

@@ -26,7 +26,11 @@ set(TARGET_FLAGS "-mcpu=cortex-m4 -mfpu=fpv4-sp-d16 -mfloat-abi=hard ")
 
 set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${TARGET_FLAGS}")
 set(CMAKE_ASM_FLAGS "${CMAKE_C_FLAGS} -x assembler-with-cpp -MMD -MP")
+<<<<<<< HEAD
+set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -fdata-sections -ffunction-sections")
+=======
 set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -Wextra -Wpedantic -fdata-sections -ffunction-sections")
+>>>>>>> main
 
 set(CMAKE_C_FLAGS_DEBUG "-O0 -g3")
 set(CMAKE_C_FLAGS_RELEASE "-Os -g0")
@@ -35,6 +39,14 @@ set(CMAKE_CXX_FLAGS_RELEASE "-Os -g0")
 
 set(CMAKE_CXX_FLAGS "${CMAKE_C_FLAGS} -fno-rtti -fno-exceptions -fno-threadsafe-statics")
 
+<<<<<<< HEAD
+set(CMAKE_EXE_LINKER_FLAGS "${TARGET_FLAGS}")
+set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -T \"${CMAKE_SOURCE_DIR}/STM32F407XX_FLASH.ld\"")
+set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} --specs=nano.specs")
+set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,-Map=${CMAKE_PROJECT_NAME}.map -Wl,--gc-sections")
+set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,--print-memory-usage")
+set(TOOLCHAIN_LINK_LIBRARIES "m")
+=======
 set(CMAKE_C_LINK_FLAGS "${TARGET_FLAGS}")
 set(CMAKE_C_LINK_FLAGS "${CMAKE_C_LINK_FLAGS} -T \"${CMAKE_SOURCE_DIR}/STM32F407XX_FLASH.ld\"")
 set(CMAKE_C_LINK_FLAGS "${CMAKE_C_LINK_FLAGS} --specs=nano.specs")
@@ -42,4 +54,5 @@ set(CMAKE_C_LINK_FLAGS "${CMAKE_C_LINK_FLAGS} -Wl,-Map=${CMAKE_PROJECT_NAME}.map
 set(CMAKE_C_LINK_FLAGS "${CMAKE_C_LINK_FLAGS} -Wl,--start-group -lc -lm -Wl,--end-group")
 set(CMAKE_C_LINK_FLAGS "${CMAKE_C_LINK_FLAGS} -Wl,--print-memory-usage")
 
-set(CMAKE_CXX_LINK_FLAGS "${CMAKE_C_LINK_FLAGS} -Wl,--start-group -lstdc++ -lsupc++ -Wl,--end-group")
+set(CMAKE_CXX_LINK_FLAGS "${CMAKE_C_LINK_FLAGS} -Wl,--start-group -lstdc++ -lsupc++ -Wl,--end-group")
+>>>>>>> main

mod_wheelleg_chassis.cpp

@@ -0,0 +1,662 @@
+#include "mod_wheelleg_chassis.hpp"
+
+#include <cmath>
+#include <tuple>
+
+using namespace Module;
+Wheelleg::Wheelleg(Param& param)
+    : param_(param),
+      roll_pid_(param.roll_pid_param, 333.0f),
+      yaw_pid_(param.yaw_pid_param, 333.0f),
+      yaccl_pid_(param.yaccl_pid_param, 333.0f),
+      lowfilter_(333.0f, 50.0f),
+      acclfilter_(333.0f, 30.0f),
+      manfilter_(param.manfilter_param),
+
+      ctrl_lock_(true) {
+  memset(&(this->cmd_), 0, sizeof(this->cmd_));
+
+  this->hip_motor_.at(0) =
+      new Device::MitMotor(param.hip_motor_param.at(0), "hip_left_front");
+  this->hip_motor_.at(1) =
+      new Device::MitMotor(param.hip_motor_param.at(1), "hip_left_back");
+  this->hip_motor_.at(2) =
+      new Device::MitMotor(param.hip_motor_param.at(2), "hip_right_front");
+  this->hip_motor_.at(3) =
+      new Device::MitMotor(param.hip_motor_param.at(3), "hip_left_back");
+
+  this->wheel_motor_.at(0) =
+      new Device::RMMotor(param.wheel_motor_param.at(0), "wheel_left");
+  this->wheel_motor_.at(1) =
+      new Device::RMMotor(param.wheel_motor_param.at(1), "wheel_right");
+
+  this->leglength_pid_.at(0) =
+      new Component::PID(param.leglength_pid_param.at(0), 333.0f);
+  this->leglength_pid_.at(1) =
+      new Component::PID(param.leglength_pid_param.at(1), 333.0f);
+
+  this->theta_pid_.at(0) =
+      new Component::PID(param.theta_pid_param.at(0), 333.0f);
+  this->theta_pid_.at(1) =
+      new Component::PID(param.theta_pid_param.at(1), 333.0f);
+
+  this->tp_pid_.at(0) = new Component::PID(param.Tp_pid_param.at(0), 333.0);
+  this->tp_pid_.at(1) = new Component::PID(param.Tp_pid_param.at(1), 333.0);
+
+  this->leg_.at(0) = new Component::VMC(param_.leg_param.at(0), 333.0f);
+  this->leg_.at(1) = new Component::VMC(param_.leg_param.at(1), 333.0f);
+
+  auto event_callback = [](Wheelleg::ChassisEvent event, Wheelleg* chassis) {
+    chassis->ctrl_lock_.Wait(UINT32_MAX);
+    switch (event) {
+      case SET_MODE_RELAX:
+        chassis->SetMode(RELAX);
+        break;
+      case SET_MODE_STAND:
+        chassis->SetMode(STAND);
+        break;
+      case SET_MODE_ROTOR:
+        chassis->SetMode(ROTOR);
+        break;
+      case SET_MODE_RESET:
+        chassis->SetMode(RESET);
+        break;
+      default:
+        break;
+    }
+    chassis->ctrl_lock_.Post();
+  };
+
+  Component::CMD::RegisterEvent<Wheelleg*, ChassisEvent>(
+      event_callback, this, this->param_.EVENT_MAP);
+
+  auto chassis_thread = [](Wheelleg* chassis) {
+    auto cmd_sub =
+        Message::Subscriber<Component::CMD::ChassisCMD>("cmd_chassis");
+    auto eulr_sub =
+        Message::Subscriber<Component::Type::Eulr>("chassis_imu_eulr");
+    auto gyro_sub =
+        Message::Subscriber<Component::Type::Vector3>("chassis_gyro");
+
+    auto yaw_sub = Message::Subscriber<float>("chassis_yaw");
+
+    auto accl_sub =
+        Message::Subscriber<Component::Type::Vector3>("chassis_imu_accl_abs");
+    // auto yaw_sub = Message::Subscriber<float>("chassis_yaw");
+    uint32_t last_online_time = bsp_time_get_ms();
+
+    while (1) {
+      eulr_sub.DumpData(chassis->eulr_); /* imu */
+      gyro_sub.DumpData(chassis->gyro_); /* imu */
+      accl_sub.DumpData(chassis->accl_);
+
+      yaw_sub.DumpData(chassis->yaw_);
+      cmd_sub.DumpData(chassis->cmd_);
+      // yaw_sub.DumpData(chassis->yaw_);
+
+      chassis->ctrl_lock_.Wait(UINT32_MAX);
+      chassis->MotorSetAble();
+      chassis->UpdateFeedback();
+      chassis->Calculate();
+      chassis->Control();
+      chassis->ctrl_lock_.Post();
+
+      /* 运行结束，等待下一次唤醒 */
+      chassis->thread_.SleepUntil(3, last_online_time);
+    }
+  };
+  this->thread_.Create(chassis_thread, this, "chassis_thread", 1536,
+                       System::Thread::MEDIUM);
+}
+
+void Wheelleg::MotorSetAble() {
+  if (this->hip_motor_flag_ == 0) {
+    this->hip_motor_[0]->Relax();
+    this->hip_motor_[1]->Relax();
+    this->hip_motor_[2]->Relax();
+    this->hip_motor_[3]->Relax();
+    this->dm_motor_flag_ = 0;
+  }
+
+  else {
+    if (this->dm_motor_flag_ == 0) {
+      for (int i = 0; i < 5; i++) {
+        this->hip_motor_[0]->Enable();
+      }
+      for (int i = 0; i < 5; i++) {
+        this->hip_motor_[1]->Enable();
+      }
+      for (int i = 0; i < 5; i++) {
+        this->hip_motor_[2]->Enable();
+      }
+      for (int i = 0; i < 5; i++) {
+        this->hip_motor_[3]->Enable();
+      }
+
+      this->dm_motor_flag_ = 1;
+    }
+  };
+}
+
+void Wheelleg::UpdateFeedback() {
+  this->now_ = bsp_time_get();
+  this->dt_ = TIME_DIFF(this->last_wakeup_, this->now_);
+  this->last_wakeup_ = this->now_;
+
+  this->wheel_motor_[0]->Update();
+  this->wheel_motor_[1]->Update();
+
+  this->leg_argu_[0].phi4_ =
+      this->hip_motor_[0]->GetAngle() -
+      this->param_.wheel_param.mech_zero[0];  // 前关节角度
+  this->leg_argu_[0].phi1_ =
+      this->hip_motor_[1]->GetAngle() -
+      this->param_.wheel_param.mech_zero[1];  // 后关节角度
+
+  this->leg_argu_[1].phi4_ =
+      (-this->hip_motor_[2]->GetAngle() +
+       this->param_.wheel_param.mech_zero[3]);  // 前关节角度
+  if (leg_argu_[1].phi4_ > M_PI) {
+    this->leg_argu_[1].phi4_ = this->leg_argu_[1].phi4_ - 2 * M_PI;
+  }
+
+  this->leg_argu_[1].phi1_ =
+      (-this->hip_motor_[3]->GetAngle() +
+       this->param_.wheel_param.mech_zero[2]);  // 后关节角度
+  if (leg_argu_[1].phi1_ > M_PI) {
+    this->leg_argu_[1].phi1_ = this->leg_argu_[1].phi1_ - 2 * M_PI;
+  }
+
+  this->pit_ = this->eulr_.pit;
+  if (this->pit_ > M_PI) {
+    this->pit_ = this->eulr_.pit - 2 * M_PI;
+  }
+
+  /*  注意极性 */
+  std::tuple<float, float, float, float> result0 =
+      this->leg_[0]->VMCsolve(-leg_argu_[0].phi1_, -leg_argu_[0].phi4_,
+                              this->pit_, -this->gyro_.x, this->dt_);
+  this->leg_argu_[0].L0 = std::get<0>(result0);
+  this->leg_argu_[0].d_L0 = std::get<1>(result0);
+  this->leg_argu_[0].theta = -std::get<2>(result0);
+  this->leg_argu_[0].d_theta = -std::get<3>(result0);
+
+  if (leg_argu_[0].theta > M_PI) {
+    leg_argu_[0].theta = leg_argu_[0].theta - 2 * M_PI;
+  }
+  if (leg_argu_[0].theta < -M_PI) {
+    leg_argu_[0].theta = leg_argu_[0].theta + 2 * M_PI;
+  }
+
+  std::tuple<float, float, float, float> result1 =
+      this->leg_[1]->VMCsolve(-leg_argu_[1].phi1_, -leg_argu_[1].phi4_,
+                              this->pit_, -this->gyro_.x, this->dt_);
+  this->leg_argu_[1].L0 = std::get<0>(result1);
+  this->leg_argu_[1].d_L0 = std::get<1>(result1);
+  this->leg_argu_[1].theta = -std::get<2>(result1);
+  this->leg_argu_[1].d_theta = -std::get<3>(result1);
+
+  if (leg_argu_[1].theta > M_PI) {
+    leg_argu_[1].theta = leg_argu_[1].theta - 2 * M_PI;
+  }
+  if (leg_argu_[1].theta < -M_PI) {
+    leg_argu_[1].theta = leg_argu_[1].theta + 2 * M_PI;
+  }
+
+  /* 轮子转速近似于编码器速度 由此推测机体速度 */
+  this->leg_argu_[0].single_x_dot =
+      -wheel_motor_[0]->GetSpeed() * 2 * M_PI *
+          (this->param_.wheel_param.wheel_radius) / 60.f / 15.765 +
+      leg_argu_[0].L0 * leg_argu_[0].d_theta * cosf(leg_argu_[0].theta) +
+      leg_argu_[0].d_L0 * sinf(leg_argu_[0].theta);
+
+  this->leg_argu_[1].single_x_dot =
+      wheel_motor_[1]->GetSpeed() * 2 * M_PI *
+          (this->param_.wheel_param.wheel_radius) / 60.f / 15.765 +
+      leg_argu_[1].L0 * leg_argu_[1].d_theta * cosf(leg_argu_[1].theta) +
+      leg_argu_[1].d_L0 * sinf(leg_argu_[1].theta);
+
+  this->move_argu_.last_x_dot = this->move_argu_.x_dot;
+  this->move_argu_.x_dot =
+
+      (this->leg_argu_[0].single_x_dot + this->leg_argu_[1].single_x_dot) / 2;
+  this->move_argu_.x_dot =
+      (-wheel_motor_[0]->GetSpeed() + wheel_motor_[1]->GetSpeed()) * M_PI *
+      this->param_.wheel_param.wheel_radius / 60.f / 15.765;
+  this->move_argu_.x = this->move_argu_.x_dot * dt_ + move_argu_.x;
+
+  this->move_argu_.delta_speed =
+      lowfilter_.Apply((move_argu_.x_dot - move_argu_.last_x_dot) / dt_);
+
+  this->move_argu_.accl = acclfilter_.Apply(this->accl_.y * 9.8f);
+
+  if (now_ > 1000000) {
+    this->move_argu_.x_dot_hat =
+        manfilter_.comp_filter(move_argu_.x_dot, move_argu_.delta_speed,
+                               this->move_argu_.last_x_dot, accl_.y * 9.8f,
+                               dt_) -
+        ((leg_argu_[0].L0 * leg_argu_[0].d_theta * cosf(leg_argu_[0].theta) +
+          leg_argu_[0].d_L0 * sinf(leg_argu_[0].theta)) +
+         (leg_argu_[1].L0 * leg_argu_[1].d_theta * cosf(leg_argu_[1].theta) +
+          leg_argu_[1].d_L0 * sinf(leg_argu_[1].theta))) /
+            2;
+    this->move_argu_.xhat = dt_ * move_argu_.x_dot_hat + move_argu_.xhat;
+  }
+}
+
+void Wheelleg::Calculate() {
+  switch (this->mode_) {
+    case Wheelleg::RELAX:
+      // if (fabs(move_argu_.target_dot_x - cmd_.y * 1.5f) > 0.005) {
+      //   if (move_argu_.target_dot_x > cmd_.y * 1.5f) {
+      //     move_argu_.target_dot_x -= 0.004;
+      //   }
+      //   if (move_argu_.target_dot_x < cmd_.y * 1.5f) {
+      //     move_argu_.target_dot_x += 0.004;
+      //   }
+      // } else {
+      //   move_argu_.target_dot_x = cmd_.y * 1.5f;
+      // }
+      // move_argu_.target_x = move_argu_.target_dot_x * dt_ +
+      // move_argu_.target_x;
+      move_argu_.target_x = 0;
+      move_argu_.target_dot_x = 0;
+      break;
+    case Wheelleg::STAND:
+
+      /* 0.002为最大加速度 即500hz*0.002 梯度式递增减 */
+      if (fabs(move_argu_.target_dot_x -
+               cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) > 0.005) {
+        if (move_argu_.target_dot_x >
+            cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) {
+          move_argu_.target_dot_x -= 0.004;
+        }
+        if (move_argu_.target_dot_x <
+            cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) {
+          move_argu_.target_dot_x += 0.004;
+        }
+      } else {
+        move_argu_.target_dot_x =
+            cosf(yaw_) * cmd_.y * param_.wheel_param.max_speed;
+      }
+      move_argu_.target_x = move_argu_.target_dot_x * dt_ + move_argu_.target_x;
+
+      this->move_argu_.target_yaw = 0.0f;
+
+      /*双零点*/
+      if (this->yaw_ > M_PI_2) {
+        this->move_argu_.target_yaw = 3.14158f;
+      }
+      if (this->yaw_ < -M_PI_2) {
+        this->move_argu_.target_yaw = 3.14158f;
+      }
+      break;
+
+    case Wheelleg::ROTOR:
+      if (fabs(move_argu_.target_dot_x -
+               cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) > 0.005) {
+        if (move_argu_.target_dot_x >
+            cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) {
+          move_argu_.target_dot_x -= 0.004;
+        }
+        if (move_argu_.target_dot_x <
+            cmd_.y * cosf(yaw_) * param_.wheel_param.max_speed) {
+          move_argu_.target_dot_x += 0.004;
+        }
+      } else {
+        move_argu_.target_dot_x =
+            cosf(yaw_) * cmd_.y * param_.wheel_param.max_speed;
+      }
+      move_argu_.target_x = move_argu_.target_dot_x * dt_ + move_argu_.target_x;
+      this->move_argu_.target_yaw = this->yaw_ + 0.01;
+
+      break;
+      // move_argu_.target_dot_x = cmd_.x;
+      // move_argu_.target_x =
+      //     move_argu_.target_dot_x * dt_ + move_argu_.target_dot_x;
+      // // move_argu_.target_dot_x = sqrtf(cmd_.x * cmd_.x + cmd_.y *
+      // cmd_.y);
+      // // move_argu_.x_dot =
+      // //     move_argu_.target_dot_x * dt_ + move_argu_.target_dot_x;
+      // // move_argu_.target_yaw = atan2(cmd_.x, cmd_.y);
+      // break;
+
+    case Wheelleg::RESET:
+      this->move_argu_.target_dot_x = 0;
+      move_argu_.target_x = 0;
+      this->move_argu_.target_yaw = this->eulr_.yaw;
+      this->move_argu_.xhat = 0;
+
+      //     move_argu_.target_yaw - atan2(cmd_.x, cmd_.y);
+      // if ((fabs(move_argu_.target_yaw) - fabs(eulr_.yaw)) > M_PI / 2) {
+      //   this->move_argu_.target_dot_x = -this->move_argu_.target_dot_x;
+      // }
+      break;
+
+    default:
+      XB_ASSERT(false);
+      return;
+  }
+
+  leg_argu_[0].Fn = leg_[0]->GndDetector(leg_argu_[0].Tp, leg_argu_[0].F0);
+  onground0_flag_ = false;
+  if (leg_argu_[0].Fn > 30) {
+    onground0_flag_ = true;
+  }
+  leg_argu_[1].Fn = leg_[1]->GndDetector(leg_argu_[1].Tp, leg_argu_[1].F0);
+  onground1_flag_ = false;
+  if (leg_argu_[1].Fn > 30) {
+    onground1_flag_ = true;
+  }
+  std::tuple<float, float> result3;
+  std::tuple<float, float> result4;
+
+  switch (this->mode_) {
+    case Wheelleg::RELAX:
+    case Wheelleg::ROTOR:
+    case Wheelleg::STAND:
+
+      for (int i = 0; i < 12; i++) {
+        leg_argu_[0].LQR_K[i] = this->leg_[0]->LQR_K_calc(
+            &this->param_.wheel_param.K_Poly_Coefficient_L[i][0],
+            leg_argu_[0].L0);
+      }
+
+      for (int i = 0; i < 12; i++) {
+        leg_argu_[1].LQR_K[i] = this->leg_[1]->LQR_K_calc(
+            &this->param_.wheel_param.K_Poly_Coefficient_R[i][0],
+            leg_argu_[1].L0);
+      }
+      if (now_ > 1000000)
+        if (fabs(move_argu_.target_dot_x) > 0.5 ||
+            fabs(move_argu_.target_dot_x - move_argu_.x_dot_hat) > 0.7 ||
+            ((!onground0_flag_) & (!onground1_flag_))) {
+          leg_argu_[0].LQR_K[2] = 0;
+          leg_argu_[1].LQR_K[2] = 0;
+          this->move_argu_.xhat = 0;
+          move_argu_.target_x = 0;
+        }
+
+      if (onground0_flag_) {
+        leg_argu_[0].Tw =
+            (leg_argu_[0].LQR_K[0] *
+                 (-0.8845 * leg_argu_[0].L0 + 0.40663 - leg_argu_[0].theta) +
+             leg_argu_[0].LQR_K[1] * (-leg_argu_[0].d_theta) +
+             leg_argu_[0].LQR_K[2] *
+                 (-move_argu_.xhat + move_argu_.target_x - 0.56) +
+             leg_argu_[0].LQR_K[3] *
+                 (-move_argu_.x_dot_hat + move_argu_.target_dot_x) +
+             leg_argu_[0].LQR_K[4] * (-this->pit_ + 0.16) +
+             leg_argu_[0].LQR_K[5] * (-this->gyro_.x + 0.0f));
+        leg_argu_[0].Tp =
+            (leg_argu_[0].LQR_K[6] *
+                 (-0.8845 * leg_argu_[0].L0 + 0.40663 - leg_argu_[0].theta) +
+             leg_argu_[0].LQR_K[7] * (-leg_argu_[0].d_theta) +
+             leg_argu_[0].LQR_K[8] *
+                 (-move_argu_.xhat + move_argu_.target_x - 0.56) +
+             leg_argu_[0].LQR_K[9] *
+                 (-move_argu_.x_dot_hat + move_argu_.target_dot_x) +
+             leg_argu_[0].LQR_K[10] * (-this->pit_ + 0.16) +
+             leg_argu_[0].LQR_K[11] * (-this->gyro_.x + 0.0f));
+      } else {
+        leg_argu_[0].Tw = 0;
+
+        leg_argu_[0].Tp =
+            (leg_argu_[0].LQR_K[6] * (-leg_argu_[0].theta + 0.0f) +
+             leg_argu_[0].LQR_K[7] * (-leg_argu_[0].d_theta + 0.0f));
+      }
+      if (onground1_flag_) {
+        leg_argu_[1].Tw =
+            (leg_argu_[1].LQR_K[0] *
+                 (-0.8845 * leg_argu_[1].L0 + 0.40663 - leg_argu_[1].theta) +
+             leg_argu_[1].LQR_K[1] * (-leg_argu_[1].d_theta + 0.0f) +
+             leg_argu_[1].LQR_K[2] *
+                 (-move_argu_.xhat + move_argu_.target_x - 0.56) +
+             leg_argu_[1].LQR_K[3] *
+                 (-move_argu_.x_dot_hat + move_argu_.target_dot_x) +
+             leg_argu_[1].LQR_K[4] * (-this->pit_ + 0.16) +
+             leg_argu_[1].LQR_K[5] * (-this->gyro_.x + 0.0f));
+        leg_argu_[1].Tp =
+            (leg_argu_[1].LQR_K[6] *
+                 (-0.8845 * leg_argu_[1].L0 + 0.40663 - leg_argu_[1].theta) +
+             leg_argu_[1].LQR_K[7] * (-leg_argu_[1].d_theta + 0.0f) +
+             leg_argu_[1].LQR_K[8] *
+                 (-move_argu_.xhat + move_argu_.target_x - 0.56) +
+             leg_argu_[1].LQR_K[9] *
+                 (-move_argu_.x_dot_hat + move_argu_.target_dot_x) +
+             leg_argu_[1].LQR_K[10] * (-this->pit_ + 0.16) +
+             leg_argu_[1].LQR_K[11] * (-this->gyro_.x + 0.0f));
+      } else {
+        leg_argu_[1].Tw = 0;
+        leg_argu_[1].Tp =
+            (leg_argu_[1].LQR_K[6] * (-leg_argu_[1].theta + 0.0f) +
+             leg_argu_[1].LQR_K[7] * (-leg_argu_[1].d_theta + 0.0f));
+      }
+
+      this->leg_argu_[0].Delat_L0 = fabs(0.35 * cmd_.z) - fabs(gyro_.z) / 100 +
+                                    this->param_.wheel_param.static_L0[0] +
+                                    +roll_pid_.Calculate(0, eulr_.rol, dt_);
+      this->leg_argu_[1].Delat_L0 = fabs(0.35 * cmd_.z) - fabs(gyro_.z) / 100 +
+                                    this->param_.wheel_param.static_L0[1] -
+                                    roll_pid_.Calculate(0, eulr_.rol, dt_);
+
+      leg_argu_[0].F0 =
+          leg_argu_[0].Tp * sinf(leg_argu_[0].theta) +
+          this->param_.wheel_param.static_F0[0] +
+          leglength_pid_.at(0)->Calculate(this->leg_argu_[0].Delat_L0,
+                                          this->leg_argu_[0].L0, this->dt_);
+      leg_argu_[1].F0 =
+          leg_argu_[1].Tp * sinf(leg_argu_[1].theta) +
+          this->param_.wheel_param.static_F0[1] +
+          leglength_pid_.at(1)->Calculate(this->leg_argu_[1].Delat_L0,
+                                          this->leg_argu_[1].L0, this->dt_);
+
+      this->leg_argu_[0].Delta_Tp =
+          leg_argu_[0].L0 * 10.0f *
+          tp_pid_.at(0)->Calculate(this->leg_argu_[1].theta,
+                                   this->leg_argu_[0].theta, this->dt_);
+      this->leg_argu_[1].Delta_Tp =
+          -leg_argu_[1].L0 * 10.0f *
+          tp_pid_.at(0)->Calculate(this->leg_argu_[1].theta,
+                                   this->leg_argu_[0].theta, this->dt_);
+
+      result3 = leg_[0]->VMCinserve(
+          -leg_argu_[0].phi1_, -leg_argu_[0].phi4_,
+          -leg_argu_[0].Tp - this->leg_argu_[0].Delta_Tp, leg_argu_[0].F0);
+      this->leg_argu_[0].T1 = std::get<0>(result3);
+      this->leg_argu_[0].T2 = std::get<1>(result3);
+
+      result4 = leg_[1]->VMCinserve(
+          -leg_argu_[1].phi1_, -leg_argu_[1].phi4_,
+          -leg_argu_[1].Tp - this->leg_argu_[1].Delta_Tp, leg_argu_[1].F0);
+      this->leg_argu_[1].T1 = -std::get<0>(result4);
+      this->leg_argu_[1].T2 = -std::get<1>(result4);
+
+      if (onground0_flag_ & onground1_flag_) {
+        move_argu_.yaw_force =
+            -this->yaw_pid_.Calculate(move_argu_.target_yaw, this->yaw_, dt_);
+      } else {
+        move_argu_.yaw_force = 0;
+      }
+      /*3508不带减速箱是Tw*3.2f*/
+      /*2006带减速是Tw/1.8f*/
+      /* 3508带15.7减速箱是Tw/4.9256 */
+
+      this->wheel_motor_out_[0] =
+          this->leg_argu_[0].Tw / 4.9256f - move_argu_.yaw_force;
+
+      this->wheel_motor_out_[1] =
+          this->leg_argu_[1].Tw / 4.9256f + move_argu_.yaw_force;
+
+      this->hip_motor_out_[0] = this->leg_argu_[0].T1;
+      this->hip_motor_out_[1] = this->leg_argu_[0].T2;
+      this->hip_motor_out_[2] = this->leg_argu_[1].T1;
+      this->hip_motor_out_[3] = this->leg_argu_[1].T2;
+
+      this->hip_motor_flag_ = 1;
+      break;
+
+    case Wheelleg::RESET:
+      if (fabs(pit_) > M_PI / 2 || fabs(leg_argu_[0].theta) > 1.57 ||
+          fabs(leg_argu_[1].theta) > 1.57) {
+        leg_argu_[0].target_theta = leg_argu_[0].theta + cmd_.y / 1000;
+
+        if (fabs(pit_) > M_PI / 2) {
+          if ((leg_argu_[0].theta - leg_argu_[1].theta) > 0.03) {
+            leg_argu_[1].target_theta = leg_argu_[1].theta + 0.001;
+          }
+          if ((leg_argu_[0].theta - leg_argu_[1].theta) < -0.03) {
+            leg_argu_[1].target_theta = leg_argu_[1].theta - 0.001;
+          }
+          leg_argu_[0].F0 = 10 * leglength_pid_.at(0)->Calculate(
+                                     0.65f, this->leg_argu_[0].L0, this->dt_);
+          leg_argu_[1].F0 = 10 * leglength_pid_.at(1)->Calculate(
+                                     0.65f, this->leg_argu_[1].L0, this->dt_);
+        }
+        if (fabs(leg_argu_[0].theta) < 1.57) {
+          leg_argu_[1].target_theta = leg_argu_[1].theta + cmd_.y / 1000;
+          leg_argu_[0].target_theta = leg_argu_[0].theta;
+        }
+
+        if (fabs(leg_argu_[1].theta) < 1.57) {
+          leg_argu_[0].target_theta = leg_argu_[0].theta + cmd_.y / 1000;
+          leg_argu_[1].target_theta = leg_argu_[1].theta;
+        }
+
+        if (leg_argu_[0].target_theta > M_PI) {
+          leg_argu_[0].target_theta -= 2 * M_PI;
+        }
+        if (leg_argu_[0].target_theta < -M_PI) {
+          leg_argu_[0].target_theta += 2 * M_PI;
+        }
+        if (leg_argu_[1].target_theta > M_PI) {
+          leg_argu_[1].target_theta -= 2 * M_PI;
+        }
+        if (leg_argu_[1].target_theta < -M_PI) {
+          leg_argu_[1].target_theta += 2 * M_PI;
+        }
+        leg_argu_[0].Tp =
+            500 * this->theta_pid_[0]->Calculate(leg_argu_[0].target_theta,
+                                                 leg_argu_[0].theta, dt_);
+        leg_argu_[1].Tp =
+            500 * this->theta_pid_[1]->Calculate(leg_argu_[1].target_theta,
+                                                 leg_argu_[1].theta, dt_);
+
+      } else {
+        leg_argu_[0].F0 = 3 * leglength_pid_.at(0)->Calculate(
+                                  0.10f, this->leg_argu_[0].L0, this->dt_);
+        leg_argu_[1].F0 = 3 * leglength_pid_.at(1)->Calculate(
+                                  0.10f, this->leg_argu_[1].L0, this->dt_);
+
+        if ((this->leg_argu_[0].L0 < 0.20) && (this->leg_argu_[1].L0 < 0.20)) {
+          leg_argu_[0].Tp = 5.5 * this->theta_pid_[0]->Calculate(
+                                      0.1, leg_argu_[0].theta, dt_);
+          leg_argu_[1].Tp = 5.5 * this->theta_pid_[1]->Calculate(
+                                      0.1, leg_argu_[1].theta, dt_);
+          clampf(&leg_argu_[0].Tp, -10, 10);
+          clampf(&leg_argu_[1].Tp, -10, 10);
+        } else {
+          leg_argu_[0].Tp = 0;
+          leg_argu_[1].Tp = 0;
+        }
+      }
+
+      result3 = leg_[0]->VMCinserve(-leg_argu_[0].phi1_, -leg_argu_[0].phi4_,
+                                    -leg_argu_[0].Tp, leg_argu_[0].F0);
+      this->leg_argu_[0].T1 = std::get<0>(result3);
+      this->leg_argu_[0].T2 = std::get<1>(result3);
+
+      result4 = leg_[1]->VMCinserve(-leg_argu_[1].phi1_, -leg_argu_[1].phi4_,
+                                    -leg_argu_[1].Tp, leg_argu_[1].F0);
+      this->leg_argu_[1].T1 = -std::get<0>(result4);
+      this->leg_argu_[1].T2 = -std::get<1>(result4);
+
+      this->hip_motor_out_[0] = this->leg_argu_[0].T1;
+      this->hip_motor_out_[1] = this->leg_argu_[0].T2;
+      this->hip_motor_out_[2] = this->leg_argu_[1].T1;
+      this->hip_motor_out_[3] = this->leg_argu_[1].T2;
+
+      this->hip_motor_flag_ = 1;
+      break;
+  }
+}
+
+void Wheelleg::Control() {
+  clampf(&wheel_motor_out_[0], -0.8f, 0.8f);
+  clampf(&wheel_motor_out_[1], -0.8f, 0.8f);
+  clampf(&hip_motor_out_[0], -20.0f, 20.0f);
+  clampf(&hip_motor_out_[1], -20.0f, 20.0f);
+  clampf(&hip_motor_out_[2], -20.0f, 20.0f);
+  clampf(&hip_motor_out_[3], -20.0f, 20.0f);
+
+  // if (fabs(wheel_motor_[0]->GetSpeed()) > 5000 ||
+  //     fabs(wheel_motor_[1]->GetSpeed()) > 5000) {
+  //   wheel_motor_out_[0] = 0;
+  //   wheel_motor_out_[1] = 0;
+
+  //   if (fabs(this->pit_) > 0.5f) {
+  //     this->hip_motor_flag_ = 0;
+  //   }
+  // }
+
+  switch (this->mode_) {
+    case Wheelleg::RELAX:
+
+      this->wheel_motor_[0]->Relax();
+      this->wheel_motor_[1]->Relax();
+      hip_motor_flag_ = 0;
+      hip_motor_[0]->SetMit(0.0f);
+      hip_motor_[1]->SetMit(0.0f);
+      hip_motor_[2]->SetMit(0.0f);
+      hip_motor_[3]->SetMit(0.0f);
+      break;
+
+    case Wheelleg::STAND:
+    case Wheelleg::ROTOR:
+      // this->wheel_motor_[0]->Relax();
+      // this->wheel_motor_[1]->Relax();
+      this->wheel_motor_[0]->Control(-wheel_motor_out_[0]);
+      this->wheel_motor_[1]->Control(wheel_motor_out_[1]);
+      hip_motor_[0]->SetMit(this->hip_motor_out_[0]);
+      hip_motor_[1]->SetMit(this->hip_motor_out_[1]);
+      hip_motor_[2]->SetMit(this->hip_motor_out_[2]);
+      hip_motor_[3]->SetMit(this->hip_motor_out_[3]);
+      // hip_motor_[0]->SetMit(0.0f);
+      // hip_motor_[1]->SetMit(0.0f);
+      // hip_motor_[2]->SetMit(0.0f);
+      // hip_motor_[3]->SetMit(0.0f);
+      break;
+
+    case Wheelleg::RESET:
+
+      this->wheel_motor_[0]->Relax();
+      this->wheel_motor_[1]->Relax();
+
+      hip_motor_[0]->SetMit(this->hip_motor_out_[0]);
+      hip_motor_[1]->SetMit(this->hip_motor_out_[1]);
+
+      hip_motor_[2]->SetMit(this->hip_motor_out_[2]);
+      hip_motor_[3]->SetMit(this->hip_motor_out_[3]);
+
+      break;
+  }
+}
+
+void Wheelleg::SetMode(Wheelleg::Mode mode) {
+  if (mode == this->mode_) {
+    return;
+  }
+
+  this->leg_[0]->Reset();
+  this->leg_[1]->Reset();
+  move_argu_.x = 0;
+  move_argu_.x_dot = 0;
+  move_argu_.last_x_dot = 0;
+  move_argu_.target_x = move_argu_.xhat;
+  move_argu_.target_yaw = this->eulr_.yaw;
+  move_argu_.target_dot_x = 0;
+  move_argu_.xhat = 0;
+  move_argu_.x_dot_hat = 0;
+  this->manfilter_.reset_comp();
+  this->mode_ = mode;
+}

utils/Simulation-master/balance/series_legs/get_k_length.m

@@ -48,8 +48,8 @@ function K = get_k_length(leg_length)
     B=subs(B,[R,L,LM,l,mw,mp,M,Iw,Ip,IM,g],[R1,L1,LM1,l1,mw1,mp1,M1,Iw1,Ip1,IM1,9.8]);
     B=double(B);
     
-    Q=diag([700 1 5000 100 3000 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
-    R=[240 0;0 25];                %T Tp
+    Q=diag([700 100 2000 1500 5000 1]);%theta d_theta x d_x phi d_phi%700 1 600 200 1000 1
+    R=[140 0;0 50];                %T Tp
     
     K=lqr(A,B,Q,R);