运动学正确

.vscode/settings.json

@@ -0,0 +1,9 @@
+{
+    "files.associations": {
+        "device.h": "c",
+        "ahrs.h": "c",
+        "string.h": "c",
+        "user_math.h": "c",
+        "__locale": "c"
+    }
+}

Core/Inc/stm32h7xx_it.h

@@ -71,6 +71,9 @@ void EXTI15_10_IRQHandler(void);
 void DMA1_Stream7_IRQHandler(void);
 void UART5_IRQHandler(void);
 void DMA2_Stream0_IRQHandler(void);
+void DMA2_Stream1_IRQHandler(void);
+void DMA2_Stream2_IRQHandler(void);
+void DMA2_Stream3_IRQHandler(void);
 void OTG_HS_IRQHandler(void);
 void UART7_IRQHandler(void);
 void USART10_IRQHandler(void);

Core/Src/dma.c

@@ -71,6 +71,15 @@ void MX_DMA_Init(void)
   /* DMA2_Stream0_IRQn interrupt configuration */
   HAL_NVIC_SetPriority(DMA2_Stream0_IRQn, 5, 0);
   HAL_NVIC_EnableIRQ(DMA2_Stream0_IRQn);
+  /* DMA2_Stream1_IRQn interrupt configuration */
+  HAL_NVIC_SetPriority(DMA2_Stream1_IRQn, 5, 0);
+  HAL_NVIC_EnableIRQ(DMA2_Stream1_IRQn);
+  /* DMA2_Stream2_IRQn interrupt configuration */
+  HAL_NVIC_SetPriority(DMA2_Stream2_IRQn, 5, 0);
+  HAL_NVIC_EnableIRQ(DMA2_Stream2_IRQn);
+  /* DMA2_Stream3_IRQn interrupt configuration */
+  HAL_NVIC_SetPriority(DMA2_Stream3_IRQn, 5, 0);
+  HAL_NVIC_EnableIRQ(DMA2_Stream3_IRQn);
 
 }
 

Core/Src/spi.c

@@ -89,7 +89,7 @@ void MX_SPI2_Init(void)
   hspi2.Init.CLKPolarity = SPI_POLARITY_HIGH;
   hspi2.Init.CLKPhase = SPI_PHASE_2EDGE;
   hspi2.Init.NSS = SPI_NSS_SOFT;
-  hspi2.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_32;
+  hspi2.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_8;
   hspi2.Init.FirstBit = SPI_FIRSTBIT_MSB;
   hspi2.Init.TIMode = SPI_TIMODE_DISABLE;
   hspi2.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
@@ -210,7 +210,7 @@ void HAL_SPI_MspInit(SPI_HandleTypeDef* spiHandle)
     hdma_spi2_rx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
     hdma_spi2_rx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
     hdma_spi2_rx.Init.Mode = DMA_NORMAL;
-    hdma_spi2_rx.Init.Priority = DMA_PRIORITY_LOW;
+    hdma_spi2_rx.Init.Priority = DMA_PRIORITY_VERY_HIGH;
     hdma_spi2_rx.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
     if (HAL_DMA_Init(&hdma_spi2_rx) != HAL_OK)
     {
@@ -228,7 +228,7 @@ void HAL_SPI_MspInit(SPI_HandleTypeDef* spiHandle)
     hdma_spi2_tx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
     hdma_spi2_tx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
     hdma_spi2_tx.Init.Mode = DMA_NORMAL;
-    hdma_spi2_tx.Init.Priority = DMA_PRIORITY_LOW;
+    hdma_spi2_tx.Init.Priority = DMA_PRIORITY_VERY_HIGH;
     hdma_spi2_tx.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
     if (HAL_DMA_Init(&hdma_spi2_tx) != HAL_OK)
     {

Core/Src/stm32h7xx_it.c

@@ -65,11 +65,14 @@ extern DMA_HandleTypeDef hdma_spi2_rx;
 extern DMA_HandleTypeDef hdma_spi2_tx;
 extern SPI_HandleTypeDef hspi2;
 extern DMA_HandleTypeDef hdma_uart5_rx;
+extern DMA_HandleTypeDef hdma_uart7_rx;
+extern DMA_HandleTypeDef hdma_uart7_tx;
 extern DMA_HandleTypeDef hdma_usart2_rx;
 extern DMA_HandleTypeDef hdma_usart2_tx;
 extern DMA_HandleTypeDef hdma_usart3_rx;
 extern DMA_HandleTypeDef hdma_usart3_tx;
 extern DMA_HandleTypeDef hdma_usart10_tx;
+extern DMA_HandleTypeDef hdma_usart10_rx;
 extern UART_HandleTypeDef huart5;
 extern UART_HandleTypeDef huart7;
 extern UART_HandleTypeDef huart1;
@@ -358,7 +361,7 @@ void USART1_IRQHandler(void)
   /* USER CODE END USART1_IRQn 0 */
   HAL_UART_IRQHandler(&huart1);
   /* USER CODE BEGIN USART1_IRQn 1 */
-
+  BSP_UART_IRQHandler(&huart1);
   /* USER CODE END USART1_IRQn 1 */
 }
 
@@ -430,7 +433,7 @@ void UART5_IRQHandler(void)
   /* USER CODE END UART5_IRQn 0 */
   HAL_UART_IRQHandler(&huart5);
   /* USER CODE BEGIN UART5_IRQn 1 */
-
+  BSP_UART_IRQHandler(&huart5);
   /* USER CODE END UART5_IRQn 1 */
 }
 
@@ -448,6 +451,48 @@ void DMA2_Stream0_IRQHandler(void)
   /* USER CODE END DMA2_Stream0_IRQn 1 */
 }
 
+/**
+  * @brief This function handles DMA2 stream1 global interrupt.
+  */
+void DMA2_Stream1_IRQHandler(void)
+{
+  /* USER CODE BEGIN DMA2_Stream1_IRQn 0 */
+
+  /* USER CODE END DMA2_Stream1_IRQn 0 */
+  HAL_DMA_IRQHandler(&hdma_uart7_rx);
+  /* USER CODE BEGIN DMA2_Stream1_IRQn 1 */
+
+  /* USER CODE END DMA2_Stream1_IRQn 1 */
+}
+
+/**
+  * @brief This function handles DMA2 stream2 global interrupt.
+  */
+void DMA2_Stream2_IRQHandler(void)
+{
+  /* USER CODE BEGIN DMA2_Stream2_IRQn 0 */
+
+  /* USER CODE END DMA2_Stream2_IRQn 0 */
+  HAL_DMA_IRQHandler(&hdma_uart7_tx);
+  /* USER CODE BEGIN DMA2_Stream2_IRQn 1 */
+
+  /* USER CODE END DMA2_Stream2_IRQn 1 */
+}
+
+/**
+  * @brief This function handles DMA2 stream3 global interrupt.
+  */
+void DMA2_Stream3_IRQHandler(void)
+{
+  /* USER CODE BEGIN DMA2_Stream3_IRQn 0 */
+
+  /* USER CODE END DMA2_Stream3_IRQn 0 */
+  HAL_DMA_IRQHandler(&hdma_usart10_rx);
+  /* USER CODE BEGIN DMA2_Stream3_IRQn 1 */
+
+  /* USER CODE END DMA2_Stream3_IRQn 1 */
+}
+
 /**
   * @brief This function handles USB On The Go HS global interrupt.
   */
@@ -472,7 +517,7 @@ void UART7_IRQHandler(void)
   /* USER CODE END UART7_IRQn 0 */
   HAL_UART_IRQHandler(&huart7);
   /* USER CODE BEGIN UART7_IRQn 1 */
-
+  BSP_UART_IRQHandler(&huart7);
   /* USER CODE END UART7_IRQn 1 */
 }
 
@@ -486,7 +531,7 @@ void USART10_IRQHandler(void)
   /* USER CODE END USART10_IRQn 0 */
   HAL_UART_IRQHandler(&huart10);
   /* USER CODE BEGIN USART10_IRQn 1 */
-
+  BSP_UART_IRQHandler(&huart10);
   /* USER CODE END USART10_IRQn 1 */
 }
 

Core/Src/usart.c

@@ -31,11 +31,14 @@ UART_HandleTypeDef huart2;
 UART_HandleTypeDef huart3;
 UART_HandleTypeDef huart10;
 DMA_HandleTypeDef hdma_uart5_rx;
+DMA_HandleTypeDef hdma_uart7_rx;
+DMA_HandleTypeDef hdma_uart7_tx;
 DMA_HandleTypeDef hdma_usart2_rx;
 DMA_HandleTypeDef hdma_usart2_tx;
 DMA_HandleTypeDef hdma_usart3_rx;
 DMA_HandleTypeDef hdma_usart3_tx;
 DMA_HandleTypeDef hdma_usart10_tx;
+DMA_HandleTypeDef hdma_usart10_rx;
 
 /* UART5 init function */
 void MX_UART5_Init(void)
@@ -268,7 +271,7 @@ void MX_USART10_UART_Init(void)
 
   /* USER CODE END USART10_Init 1 */
   huart10.Instance = USART10;
-  huart10.Init.BaudRate = 115200;
+  huart10.Init.BaudRate = 921600;
   huart10.Init.WordLength = UART_WORDLENGTH_8B;
   huart10.Init.StopBits = UART_STOPBITS_1;
   huart10.Init.Parity = UART_PARITY_NONE;
@@ -399,6 +402,43 @@ void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle)
     GPIO_InitStruct.Alternate = GPIO_AF7_UART7;
     HAL_GPIO_Init(GPIOE, &GPIO_InitStruct);
 
+    /* UART7 DMA Init */
+    /* UART7_RX Init */
+    hdma_uart7_rx.Instance = DMA2_Stream1;
+    hdma_uart7_rx.Init.Request = DMA_REQUEST_UART7_RX;
+    hdma_uart7_rx.Init.Direction = DMA_PERIPH_TO_MEMORY;
+    hdma_uart7_rx.Init.PeriphInc = DMA_PINC_DISABLE;
+    hdma_uart7_rx.Init.MemInc = DMA_MINC_ENABLE;
+    hdma_uart7_rx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
+    hdma_uart7_rx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
+    hdma_uart7_rx.Init.Mode = DMA_NORMAL;
+    hdma_uart7_rx.Init.Priority = DMA_PRIORITY_MEDIUM;
+    hdma_uart7_rx.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
+    if (HAL_DMA_Init(&hdma_uart7_rx) != HAL_OK)
+    {
+      Error_Handler();
+    }
+
+    __HAL_LINKDMA(uartHandle,hdmarx,hdma_uart7_rx);
+
+    /* UART7_TX Init */
+    hdma_uart7_tx.Instance = DMA2_Stream2;
+    hdma_uart7_tx.Init.Request = DMA_REQUEST_UART7_TX;
+    hdma_uart7_tx.Init.Direction = DMA_MEMORY_TO_PERIPH;
+    hdma_uart7_tx.Init.PeriphInc = DMA_PINC_DISABLE;
+    hdma_uart7_tx.Init.MemInc = DMA_MINC_ENABLE;
+    hdma_uart7_tx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
+    hdma_uart7_tx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
+    hdma_uart7_tx.Init.Mode = DMA_NORMAL;
+    hdma_uart7_tx.Init.Priority = DMA_PRIORITY_MEDIUM;
+    hdma_uart7_tx.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
+    if (HAL_DMA_Init(&hdma_uart7_tx) != HAL_OK)
+    {
+      Error_Handler();
+    }
+
+    __HAL_LINKDMA(uartHandle,hdmatx,hdma_uart7_tx);
+
     /* UART7 interrupt Init */
     HAL_NVIC_SetPriority(UART7_IRQn, 5, 0);
     HAL_NVIC_EnableIRQ(UART7_IRQn);
@@ -648,7 +688,7 @@ void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle)
     hdma_usart10_tx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
     hdma_usart10_tx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
     hdma_usart10_tx.Init.Mode = DMA_NORMAL;
-    hdma_usart10_tx.Init.Priority = DMA_PRIORITY_LOW;
+    hdma_usart10_tx.Init.Priority = DMA_PRIORITY_VERY_HIGH;
     hdma_usart10_tx.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
     if (HAL_DMA_Init(&hdma_usart10_tx) != HAL_OK)
     {
@@ -657,6 +697,24 @@ void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle)
 
     __HAL_LINKDMA(uartHandle,hdmatx,hdma_usart10_tx);
 
+    /* USART10_RX Init */
+    hdma_usart10_rx.Instance = DMA2_Stream3;
+    hdma_usart10_rx.Init.Request = DMA_REQUEST_USART10_RX;
+    hdma_usart10_rx.Init.Direction = DMA_PERIPH_TO_MEMORY;
+    hdma_usart10_rx.Init.PeriphInc = DMA_PINC_DISABLE;
+    hdma_usart10_rx.Init.MemInc = DMA_MINC_ENABLE;
+    hdma_usart10_rx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
+    hdma_usart10_rx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
+    hdma_usart10_rx.Init.Mode = DMA_NORMAL;
+    hdma_usart10_rx.Init.Priority = DMA_PRIORITY_VERY_HIGH;
+    hdma_usart10_rx.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
+    if (HAL_DMA_Init(&hdma_usart10_rx) != HAL_OK)
+    {
+      Error_Handler();
+    }
+
+    __HAL_LINKDMA(uartHandle,hdmarx,hdma_usart10_rx);
+
     /* USART10 interrupt Init */
     HAL_NVIC_SetPriority(USART10_IRQn, 5, 0);
     HAL_NVIC_EnableIRQ(USART10_IRQn);
@@ -708,6 +766,10 @@ void HAL_UART_MspDeInit(UART_HandleTypeDef* uartHandle)
     */
     HAL_GPIO_DeInit(GPIOE, GPIO_PIN_7|GPIO_PIN_8);
 
+    /* UART7 DMA DeInit */
+    HAL_DMA_DeInit(uartHandle->hdmarx);
+    HAL_DMA_DeInit(uartHandle->hdmatx);
+
     /* UART7 interrupt Deinit */
     HAL_NVIC_DisableIRQ(UART7_IRQn);
   /* USER CODE BEGIN UART7_MspDeInit 1 */
@@ -802,6 +864,7 @@ void HAL_UART_MspDeInit(UART_HandleTypeDef* uartHandle)
 
     /* USART10 DMA DeInit */
     HAL_DMA_DeInit(uartHandle->hdmatx);
+    HAL_DMA_DeInit(uartHandle->hdmarx);
 
     /* USART10 interrupt Deinit */
     HAL_NVIC_DisableIRQ(USART10_IRQn);

CtrBoard-H7_ALL.ioc

@@ -44,6 +44,8 @@ Dma.ADC1.0.SyncRequestNumber=1
 Dma.ADC1.0.SyncSignalID=NONE
 Dma.Request0=ADC1
 Dma.Request1=UART5_RX
+Dma.Request10=UART7_TX
+Dma.Request11=USART10_RX
 Dma.Request2=SPI2_RX
 Dma.Request3=SPI2_TX
 Dma.Request4=USART3_RX
@@ -51,7 +53,8 @@ Dma.Request5=USART3_TX
 Dma.Request6=USART2_RX
 Dma.Request7=USART2_TX
 Dma.Request8=USART10_TX
-Dma.RequestsNb=9
+Dma.Request9=UART7_RX
+Dma.RequestsNb=12
 Dma.SPI2_RX.2.Direction=DMA_PERIPH_TO_MEMORY
 Dma.SPI2_RX.2.EventEnable=DISABLE
 Dma.SPI2_RX.2.FIFOMode=DMA_FIFOMODE_DISABLE
@@ -62,7 +65,7 @@ Dma.SPI2_RX.2.Mode=DMA_NORMAL
 Dma.SPI2_RX.2.PeriphDataAlignment=DMA_PDATAALIGN_BYTE
 Dma.SPI2_RX.2.PeriphInc=DMA_PINC_DISABLE
 Dma.SPI2_RX.2.Polarity=HAL_DMAMUX_REQ_GEN_RISING
-Dma.SPI2_RX.2.Priority=DMA_PRIORITY_LOW
+Dma.SPI2_RX.2.Priority=DMA_PRIORITY_VERY_HIGH
 Dma.SPI2_RX.2.RequestNumber=1
 Dma.SPI2_RX.2.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode,SignalID,Polarity,RequestNumber,SyncSignalID,SyncPolarity,SyncEnable,EventEnable,SyncRequestNumber
 Dma.SPI2_RX.2.SignalID=NONE
@@ -80,7 +83,7 @@ Dma.SPI2_TX.3.Mode=DMA_NORMAL
 Dma.SPI2_TX.3.PeriphDataAlignment=DMA_PDATAALIGN_BYTE
 Dma.SPI2_TX.3.PeriphInc=DMA_PINC_DISABLE
 Dma.SPI2_TX.3.Polarity=HAL_DMAMUX_REQ_GEN_RISING
-Dma.SPI2_TX.3.Priority=DMA_PRIORITY_LOW
+Dma.SPI2_TX.3.Priority=DMA_PRIORITY_VERY_HIGH
 Dma.SPI2_TX.3.RequestNumber=1
 Dma.SPI2_TX.3.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode,SignalID,Polarity,RequestNumber,SyncSignalID,SyncPolarity,SyncEnable,EventEnable,SyncRequestNumber
 Dma.SPI2_TX.3.SignalID=NONE
@@ -106,6 +109,60 @@ Dma.UART5_RX.1.SyncEnable=DISABLE
 Dma.UART5_RX.1.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT
 Dma.UART5_RX.1.SyncRequestNumber=1
 Dma.UART5_RX.1.SyncSignalID=NONE
+Dma.UART7_RX.9.Direction=DMA_PERIPH_TO_MEMORY
+Dma.UART7_RX.9.EventEnable=DISABLE
+Dma.UART7_RX.9.FIFOMode=DMA_FIFOMODE_DISABLE
+Dma.UART7_RX.9.Instance=DMA2_Stream1
+Dma.UART7_RX.9.MemDataAlignment=DMA_MDATAALIGN_BYTE
+Dma.UART7_RX.9.MemInc=DMA_MINC_ENABLE
+Dma.UART7_RX.9.Mode=DMA_NORMAL
+Dma.UART7_RX.9.PeriphDataAlignment=DMA_PDATAALIGN_BYTE
+Dma.UART7_RX.9.PeriphInc=DMA_PINC_DISABLE
+Dma.UART7_RX.9.Polarity=HAL_DMAMUX_REQ_GEN_RISING
+Dma.UART7_RX.9.Priority=DMA_PRIORITY_MEDIUM
+Dma.UART7_RX.9.RequestNumber=1
+Dma.UART7_RX.9.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode,SignalID,Polarity,RequestNumber,SyncSignalID,SyncPolarity,SyncEnable,EventEnable,SyncRequestNumber
+Dma.UART7_RX.9.SignalID=NONE
+Dma.UART7_RX.9.SyncEnable=DISABLE
+Dma.UART7_RX.9.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT
+Dma.UART7_RX.9.SyncRequestNumber=1
+Dma.UART7_RX.9.SyncSignalID=NONE
+Dma.UART7_TX.10.Direction=DMA_MEMORY_TO_PERIPH
+Dma.UART7_TX.10.EventEnable=DISABLE
+Dma.UART7_TX.10.FIFOMode=DMA_FIFOMODE_DISABLE
+Dma.UART7_TX.10.Instance=DMA2_Stream2
+Dma.UART7_TX.10.MemDataAlignment=DMA_MDATAALIGN_BYTE
+Dma.UART7_TX.10.MemInc=DMA_MINC_ENABLE
+Dma.UART7_TX.10.Mode=DMA_NORMAL
+Dma.UART7_TX.10.PeriphDataAlignment=DMA_PDATAALIGN_BYTE
+Dma.UART7_TX.10.PeriphInc=DMA_PINC_DISABLE
+Dma.UART7_TX.10.Polarity=HAL_DMAMUX_REQ_GEN_RISING
+Dma.UART7_TX.10.Priority=DMA_PRIORITY_MEDIUM
+Dma.UART7_TX.10.RequestNumber=1
+Dma.UART7_TX.10.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode,SignalID,Polarity,RequestNumber,SyncSignalID,SyncPolarity,SyncEnable,EventEnable,SyncRequestNumber
+Dma.UART7_TX.10.SignalID=NONE
+Dma.UART7_TX.10.SyncEnable=DISABLE
+Dma.UART7_TX.10.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT
+Dma.UART7_TX.10.SyncRequestNumber=1
+Dma.UART7_TX.10.SyncSignalID=NONE
+Dma.USART10_RX.11.Direction=DMA_PERIPH_TO_MEMORY
+Dma.USART10_RX.11.EventEnable=DISABLE
+Dma.USART10_RX.11.FIFOMode=DMA_FIFOMODE_DISABLE
+Dma.USART10_RX.11.Instance=DMA2_Stream3
+Dma.USART10_RX.11.MemDataAlignment=DMA_MDATAALIGN_BYTE
+Dma.USART10_RX.11.MemInc=DMA_MINC_ENABLE
+Dma.USART10_RX.11.Mode=DMA_NORMAL
+Dma.USART10_RX.11.PeriphDataAlignment=DMA_PDATAALIGN_BYTE
+Dma.USART10_RX.11.PeriphInc=DMA_PINC_DISABLE
+Dma.USART10_RX.11.Polarity=HAL_DMAMUX_REQ_GEN_RISING
+Dma.USART10_RX.11.Priority=DMA_PRIORITY_VERY_HIGH
+Dma.USART10_RX.11.RequestNumber=1
+Dma.USART10_RX.11.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode,SignalID,Polarity,RequestNumber,SyncSignalID,SyncPolarity,SyncEnable,EventEnable,SyncRequestNumber
+Dma.USART10_RX.11.SignalID=NONE
+Dma.USART10_RX.11.SyncEnable=DISABLE
+Dma.USART10_RX.11.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT
+Dma.USART10_RX.11.SyncRequestNumber=1
+Dma.USART10_RX.11.SyncSignalID=NONE
 Dma.USART10_TX.8.Direction=DMA_MEMORY_TO_PERIPH
 Dma.USART10_TX.8.EventEnable=DISABLE
 Dma.USART10_TX.8.FIFOMode=DMA_FIFOMODE_DISABLE
@@ -116,7 +173,7 @@ Dma.USART10_TX.8.Mode=DMA_NORMAL
 Dma.USART10_TX.8.PeriphDataAlignment=DMA_PDATAALIGN_BYTE
 Dma.USART10_TX.8.PeriphInc=DMA_PINC_DISABLE
 Dma.USART10_TX.8.Polarity=HAL_DMAMUX_REQ_GEN_RISING
-Dma.USART10_TX.8.Priority=DMA_PRIORITY_LOW
+Dma.USART10_TX.8.Priority=DMA_PRIORITY_VERY_HIGH
 Dma.USART10_TX.8.RequestNumber=1
 Dma.USART10_TX.8.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode,SignalID,Polarity,RequestNumber,SyncSignalID,SyncPolarity,SyncEnable,EventEnable,SyncRequestNumber
 Dma.USART10_TX.8.SignalID=NONE
@@ -395,6 +452,9 @@ NVIC.DMA1_Stream5_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
 NVIC.DMA1_Stream6_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
 NVIC.DMA1_Stream7_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
 NVIC.DMA2_Stream0_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
+NVIC.DMA2_Stream1_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
+NVIC.DMA2_Stream2_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
+NVIC.DMA2_Stream3_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
 NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
 NVIC.EXTI15_10_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
 NVIC.FDCAN1_IT0_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
@@ -777,10 +837,10 @@ SPI1.Direction=SPI_DIRECTION_2LINES_TXONLY
 SPI1.IPParameters=VirtualType,Mode,Direction,CalculateBaudRate,DataSize,BaudRatePrescaler,CLKPolarity
 SPI1.Mode=SPI_MODE_MASTER
 SPI1.VirtualType=VM_MASTER
-SPI2.BaudRatePrescaler=SPI_BAUDRATEPRESCALER_32
+SPI2.BaudRatePrescaler=SPI_BAUDRATEPRESCALER_8
 SPI2.CLKPhase=SPI_PHASE_2EDGE
 SPI2.CLKPolarity=SPI_POLARITY_HIGH
-SPI2.CalculateBaudRate=3.75 MBits/s
+SPI2.CalculateBaudRate=15.0 MBits/s
 SPI2.DataSize=SPI_DATASIZE_8BIT
 SPI2.Direction=SPI_DIRECTION_2LINES
 SPI2.IPParameters=VirtualType,Mode,Direction,CalculateBaudRate,DataSize,BaudRatePrescaler,CLKPolarity,CLKPhase
@@ -820,7 +880,7 @@ UART7.IPParameters=BaudRate
 USART1.BaudRate=921600
 USART1.IPParameters=VirtualMode-Asynchronous,BaudRate
 USART1.VirtualMode-Asynchronous=VM_ASYNC
-USART10.BaudRate=115200
+USART10.BaudRate=921600
 USART10.IPParameters=VirtualMode,BaudRate
 USART10.VirtualMode=VM_ASYNC
 USART2.BaudRate=4000000

MDK-ARM/CtrBoard-H7_ALL.uvoptx

@@ -1,4 +1,4 @@
-<?xml version="1.0" encoding="UTF-8"?>
+<?xml version="1.0" encoding="UTF-8" standalone="no" ?>
 <ProjectOpt xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="project_optx.xsd">
 
   <SchemaVersion>1.0</SchemaVersion>
@@ -45,7 +45,7 @@
         <PageWidth>79</PageWidth>
         <PageLength>66</PageLength>
         <TabStop>8</TabStop>
-        <ListingPath />
+        <ListingPath></ListingPath>
       </OPTLEX>
       <ListingPage>
         <CreateCListing>1</CreateCListing>
@@ -104,16 +104,16 @@
         <bSchkAxf>0</bSchkAxf>
         <bTchkAxf>0</bTchkAxf>
         <nTsel>3</nTsel>
-        <sDll />
+        <sDll></sDll>
-        <sDllPa />
+        <sDllPa></sDllPa>
-        <sDlgDll />
+        <sDlgDll></sDlgDll>
-        <sDlgPa />
+        <sDlgPa></sDlgPa>
-        <sIfile />
+        <sIfile></sIfile>
-        <tDll />
+        <tDll></tDll>
-        <tDllPa />
+        <tDllPa></tDllPa>
-        <tDlgDll />
+        <tDlgDll></tDlgDll>
-        <tDlgPa />
+        <tDlgPa></tDlgPa>
-        <tIfile />
+        <tIfile></tIfile>
         <pMon>BIN\CMSIS_AGDI.dll</pMon>
       </DebugOpt>
       <TargetDriverDllRegistry>
@@ -130,17 +130,17 @@
         <SetRegEntry>
           <Number>0</Number>
           <Key>ARMDBGFLAGS</Key>
-          <Name />
+          <Name></Name>
         </SetRegEntry>
         <SetRegEntry>
           <Number>0</Number>
           <Key>CMSIS_AGDI</Key>
-          <Name>-X"" -O206 -S8 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(6BA02477) -L00(0) -TO65554 -TC10000000 -TT10000000 -TP20 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO7 -FD20000000 -FC8000 -FN1 -FF0STM32H72x-73x_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32H723VGTx$CMSIS\Flash\STM32H72x-73x_1024.FLM)</Name>
+          <Name>-X"Any" -UAny -O206 -S8 -C0 -P00000000 -N00("ARM CoreSight SW-DP") -D00(6BA02477) -L00(0) -TO65554 -TC10000000 -TT10000000 -TP20 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO15 -FD20000000 -FC8000 -FN1 -FF0STM32H72x-73x_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32H723VGTx$CMSIS\Flash\STM32H72x-73x_1024.FLM)</Name>
         </SetRegEntry>
         <SetRegEntry>
           <Number>0</Number>
           <Key>DLGUARM</Key>
-          <Name />
+          <Name></Name>
         </SetRegEntry>
         <SetRegEntry>
           <Number>0</Number>
@@ -153,7 +153,7 @@
           <Name>-U-O142 -O2254 -S0 -C0 -N00("ARM CoreSight SW-DP") -D00(5BA02477) -L00(0) -TO18 -TC10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO7 -FD20000000 -FC1000 -FN1 -FF0STM32H72x-73x_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32H723VGTx$CMSIS\Flash\STM32H72x-73x_1024.FLM)</Name>
         </SetRegEntry>
       </TargetDriverDllRegistry>
-      <Breakpoint />
+      <Breakpoint/>
       <WatchWindow1>
         <Ww>
           <count>0</count>
@@ -163,47 +163,42 @@
         <Ww>
           <count>1</count>
           <WinNumber>1</WinNumber>
-          <ItemText>bmi088</ItemText>
+          <ItemText>chassis</ItemText>
         </Ww>
         <Ww>
           <count>2</count>
           <WinNumber>1</WinNumber>
-          <ItemText>eulr_to_send</ItemText>
+          <ItemText>n100</ItemText>
         </Ww>
         <Ww>
           <count>3</count>
           <WinNumber>1</WinNumber>
-          <ItemText>cmd_rc</ItemText>
+          <ItemText>go_feedback</ItemText>
         </Ww>
         <Ww>
           <count>4</count>
           <WinNumber>1</WinNumber>
-          <ItemText>go</ItemText>
+          <ItemText>param_default</ItemText>
         </Ww>
         <Ww>
           <count>5</count>
           <WinNumber>1</WinNumber>
-          <ItemText>task_runtime.freq.ctrl_leg</ItemText>
+          <ItemText>angle_pose</ItemText>
         </Ww>
         <Ww>
           <count>6</count>
           <WinNumber>1</WinNumber>
-          <ItemText>go_feedback</ItemText>
+          <ItemText>chassis_cmd</ItemText>
         </Ww>
         <Ww>
           <count>7</count>
           <WinNumber>1</WinNumber>
-          <ItemText>cmd.chassis</ItemText>
+          <ItemText>target_pose</ItemText>
         </Ww>
         <Ww>
           <count>8</count>
           <WinNumber>1</WinNumber>
-          <ItemText>chassis</ItemText>
+          <ItemText>c-&gt;param-&gt;mech_param.sign.leg[i]</ItemText>
-        </Ww>
-        <Ww>
-          <count>9</count>
-          <WinNumber>1</WinNumber>
-          <ItemText>go_feedback</ItemText>
         </Ww>
       </WatchWindow1>
       <Tracepoint>
@@ -235,19 +230,19 @@
         <newCpu>0</newCpu>
         <uProt>0</uProt>
       </DebugFlag>
-      <LintExecutable />
+      <LintExecutable></LintExecutable>
-      <LintConfigFile />
+      <LintConfigFile></LintConfigFile>
       <bLintAuto>0</bLintAuto>
       <bAutoGenD>0</bAutoGenD>
       <LntExFlags>0</LntExFlags>
-      <pMisraName />
+      <pMisraName></pMisraName>
-      <pszMrule />
+      <pszMrule></pszMrule>
-      <pSingCmds />
+      <pSingCmds></pSingCmds>
-      <pMultCmds />
+      <pMultCmds></pMultCmds>
-      <pMisraNamep />
+      <pMisraNamep></pMisraNamep>
-      <pszMrulep />
+      <pszMrulep></pszMrulep>
-      <pSingCmdsp />
+      <pSingCmdsp></pSingCmdsp>
-      <pMultCmdsp />
+      <pMultCmdsp></pMultCmdsp>
       <DebugDescription>
         <Enable>1</Enable>
         <EnableFlashSeq>1</EnableFlashSeq>
@@ -1267,8 +1262,32 @@
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\User\component\kinematics.c</PathWithFileName>
+      <PathWithFileName>..\User\component\crc8.c</PathWithFileName>
-      <FilenameWithoutPath>kinematics.c</FilenameWithoutPath>
+      <FilenameWithoutPath>crc8.c</FilenameWithoutPath>
+      <RteFlg>0</RteFlg>
+      <bShared>0</bShared>
+    </File>
+    <File>
+      <GroupNumber>10</GroupNumber>
+      <FileNumber>79</FileNumber>
+      <FileType>1</FileType>
+      <tvExp>0</tvExp>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
+      <PathWithFileName>..\User\component\crc16.c</PathWithFileName>
+      <FilenameWithoutPath>crc16.c</FilenameWithoutPath>
+      <RteFlg>0</RteFlg>
+      <bShared>0</bShared>
+    </File>
+    <File>
+      <GroupNumber>10</GroupNumber>
+      <FileNumber>80</FileNumber>
+      <FileType>1</FileType>
+      <tvExp>0</tvExp>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
+      <PathWithFileName>..\User\component\limiter.c</PathWithFileName>
+      <FilenameWithoutPath>limiter.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
@@ -1282,7 +1301,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>11</GroupNumber>
-      <FileNumber>79</FileNumber>
+      <FileNumber>81</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1294,7 +1313,7 @@
     </File>
     <File>
       <GroupNumber>11</GroupNumber>
-      <FileNumber>80</FileNumber>
+      <FileNumber>82</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1306,7 +1325,7 @@
     </File>
     <File>
       <GroupNumber>11</GroupNumber>
-      <FileNumber>81</FileNumber>
+      <FileNumber>83</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1316,6 +1335,18 @@
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
+    <File>
+      <GroupNumber>11</GroupNumber>
+      <FileNumber>84</FileNumber>
+      <FileType>1</FileType>
+      <tvExp>0</tvExp>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
+      <PathWithFileName>..\User\device\n100.c</PathWithFileName>
+      <FilenameWithoutPath>n100.c</FilenameWithoutPath>
+      <RteFlg>0</RteFlg>
+      <bShared>0</bShared>
+    </File>
   </Group>
 
   <Group>
@@ -1326,7 +1357,19 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>12</GroupNumber>
-      <FileNumber>82</FileNumber>
+      <FileNumber>85</FileNumber>
+      <FileType>1</FileType>
+      <tvExp>0</tvExp>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
+      <PathWithFileName>..\User\component\kinematics.c</PathWithFileName>
+      <FilenameWithoutPath>kinematics.c</FilenameWithoutPath>
+      <RteFlg>0</RteFlg>
+      <bShared>0</bShared>
+    </File>
+    <File>
+      <GroupNumber>12</GroupNumber>
+      <FileNumber>86</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1338,7 +1381,7 @@
     </File>
     <File>
       <GroupNumber>12</GroupNumber>
-      <FileNumber>83</FileNumber>
+      <FileNumber>87</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1358,7 +1401,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>13</GroupNumber>
-      <FileNumber>84</FileNumber>
+      <FileNumber>88</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1370,7 +1413,7 @@
     </File>
     <File>
       <GroupNumber>13</GroupNumber>
-      <FileNumber>85</FileNumber>
+      <FileNumber>89</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1382,7 +1425,7 @@
     </File>
     <File>
       <GroupNumber>13</GroupNumber>
-      <FileNumber>86</FileNumber>
+      <FileNumber>90</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1394,7 +1437,7 @@
     </File>
     <File>
       <GroupNumber>13</GroupNumber>
-      <FileNumber>87</FileNumber>
+      <FileNumber>91</FileNumber>
       <FileType>5</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1406,7 +1449,7 @@
     </File>
     <File>
       <GroupNumber>13</GroupNumber>
-      <FileNumber>88</FileNumber>
+      <FileNumber>92</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1418,7 +1461,7 @@
     </File>
     <File>
       <GroupNumber>13</GroupNumber>
-      <FileNumber>89</FileNumber>
+      <FileNumber>93</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1430,7 +1473,7 @@
     </File>
     <File>
       <GroupNumber>13</GroupNumber>
-      <FileNumber>90</FileNumber>
+      <FileNumber>94</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1442,7 +1485,7 @@
     </File>
     <File>
       <GroupNumber>13</GroupNumber>
-      <FileNumber>91</FileNumber>
+      <FileNumber>95</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1454,7 +1497,7 @@
     </File>
     <File>
       <GroupNumber>13</GroupNumber>
-      <FileNumber>92</FileNumber>
+      <FileNumber>96</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1466,7 +1509,7 @@
     </File>
     <File>
       <GroupNumber>13</GroupNumber>
-      <FileNumber>93</FileNumber>
+      <FileNumber>97</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1478,7 +1521,7 @@
     </File>
     <File>
       <GroupNumber>13</GroupNumber>
-      <FileNumber>94</FileNumber>
+      <FileNumber>98</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1498,7 +1541,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>14</GroupNumber>
-      <FileNumber>95</FileNumber>
+      <FileNumber>99</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1510,7 +1553,7 @@
     </File>
     <File>
       <GroupNumber>14</GroupNumber>
-      <FileNumber>96</FileNumber>
+      <FileNumber>100</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>

MDK-ARM/CtrBoard-H7_ALL.uvprojx

@@ -1,7 +1,10 @@
-<?xml version="1.0" encoding="UTF-8"?>
+<?xml version="1.0" encoding="UTF-8" standalone="no" ?>
-<Project xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" noNamespaceSchemaLocation="project_projx.xsd">
+<Project xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="project_projx.xsd">
+
   <SchemaVersion>2.1</SchemaVersion>
+
   <Header>### uVision Project, (C) Keil Software</Header>
+
   <Targets>
     <Target>
       <TargetName>CtrBoard-H7_ALL</TargetName>
@@ -16,28 +19,28 @@
           <PackID>Keil.STM32H7xx_DFP.4.0.0</PackID>
           <PackURL>https://www.keil.com/pack/</PackURL>
           <Cpu>IRAM(0x20000000-0x2001FFFF) IRAM2(0x24000000-0x2401FFFF) IROM(0x8000000-0x80FFFFF) CLOCK(12000000) FPU3(DFPU) CPUTYPE("Cortex-M7") ELITTLE TZ</Cpu>
-          <FlashUtilSpec />
+          <FlashUtilSpec></FlashUtilSpec>
-          <StartupFile />
+          <StartupFile></StartupFile>
-          <FlashDriverDll />
+          <FlashDriverDll></FlashDriverDll>
           <DeviceId>0</DeviceId>
-          <RegisterFile />
+          <RegisterFile></RegisterFile>
-          <MemoryEnv />
+          <MemoryEnv></MemoryEnv>
-          <Cmp />
+          <Cmp></Cmp>
-          <Asm />
+          <Asm></Asm>
-          <Linker />
+          <Linker></Linker>
-          <OHString />
+          <OHString></OHString>
-          <InfinionOptionDll />
+          <InfinionOptionDll></InfinionOptionDll>
-          <SLE66CMisc />
+          <SLE66CMisc></SLE66CMisc>
-          <SLE66AMisc />
+          <SLE66AMisc></SLE66AMisc>
-          <SLE66LinkerMisc />
+          <SLE66LinkerMisc></SLE66LinkerMisc>
           <SFDFile>$$Device:STM32H723VGTx$CMSIS\SVD\STM32H723.svd</SFDFile>
           <bCustSvd>0</bCustSvd>
           <UseEnv>0</UseEnv>
-          <BinPath />
+          <BinPath></BinPath>
-          <IncludePath />
+          <IncludePath></IncludePath>
-          <LibPath />
+          <LibPath></LibPath>
-          <RegisterFilePath />
+          <RegisterFilePath></RegisterFilePath>
-          <DBRegisterFilePath />
+          <DBRegisterFilePath></DBRegisterFilePath>
           <TargetStatus>
             <Error>0</Error>
             <ExitCodeStop>0</ExitCodeStop>
@@ -52,15 +55,15 @@
           <CreateHexFile>1</CreateHexFile>
           <DebugInformation>1</DebugInformation>
           <BrowseInformation>1</BrowseInformation>
-          <ListingPath />
+          <ListingPath></ListingPath>
           <HexFormatSelection>1</HexFormatSelection>
           <Merge32K>0</Merge32K>
           <CreateBatchFile>0</CreateBatchFile>
           <BeforeCompile>
             <RunUserProg1>0</RunUserProg1>
             <RunUserProg2>0</RunUserProg2>
-            <UserProg1Name />
+            <UserProg1Name></UserProg1Name>
-            <UserProg2Name />
+            <UserProg2Name></UserProg2Name>
             <UserProg1Dos16Mode>0</UserProg1Dos16Mode>
             <UserProg2Dos16Mode>0</UserProg2Dos16Mode>
             <nStopU1X>0</nStopU1X>
@@ -69,8 +72,8 @@
           <BeforeMake>
             <RunUserProg1>0</RunUserProg1>
             <RunUserProg2>0</RunUserProg2>
-            <UserProg1Name />
+            <UserProg1Name></UserProg1Name>
-            <UserProg2Name />
+            <UserProg2Name></UserProg2Name>
             <UserProg1Dos16Mode>0</UserProg1Dos16Mode>
             <UserProg2Dos16Mode>0</UserProg2Dos16Mode>
             <nStopB1X>0</nStopB1X>
@@ -78,16 +81,16 @@
           </BeforeMake>
           <AfterMake>
             <RunUserProg1>0</RunUserProg1>
-            <RunUserProg2>1</RunUserProg2>
+            <RunUserProg2>0</RunUserProg2>
-            <UserProg1Name />
+            <UserProg1Name></UserProg1Name>
-            <UserProg2Name />
+            <UserProg2Name></UserProg2Name>
             <UserProg1Dos16Mode>0</UserProg1Dos16Mode>
             <UserProg2Dos16Mode>0</UserProg2Dos16Mode>
             <nStopA1X>0</nStopA1X>
             <nStopA2X>0</nStopA2X>
           </AfterMake>
           <SelectedForBatchBuild>1</SelectedForBatchBuild>
-          <SVCSIdString />
+          <SVCSIdString></SVCSIdString>
         </TargetCommonOption>
         <CommonProperty>
           <UseCPPCompiler>0</UseCPPCompiler>
@@ -101,8 +104,8 @@
           <AssembleAssemblyFile>0</AssembleAssemblyFile>
           <PublicsOnly>0</PublicsOnly>
           <StopOnExitCode>3</StopOnExitCode>
-          <CustomArgument />
+          <CustomArgument></CustomArgument>
-          <IncludeLibraryModules />
+          <IncludeLibraryModules></IncludeLibraryModules>
           <ComprImg>0</ComprImg>
         </CommonProperty>
         <DllOption>
@@ -135,11 +138,11 @@
           </Flash1>
           <bUseTDR>1</bUseTDR>
           <Flash2>BIN\UL2V8M.DLL</Flash2>
-          <Flash3 />
+          <Flash3></Flash3>
-          <Flash4 />
+          <Flash4></Flash4>
-          <pFcarmOut />
+          <pFcarmOut></pFcarmOut>
-          <pFcarmGrp />
+          <pFcarmGrp></pFcarmGrp>
-          <pFcArmRoot />
+          <pFcArmRoot></pFcArmRoot>
           <FcArmLst>0</FcArmLst>
         </Utilities>
         <TargetArmAds>
@@ -172,7 +175,7 @@
             <RvctClst>0</RvctClst>
             <GenPPlst>0</GenPPlst>
             <AdsCpuType>"Cortex-M7"</AdsCpuType>
-            <RvctDeviceName />
+            <RvctDeviceName></RvctDeviceName>
             <mOS>0</mOS>
             <uocRom>0</uocRom>
             <uocRam>0</uocRam>
@@ -306,7 +309,7 @@
                 <Size>0x20000</Size>
               </OCR_RVCT10>
             </OnChipMemories>
-            <RvctStartVector />
+            <RvctStartVector></RvctStartVector>
           </ArmAdsMisc>
           <Cads>
             <interw>1</interw>
@@ -333,9 +336,9 @@
             <v6WtE>0</v6WtE>
             <v6Rtti>0</v6Rtti>
             <VariousControls>
-              <MiscControls />
+              <MiscControls></MiscControls>
               <Define>USE_PWR_LDO_SUPPLY,USE_HAL_DRIVER,STM32H723xx</Define>
-              <Undefine />
+              <Undefine></Undefine>
               <IncludePath>../Core/Inc;../USB_DEVICE/App;../USB_DEVICE/Target;../Drivers/STM32H7xx_HAL_Driver/Inc;../Drivers/STM32H7xx_HAL_Driver/Inc/Legacy;../Middlewares/ST/STM32_USB_Device_Library/Core/Inc;../Middlewares/ST/STM32_USB_Device_Library/Class/CDC/Inc;../Drivers/CMSIS/Device/ST/STM32H7xx/Include;../Drivers/CMSIS/Include;../Middlewares/Third_Party/FreeRTOS/Source/include;../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2;../Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F;../User;../Middlewares/Third_Party/Unitree</IncludePath>
             </VariousControls>
           </Cads>
@@ -351,9 +354,9 @@
             <useXO>0</useXO>
             <ClangAsOpt>1</ClangAsOpt>
             <VariousControls>
-              <MiscControls />
+              <MiscControls></MiscControls>
-              <Define />
+              <Define></Define>
-              <Undefine />
+              <Undefine></Undefine>
               <IncludePath>../Drivers/CMSIS/Include</IncludePath>
             </VariousControls>
           </Aads>
@@ -364,15 +367,15 @@
             <noStLib>0</noStLib>
             <RepFail>1</RepFail>
             <useFile>0</useFile>
-            <TextAddressRange />
+            <TextAddressRange></TextAddressRange>
-            <DataAddressRange />
+            <DataAddressRange></DataAddressRange>
-            <pXoBase />
+            <pXoBase></pXoBase>
             <ScatterFile>CtrBoard-H7_ALL\CtrBoard-H7_ALL.sct</ScatterFile>
-            <IncludeLibs />
+            <IncludeLibs></IncludeLibs>
-            <IncludeLibsPath />
+            <IncludeLibsPath></IncludeLibsPath>
-            <Misc />
+            <Misc></Misc>
-            <LinkerInputFile />
+            <LinkerInputFile></LinkerInputFile>
-            <DisabledWarnings />
+            <DisabledWarnings></DisabledWarnings>
           </LDads>
         </TargetArmAds>
       </TargetOption>
@@ -417,6 +420,8 @@
                   <AssembleAssemblyFile>2</AssembleAssemblyFile>
                   <PublicsOnly>2</PublicsOnly>
                   <StopOnExitCode>11</StopOnExitCode>
+                  <CustomArgument></CustomArgument>
+                  <IncludeLibraryModules></IncludeLibraryModules>
                   <ComprImg>1</ComprImg>
                 </CommonProperty>
                 <FileArmAds>
@@ -444,6 +449,12 @@
                     <v6Lto>2</v6Lto>
                     <v6WtE>2</v6WtE>
                     <v6Rtti>2</v6Rtti>
+                    <VariousControls>
+                      <MiscControls></MiscControls>
+                      <Define></Define>
+                      <Undefine></Undefine>
+                      <IncludePath></IncludePath>
+                    </VariousControls>
                   </Cads>
                 </FileArmAds>
               </FileOption>
@@ -735,6 +746,8 @@
               <AssembleAssemblyFile>2</AssembleAssemblyFile>
               <PublicsOnly>2</PublicsOnly>
               <StopOnExitCode>11</StopOnExitCode>
+              <CustomArgument></CustomArgument>
+              <IncludeLibraryModules></IncludeLibraryModules>
               <ComprImg>1</ComprImg>
             </CommonProperty>
             <GroupArmAds>
@@ -762,6 +775,12 @@
                 <v6Lto>2</v6Lto>
                 <v6WtE>2</v6WtE>
                 <v6Rtti>2</v6Rtti>
+                <VariousControls>
+                  <MiscControls></MiscControls>
+                  <Define></Define>
+                  <Undefine></Undefine>
+                  <IncludePath></IncludePath>
+                </VariousControls>
               </Cads>
               <Aads>
                 <interw>2</interw>
@@ -774,6 +793,12 @@
                 <uSurpInc>2</uSurpInc>
                 <useXO>2</useXO>
                 <ClangAsOpt>1</ClangAsOpt>
+                <VariousControls>
+                  <MiscControls></MiscControls>
+                  <Define></Define>
+                  <Undefine></Undefine>
+                  <IncludePath></IncludePath>
+                </VariousControls>
               </Aads>
             </GroupArmAds>
           </GroupOption>
@@ -795,6 +820,8 @@
                   <AssembleAssemblyFile>2</AssembleAssemblyFile>
                   <PublicsOnly>2</PublicsOnly>
                   <StopOnExitCode>11</StopOnExitCode>
+                  <CustomArgument></CustomArgument>
+                  <IncludeLibraryModules></IncludeLibraryModules>
                   <ComprImg>1</ComprImg>
                 </CommonProperty>
                 <FileArmAds>
@@ -822,6 +849,12 @@
                     <v6Lto>2</v6Lto>
                     <v6WtE>2</v6WtE>
                     <v6Rtti>2</v6Rtti>
+                    <VariousControls>
+                      <MiscControls></MiscControls>
+                      <Define></Define>
+                      <Undefine></Undefine>
+                      <IncludePath></IncludePath>
+                    </VariousControls>
                   </Cads>
                 </FileArmAds>
               </FileOption>
@@ -843,6 +876,8 @@
                   <AssembleAssemblyFile>2</AssembleAssemblyFile>
                   <PublicsOnly>2</PublicsOnly>
                   <StopOnExitCode>11</StopOnExitCode>
+                  <CustomArgument></CustomArgument>
+                  <IncludeLibraryModules></IncludeLibraryModules>
                   <ComprImg>1</ComprImg>
                 </CommonProperty>
                 <FileArmAds>
@@ -870,6 +905,12 @@
                     <v6Lto>2</v6Lto>
                     <v6WtE>2</v6WtE>
                     <v6Rtti>2</v6Rtti>
+                    <VariousControls>
+                      <MiscControls></MiscControls>
+                      <Define></Define>
+                      <Undefine></Undefine>
+                      <IncludePath></IncludePath>
+                    </VariousControls>
                   </Cads>
                 </FileArmAds>
               </FileOption>
@@ -891,6 +932,8 @@
                   <AssembleAssemblyFile>2</AssembleAssemblyFile>
                   <PublicsOnly>2</PublicsOnly>
                   <StopOnExitCode>11</StopOnExitCode>
+                  <CustomArgument></CustomArgument>
+                  <IncludeLibraryModules></IncludeLibraryModules>
                   <ComprImg>1</ComprImg>
                 </CommonProperty>
                 <FileArmAds>
@@ -918,6 +961,12 @@
                     <v6Lto>2</v6Lto>
                     <v6WtE>2</v6WtE>
                     <v6Rtti>2</v6Rtti>
+                    <VariousControls>
+                      <MiscControls></MiscControls>
+                      <Define></Define>
+                      <Undefine></Undefine>
+                      <IncludePath></IncludePath>
+                    </VariousControls>
                   </Cads>
                 </FileArmAds>
               </FileOption>
@@ -939,6 +988,8 @@
                   <AssembleAssemblyFile>2</AssembleAssemblyFile>
                   <PublicsOnly>2</PublicsOnly>
                   <StopOnExitCode>11</StopOnExitCode>
+                  <CustomArgument></CustomArgument>
+                  <IncludeLibraryModules></IncludeLibraryModules>
                   <ComprImg>1</ComprImg>
                 </CommonProperty>
                 <FileArmAds>
@@ -966,6 +1017,12 @@
                     <v6Lto>2</v6Lto>
                     <v6WtE>2</v6WtE>
                     <v6Rtti>2</v6Rtti>
+                    <VariousControls>
+                      <MiscControls></MiscControls>
+                      <Define></Define>
+                      <Undefine></Undefine>
+                      <IncludePath></IncludePath>
+                    </VariousControls>
                   </Cads>
                 </FileArmAds>
               </FileOption>
@@ -987,6 +1044,8 @@
                   <AssembleAssemblyFile>2</AssembleAssemblyFile>
                   <PublicsOnly>2</PublicsOnly>
                   <StopOnExitCode>11</StopOnExitCode>
+                  <CustomArgument></CustomArgument>
+                  <IncludeLibraryModules></IncludeLibraryModules>
                   <ComprImg>1</ComprImg>
                 </CommonProperty>
                 <FileArmAds>
@@ -1014,6 +1073,12 @@
                     <v6Lto>2</v6Lto>
                     <v6WtE>2</v6WtE>
                     <v6Rtti>2</v6Rtti>
+                    <VariousControls>
+                      <MiscControls></MiscControls>
+                      <Define></Define>
+                      <Undefine></Undefine>
+                      <IncludePath></IncludePath>
+                    </VariousControls>
                   </Cads>
                 </FileArmAds>
               </FileOption>
@@ -1035,6 +1100,8 @@
                   <AssembleAssemblyFile>2</AssembleAssemblyFile>
                   <PublicsOnly>2</PublicsOnly>
                   <StopOnExitCode>11</StopOnExitCode>
+                  <CustomArgument></CustomArgument>
+                  <IncludeLibraryModules></IncludeLibraryModules>
                   <ComprImg>1</ComprImg>
                 </CommonProperty>
                 <FileArmAds>
@@ -1062,6 +1129,12 @@
                     <v6Lto>2</v6Lto>
                     <v6WtE>2</v6WtE>
                     <v6Rtti>2</v6Rtti>
+                    <VariousControls>
+                      <MiscControls></MiscControls>
+                      <Define></Define>
+                      <Undefine></Undefine>
+                      <IncludePath></IncludePath>
+                    </VariousControls>
                   </Cads>
                 </FileArmAds>
               </FileOption>
@@ -1083,6 +1156,8 @@
                   <AssembleAssemblyFile>2</AssembleAssemblyFile>
                   <PublicsOnly>2</PublicsOnly>
                   <StopOnExitCode>11</StopOnExitCode>
+                  <CustomArgument></CustomArgument>
+                  <IncludeLibraryModules></IncludeLibraryModules>
                   <ComprImg>1</ComprImg>
                 </CommonProperty>
                 <FileArmAds>
@@ -1110,6 +1185,12 @@
                     <v6Lto>2</v6Lto>
                     <v6WtE>2</v6WtE>
                     <v6Rtti>2</v6Rtti>
+                    <VariousControls>
+                      <MiscControls></MiscControls>
+                      <Define></Define>
+                      <Undefine></Undefine>
+                      <IncludePath></IncludePath>
+                    </VariousControls>
                   </Cads>
                 </FileArmAds>
               </FileOption>
@@ -1131,6 +1212,8 @@
                   <AssembleAssemblyFile>2</AssembleAssemblyFile>
                   <PublicsOnly>2</PublicsOnly>
                   <StopOnExitCode>11</StopOnExitCode>
+                  <CustomArgument></CustomArgument>
+                  <IncludeLibraryModules></IncludeLibraryModules>
                   <ComprImg>1</ComprImg>
                 </CommonProperty>
                 <FileArmAds>
@@ -1158,6 +1241,12 @@
                     <v6Lto>2</v6Lto>
                     <v6WtE>2</v6WtE>
                     <v6Rtti>2</v6Rtti>
+                    <VariousControls>
+                      <MiscControls></MiscControls>
+                      <Define></Define>
+                      <Undefine></Undefine>
+                      <IncludePath></IncludePath>
+                    </VariousControls>
                   </Cads>
                 </FileArmAds>
               </FileOption>
@@ -1179,6 +1268,8 @@
                   <AssembleAssemblyFile>2</AssembleAssemblyFile>
                   <PublicsOnly>2</PublicsOnly>
                   <StopOnExitCode>11</StopOnExitCode>
+                  <CustomArgument></CustomArgument>
+                  <IncludeLibraryModules></IncludeLibraryModules>
                   <ComprImg>1</ComprImg>
                 </CommonProperty>
                 <FileArmAds>
@@ -1206,6 +1297,12 @@
                     <v6Lto>2</v6Lto>
                     <v6WtE>2</v6WtE>
                     <v6Rtti>2</v6Rtti>
+                    <VariousControls>
+                      <MiscControls></MiscControls>
+                      <Define></Define>
+                      <Undefine></Undefine>
+                      <IncludePath></IncludePath>
+                    </VariousControls>
                   </Cads>
                 </FileArmAds>
               </FileOption>
@@ -1227,6 +1324,8 @@
                   <AssembleAssemblyFile>2</AssembleAssemblyFile>
                   <PublicsOnly>2</PublicsOnly>
                   <StopOnExitCode>11</StopOnExitCode>
+                  <CustomArgument></CustomArgument>
+                  <IncludeLibraryModules></IncludeLibraryModules>
                   <ComprImg>1</ComprImg>
                 </CommonProperty>
                 <FileArmAds>
@@ -1254,6 +1353,12 @@
                     <v6Lto>2</v6Lto>
                     <v6WtE>2</v6WtE>
                     <v6Rtti>2</v6Rtti>
+                    <VariousControls>
+                      <MiscControls></MiscControls>
+                      <Define></Define>
+                      <Undefine></Undefine>
+                      <IncludePath></IncludePath>
+                    </VariousControls>
                   </Cads>
                 </FileArmAds>
               </FileOption>
@@ -1339,9 +1444,19 @@
               <FilePath>..\User\component\FreeRTOS_CLI.c</FilePath>
             </File>
             <File>
-              <FileName>kinematics.c</FileName>
+              <FileName>crc8.c</FileName>
               <FileType>1</FileType>
-              <FilePath>..\User\component\kinematics.c</FilePath>
+              <FilePath>..\User\component\crc8.c</FilePath>
+            </File>
+            <File>
+              <FileName>crc16.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\component\crc16.c</FilePath>
+            </File>
+            <File>
+              <FileName>limiter.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\component\limiter.c</FilePath>
             </File>
           </Files>
         </Group>
@@ -1363,11 +1478,21 @@
               <FileType>1</FileType>
               <FilePath>..\User\device\go.c</FilePath>
             </File>
+            <File>
+              <FileName>n100.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\device\n100.c</FilePath>
+            </File>
           </Files>
         </Group>
         <Group>
           <GroupName>User/module</GroupName>
           <Files>
+            <File>
+              <FileName>kinematics.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\component\kinematics.c</FilePath>
+            </File>
             <File>
               <FileName>chassis.c</FileName>
               <FileType>1</FileType>
@@ -1461,18 +1586,20 @@
       </Groups>
     </Target>
   </Targets>
+
   <RTE>
-    <apis />
+    <apis/>
     <components>
       <component Cclass="CMSIS" Cgroup="CORE" Cvendor="ARM" Cversion="4.3.0" condition="CMSIS Core">
-        <package name="CMSIS" schemaVersion="1.3" url="http://www.keil.com/pack/" vendor="ARM" version="4.5.0" />
+        <package name="CMSIS" schemaVersion="1.3" url="http://www.keil.com/pack/" vendor="ARM" version="4.5.0"/>
         <targetInfos>
-          <targetInfo name="CtrBoard-H7_ALL" />
+          <targetInfo name="CtrBoard-H7_ALL"/>
         </targetInfos>
       </component>
     </components>
-    <files />
+    <files/>
   </RTE>
+
   <LayerInfo>
     <Layers>
       <Layer>
@@ -1481,5 +1608,5 @@
       </Layer>
     </Layers>
   </LayerInfo>
-</Project>
 
+</Project>

User/bsp/uart.c

@@ -16,6 +16,8 @@ static BSP_UART_t UART_Get(UART_HandleTypeDef *huart) {
   if (huart->Instance == USART3)
     return BSP_UART_RIGHT_LEG;
   if (huart->Instance == USART10)
+    return BSP_UART_IMU_N100;
+  if (huart->Instance == UART7)
     return BSP_UART_AI;
   // else if (huart->Instance == USARTX)
   //   return BSP_UART_XXX;
@@ -114,6 +116,8 @@ UART_HandleTypeDef *BSP_UART_GetHandle(BSP_UART_t uart) {
     case BSP_UART_RIGHT_LEG:
       return &huart3;
     case BSP_UART_AI:
+      return &huart7;
+    case BSP_UART_IMU_N100:
       return &huart10;
     // case BSP_UART_XXX:
     //   return &huartX;

User/bsp/uart.h

@@ -20,6 +20,7 @@ typedef enum {
   BSP_UART_DR16,
   BSP_UART_LEFT_LEG,
   BSP_UART_RIGHT_LEG,
+  BSP_UART_IMU_N100,
   BSP_UART_AI,
   /*BSP_UART_XXX*/
   BSP_UART_NUM,

User/component/cmd.c

@@ -53,8 +53,8 @@ static void CMD_RcLogic(const CMD_RC_t *rc, CMD_t *cmd, float dt_sec) {
       break;
   }
   /* 将操纵杆的对应值转换为底盘的控制向量和云台变化的欧拉角 */
-  cmd->chassis.ctrl_vec.vx = rc->ch_l_x;
+  cmd->chassis.ctrl_vec.vx = rc->ch_l_y;
-  cmd->chassis.ctrl_vec.vy = rc->ch_l_y;
+  cmd->chassis.ctrl_vec.vy = rc->ch_l_x;
   cmd->chassis.ctrl_vec.wz = rc->ch_r_x;
   cmd->chassis.delta_hight = rc->ch_res;
 

User/component/crc16.c

@@ -0,0 +1,35 @@
+/*
+    N100陀螺仪包头crc16校验
+*/
+
+#include <stdint.h>
+
+// CRC16 查找表（多项式 0x1021，初始值 0x0000）
+static const uint16_t CRC16Table[256] = {
+    0x0000, 0x1021, 0x2042, 0x3063, 0x4084, 0x50A5, 0x60C6, 0x70E7, 0x8108, 0x9129, 0xA14A, 0xB16B, 0xC18C, 0xD1AD, 0xE1CE, 0xF1EF,
+    0x1231, 0x0210, 0x3273, 0x2252, 0x52B5, 0x4294, 0x72F7, 0x62D6, 0x9339, 0x8318, 0xB37B, 0xA35A, 0xD3BD, 0xC39C, 0xF3FF, 0xE3DE,
+    0x2462, 0x3443, 0x0420, 0x1401, 0x64E6, 0x74C7, 0x44A4, 0x5485, 0xA56A, 0xB54B, 0x8528, 0x9509, 0xE5EE, 0xF5CF, 0xC5AC, 0xD58D,
+    0x3653, 0x2672, 0x1611, 0x0630, 0x76D7, 0x66F6, 0x5695, 0x46B4, 0xB75B, 0xA77A, 0x9719, 0x8738, 0xF7DF, 0xE7FE, 0xD79D, 0xC7BC,
+    0x48C4, 0x58E5, 0x6886, 0x78A7, 0x0840, 0x1861, 0x2802, 0x3823, 0xC9CC, 0xD9ED, 0xE98E, 0xF9AF, 0x8948, 0x9969, 0xA90A, 0xB92B,
+    0x5AF5, 0x4AD4, 0x7AB7, 0x6A96, 0x1A71, 0x0A50, 0x3A33, 0x2A12, 0xDBFD, 0xCBDC, 0xFBBF, 0xEB9E, 0x9B79, 0x8B58, 0xBB3B, 0xAB1A,
+    0x6CA6, 0x7C87, 0x4CE4, 0x5CC5, 0x2C22, 0x3C03, 0x0C60, 0x1C41, 0xEDAE, 0xFD8F, 0xCDEC, 0xDDCD, 0xAD2A, 0xBD0B, 0x8D68, 0x9D49,
+    0x7E97, 0x6EB6, 0x5ED5, 0x4EF4, 0x3E13, 0x2E32, 0x1E51, 0x0E70, 0xFF9F, 0xEFBE, 0xDFDD, 0xCFFC, 0xBF1B, 0xAF3A, 0x9F59, 0x8F78,
+    0x9188, 0x81A9, 0xB1CA, 0xA1EB, 0xD10C, 0xC12D, 0xF14E, 0xE16F, 0x1080, 0x00A1, 0x30C2, 0x20E3, 0x5004, 0x4025, 0x7046, 0x6067,
+    0x83B9, 0x9398, 0xA3FB, 0xB3DA, 0xC33D, 0xD31C, 0xE37F, 0xF35E, 0x02B1, 0x1290, 0x22F3, 0x32D2, 0x4235, 0x5214, 0x6277, 0x7256,
+    0xB5EA, 0xA5CB, 0x95A8, 0x8589, 0xF56E, 0xE54F, 0xD52C, 0xC50D, 0x34E2, 0x24C3, 0x14A0, 0x0481, 0x7466, 0x6447, 0x5424, 0x4405,
+    0xA7DB, 0xB7FA, 0x8799, 0x97B8, 0xE75F, 0xF77E, 0xC71D, 0xD73C, 0x26D3, 0x36F2, 0x0691, 0x16B0, 0x6657, 0x7676, 0x4615, 0x5634,
+    0xD94C, 0xC96D, 0xF90E, 0xE92F, 0x99C8, 0x89E9, 0xB98A, 0xA9AB, 0x5844, 0x4865, 0x7806, 0x6827, 0x18C0, 0x08E1, 0x3882, 0x28A3,
+    0xCB7D, 0xDB5C, 0xEB3F, 0xFB1E, 0x8BF9, 0x9BD8, 0xABBB, 0xBB9A, 0x4A75, 0x5A54, 0x6A37, 0x7A16, 0x0AF1, 0x1AD0, 0x2AB3, 0x3A92,
+    0xFD2E, 0xED0F, 0xDD6C, 0xCD4D, 0xBDAA, 0xAD8B, 0x9DE8, 0x8DC9, 0x7C26, 0x6C07, 0x5C64, 0x4C45, 0x3CA2, 0x2C83, 0x1CE0, 0x0CC1,
+    0xEF1F, 0xFF3E, 0xCF5D, 0xDF7C, 0xAF9B, 0xBFBA, 0x8FD9, 0x9FF8, 0x6E17, 0x7E36, 0x4E55, 0x5E74, 0x2E93, 0x3EB2, 0x0ED1, 0x1EF0
+};
+
+// 计算 CRC16 校验值
+uint16_t crc16_calc(const uint8_t *data, uint8_t len) {
+    uint16_t crc = 0x0000;
+    for (uint8_t i = 0; i < len; i++) {
+        uint8_t index = (crc >> 8) ^ data[i];
+        crc = (crc << 8) ^ CRC16Table[index];
+    }
+    return crc;
+}

User/component/crc16.h

@@ -0,0 +1,17 @@
+/*
+    N100陀螺仪包头crc16校验
+*/
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+
+uint16_t crc16_calc(const uint8_t *data, uint8_t len);
+
+#ifdef __cplusplus
+}
+#endif

User/component/crc8.c

@@ -0,0 +1,33 @@
+/*
+    N100陀螺仪包头crc8校验
+*/
+#include <stdint.h>
+#include "crc8.h"
+// CRC8 查找表（多项式 0x07，初始值 0x00）
+static const uint8_t CRC8Table[256] = {
+    0x00, 0x5E, 0xBC, 0xE2, 0x61, 0x3F, 0xDD, 0x83, 0xC2, 0x9C, 0x7E, 0x20, 0xA3, 0xFD, 0x1F, 0x41,
+    0x9D, 0xC3, 0x21, 0x7F, 0xFC, 0xA2, 0x40, 0x1E, 0x5F, 0x01, 0xE3, 0xBD, 0x3E, 0x60, 0x82, 0xDC,
+    0x23, 0x7D, 0x9F, 0xC1, 0x42, 0x1C, 0xFE, 0xA0, 0xE1, 0xBF, 0x5D, 0x03, 0x80, 0xDE, 0x3C, 0x62,
+    0xBE, 0xE0, 0x02, 0x5C, 0xDF, 0x81, 0x63, 0x3D, 0x7C, 0x22, 0xC0, 0x9E, 0x1D, 0x43, 0xA1, 0xFF,
+    0x46, 0x18, 0xFA, 0xA4, 0x27, 0x79, 0x9B, 0xC5, 0x84, 0xDA, 0x38, 0x66, 0xE5, 0xBB, 0x59, 0x07,
+    0xDB, 0x85, 0x67, 0x39, 0xBA, 0xE4, 0x06, 0x58, 0x19, 0x47, 0xA5, 0xFB, 0x78, 0x26, 0xC4, 0x9A,
+    0x65, 0x3B, 0xD9, 0x87, 0x04, 0x5A, 0xB8, 0xE6, 0xA7, 0xF9, 0x1B, 0x45, 0xC6, 0x98, 0x7A, 0x24,
+    0xF8, 0xA6, 0x44, 0x1A, 0x99, 0xC7, 0x25, 0x7B, 0x3A, 0x64, 0x86, 0xD8, 0x5B, 0x05, 0xE7, 0xB9,
+    0x8C, 0xD2, 0x30, 0x6E, 0xED, 0xB3, 0x51, 0x0F, 0x4E, 0x10, 0xF2, 0xAC, 0x2F, 0x71, 0x93, 0xCD,
+    0x11, 0x4F, 0xAD, 0xF3, 0x70, 0x2E, 0xCC, 0x92, 0xD3, 0x8D, 0x6F, 0x31, 0xB2, 0xEC, 0x0E, 0x50,
+    0xAF, 0xF1, 0x13, 0x4D, 0xCE, 0x90, 0x72, 0x2C, 0x6D, 0x33, 0xD1, 0x8F, 0x0C, 0x52, 0xB0, 0xEE,
+    0x32, 0x6C, 0x8E, 0xD0, 0x53, 0x0D, 0xEF, 0xB1, 0xF0, 0xAE, 0x4C, 0x12, 0x91, 0xCF, 0x2D, 0x73,
+    0xCA, 0x94, 0x76, 0x28, 0xAB, 0xF5, 0x17, 0x49, 0x08, 0x56, 0xB4, 0xEA, 0x69, 0x37, 0xD5, 0x8B,
+    0x57, 0x09, 0xEB, 0xB5, 0x36, 0x68, 0x8A, 0xD4, 0x95, 0xCB, 0x29, 0x77, 0xF4, 0xAA, 0x48, 0x16,
+    0xE9, 0xB7, 0x55, 0x0B, 0x88, 0xD6, 0x34, 0x6A, 0x2B, 0x75, 0x97, 0xC9, 0x4A, 0x14, 0xF6, 0xA8,
+    0x74, 0x2A, 0xC8, 0x96, 0x15, 0x4B, 0xA9, 0xF7, 0xB6, 0xE8, 0x0A, 0x54, 0xD7, 0x89, 0x6B, 0x35,
+};
+
+// 计算 CRC8 校验值
+uint8_t crc8_calc(const uint8_t *data, uint8_t len) {
+    uint8_t crc = 0x00;
+    for (uint8_t i = 0; i < len; i++) {
+        crc = CRC8Table[crc ^ data[i]];
+    }
+    return crc;
+}

User/component/crc8.h

@@ -0,0 +1,17 @@
+/*
+    N100陀螺仪包头crc8校验
+*/
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdint.h>
+
+uint8_t crc8_calc(const uint8_t *data, uint8_t len);
+
+#ifdef __cplusplus
+}
+#endif

User/component/kinematics.c

@@ -3,9 +3,35 @@
 */
 
 #include "filter.h"
-
+#include <math.h>
 #include "kinematics.h"
 
+static float q1_ik(float py, float pz, float l1) {
+    float q1;
+    float L = sqrtf(py*py + pz*pz - l1*l1);
+    q1 = atan2f(pz*l1 + py*L, py*l1 - pz*L);
+    return q1;
+}
+
+static float q3_ik(float b3z, float b4z, float b) {
+    float q3, temp;
+    temp = (b3z*b3z + b4z*b4z - b*b) / (2.0f * fabsf(b3z * b4z));
+    if (temp > 1.0f) temp = 1.0f;
+    if (temp < -1.0f) temp = -1.0f;
+    q3 = acosf(temp);
+    q3 = -(M_PI - q3);
+    return q3;
+}
+
+static float q2_ik(float q1, float q3, float px, float py, float pz, float b3z, float b4z) {
+    float a1 = py * sinf(q1) - pz * cosf(q1);
+    float a2 = px;
+    float m1 = b4z * sinf(q3);
+    float m2 = b3z + b4z * cosf(q3);
+    float q2 = atan2f(m1 * a1 + m2 * a2, m1 * a2 - m2 * a1);
+    return q2;
+}
+
 
 int8_t Kinematics_RealFeedback(Kinematics_JointData_t *real, const Kinematics_JointData_t *in, float ratio, float joint_zero){
     real->Pos = (in->Pos-joint_zero) / ratio;
@@ -23,27 +49,30 @@ int8_t Kinematics_RealOutput(const Kinematics_JointCMD_t *real, Kinematics_Joint
     return 0;
 }
 
-/**
+void Kinematics_ForwardKinematics(const float theta_in[3], const Kinematics_LinkLength_t *leg_params, float foot_out[3],  Kinematics_Sign_t *sign) {
- * @brief 单腿正运动学
+    float l1 = leg_params->hip * sign->hip;
- * @param theta[3] 关节角度数组（单位：弧度）：[hip, thigh, calf]
+    float l2 = -leg_params->thigh;
- * @param leg_params 单腿机械参数（长度）
+    float l3 = -leg_params->calf;
- * @param foot_out 足端位置输出 [x, y, z]（单位：米）
- */
-// void Leg_ForwardKinematics(const float theta[3], const joint_params *leg_params, float foot_out[3]) {
-//     float l1 = leg_params->named.lf_hip;    // 髋关节到大腿长度
-//     float l2 = leg_params->named.lf_thigh;  // 大腿长度
-//     float l3 = leg_params->named.lf_calf;   // 小腿长度
 
-//     float t1 = theta[0]; // 髋关节侧摆（绕y轴）
+    float q1 = theta_in[0];
-//     float t2 = theta[1]; // 髋关节前后
+    float q2 = theta_in[1] * sign->thigh;
-//     float t3 = theta[2]; // 膝关节
+    float q3 = theta_in[2] * sign->calf;
 
-//     // 末端位置
+    float s1 = sinf(q1);
-//     float leg_proj = l2 * cosf(t2) + l3 * cosf(t2 + t3);
+    float s2 = sinf(q2);
-//     foot_out[0] = cosf(t1) * leg_proj + l1 * cosf(t1); // x
+    float s3 = sinf(q3);
-//     foot_out[1] = sinf(t1) * leg_proj + l1 * sinf(t1); // y
+
-//     foot_out[2] = l2 * sinf(t2) + l3 * sinf(t2 + t3);  // z
+    float c1 = cosf(q1);
-// }
+    float c2 = cosf(q2);
+    float c3 = cosf(q3);
+
+    float c23 = c2 * c3 - s2 * s3;
+    float s23 = s2 * c3 + c2 * s3;
+
+    foot_out[0] = l3 * s23 + l2 * s2;
+    foot_out[1] = -l3 * s1 * c23 + l1 * c1 - l2 * c2 * s1;
+    foot_out[2] =  l3 * c1 * c23 + l1 * s1 + l2 * c1 * c2;
+}
 
 /**
  * @brief 单腿逆运动学
@@ -52,34 +81,28 @@ int8_t Kinematics_RealOutput(const Kinematics_JointCMD_t *real, Kinematics_Joint
  * @param theta_out 关节角度输出数组（单位：弧度）：[hip, thigh, calf]
  * @return 0成功，-1目标不可达
  */
-// int Leg_InverseKinematics(const float foot_in[3], const joint_params *leg_params, float theta_out[3]) {
+int8_t Kinematics_InverseKinematics(const float foot_in[3], const Kinematics_LinkLength_t *leg_params, float theta_out[3], Kinematics_Sign_t *sign) {
-//     float l1 = leg_params->named.lf_hip;
+    float px = foot_in[0];
-//     float l2 = leg_params->named.lf_thigh;
+    float py = foot_in[1];
-//     float l3 = leg_params->named.lf_calf;
+    float pz = foot_in[2];
 
-//     float x = foot_in[0];
+    float b2y = leg_params->hip * sign->hip;
-//     float y = foot_in[1];
+    float b3z = -leg_params->thigh;
-//     float z = foot_in[2];
+    float b4z = -leg_params->calf;
+    float a = leg_params->hip;
+    float c = sqrtf(px*px + py*py + pz*pz);
+    float b = sqrtf(c*c - a*a);
 
-//     // 1. 先算髋关节侧摆角（绕y轴）
+    // 检查可达性
-//     float r = sqrtf(x * x + y * y);
+    if (b != b) return -1; // NaN
-//     float t1 = atan2f(y, x); // 髋关节侧摆角：使用 atan2f，计
+    if (b > fabsf(b3z) + fabsf(b4z)) return -1;
 
-//     // 2. 去掉髋关节侧摆影响，得到在sagittal平面上的投影
+    float q1 = q1_ik(py, pz, b2y);
-//     float xp = r - l1;
+    float q3 = q3_ik(b3z, b4z, b);
-//     float zp = z;
+    float q2 = q2_ik(q1, q3, px, py, pz, b3z, b4z);
 
-//     // 3. 逆解余下两个关节
+    theta_out[0] = q1;
-//     float D = (xp * xp + zp * zp - l2 * l2 - l3 * l3) / (2 * l2 * l3);
+    theta_out[1] = q2 * sign->thigh;
-//     if (D < -1.0f || D > 1.0f) return -1; // 不可达
+    theta_out[2] = q3 * sign->calf;
-
+    return 0;
-//     float t3 = atan2f(-sqrtf(1 - D * D), D); // 膝关节
+}
-//     float k1 = l2 + l3 * cosf(t3);
-//     float k2 = l3 * sinf(t3);
-//     float t2 = atan2f(zp, xp) - atan2f(k2, k1);
-
-//     theta_out[0] = t1;
-//     theta_out[1] = t2;
-//     theta_out[2] = t3;
-//     return 0;
-// }

User/component/kinematics.h

@@ -24,6 +24,24 @@ typedef struct {
   float K_W;           // 关节速度系数(0-25.599)
 } Kinematics_JointCMD_t;
 
+typedef struct {
+  float hip;   // 髋关节长度(m)
+  float thigh; // 大腿长度(m)
+  float calf;  // 小腿长度(m)
+} Kinematics_LinkLength_t;
+
+typedef struct {
+  float x;
+  float y;
+  float z;
+} Kinematics_LegOffset_t;
+
+typedef struct {
+  float hip;   
+  float thigh; 
+  float calf;  
+} Kinematics_Sign_t;
+
 
 /**
  * @brief 计算实际反馈
@@ -44,6 +62,25 @@ int8_t Kinematics_RealFeedback(Kinematics_JointData_t *real, const Kinematics_Jo
 int8_t Kinematics_RealOutput(const Kinematics_JointCMD_t *real, Kinematics_JointCMD_t *out, float ratio, float joint_zero);
 
 
+/**
+ * @brief 单腿正运动学
+ * @param theta_in 关节角度输入数组（单位：弧度）：[hip, thigh, calf]
+ * @param leg_params 单腿机械参数（长度）
+ * @param foot_out 足端位置输出 [x, y, z]（单位：米）
+ * @param side_sign 侧向标志（左前/左后为-1，右前/右后为1）
+ */
+void Kinematics_ForwardKinematics(const float theta_in[3], const Kinematics_LinkLength_t *leg_params, float foot_out[3],  Kinematics_Sign_t *sign);
+
+/**
+ * @brief 单腿逆运动学
+ * @param foot_in 足端目标位置 [x, y, z]（单位：米）
+ * @param leg_params 单腿机械参数（长度）
+ * @param theta_out 关节角度输出数组（单位：弧度）：[hip, thigh, calf]
+ * @param side_sign 侧向标志（左前/左后为-1，右前/右后为1）
+ * @return 0成功，-1目标不可达
+ */
+int8_t Kinematics_InverseKinematics(const float foot_in[3], const Kinematics_LinkLength_t *leg_params, float theta_out[3], Kinematics_Sign_t *sign);
+
 
 #ifdef __cplusplus
 }

User/component/limiter.c

@@ -0,0 +1,27 @@
+/*
+  限制器
+*/
+
+#include "limiter.h"
+
+#include <math.h>
+#include <stddef.h>
+
+#include "component/user_math.h"
+
+int8_t Limit_ChassicOutput(const float feedback_pos ,float *out_pos, float max_speed, float max_angle, float min_angle){
+
+  // 限制位置变化速度
+  if (*out_pos - feedback_pos > max_speed) {
+    *out_pos = feedback_pos + max_speed;
+  } else if (*out_pos - feedback_pos < -max_speed) {
+    *out_pos = feedback_pos - max_speed;
+  }
+  // 限制角度范围
+  if (*out_pos > max_angle) {
+    *out_pos = max_angle;
+  } else if (*out_pos < min_angle) {
+    *out_pos = min_angle;
+  }
+  return 0; // 成功
+}

User/component/limiter.h

@@ -0,0 +1,20 @@
+/*
+  限制器
+*/
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <stdbool.h>
+#include <stdint.h>
+
+
+int8_t Limit_ChassicOutput(const float feedback_pos ,float *out_pos, 
+  float max_speed, float max_angle, float min_angle);
+
+#ifdef __cplusplus
+}
+#endif

User/component/unitreeLeg.cpp

@@ -0,0 +1,175 @@
+/**********************************************************************
+ Copyright (c) 2020-2023, Unitree Robotics.Co.Ltd. All rights reserved.
+***********************************************************************/
+#include "common/unitreeLeg.h"
+#include <iostream>
+
+
+/************************/
+/*******QuadrupedLeg*****/
+/************************/
+QuadrupedLeg::QuadrupedLeg(int legID, float abadLinkLength, float hipLinkLength, 
+                           float kneeLinkLength, Vec3 pHip2B)
+            :_abadLinkLength(abadLinkLength), 
+             _hipLinkLength(hipLinkLength), 
+             _kneeLinkLength(kneeLinkLength), 
+             _pHip2B(pHip2B){
+    if (legID == 0 || legID == 2)
+        _sideSign = -1;
+    else if (legID == 1 || legID == 3)
+        _sideSign = 1;
+    else{
+        std::cout << "Leg ID incorrect!" << std::endl;
+        exit(-1);
+    }
+}
+
+// Forward Kinematics
+Vec3 QuadrupedLeg::calcPEe2H(Vec3 q){
+    float l1 = _sideSign * _abadLinkLength;
+    float l2 = -_hipLinkLength;
+    float l3 = -_kneeLinkLength;
+
+    float s1 = std::sin(q(0));
+    float s2 = std::sin(q(1));
+    float s3 = std::sin(q(2));
+
+    float c1 = std::cos(q(0));
+    float c2 = std::cos(q(1));
+    float c3 = std::cos(q(2));
+
+    float c23 = c2 * c3 - s2 * s3;
+    float s23 = s2 * c3 + c2 * s3;
+
+    Vec3 pEe2H;
+
+    pEe2H(0) = l3 * s23 + l2 * s2;
+    pEe2H(1) = -l3 * s1 * c23 + l1 * c1 - l2 * c2 * s1;
+    pEe2H(2) =  l3 * c1 * c23 + l1 * s1 + l2 * c1 * c2;
+
+    return pEe2H;
+}
+
+// Forward Kinematics
+Vec3 QuadrupedLeg::calcPEe2B(Vec3 q){
+    return _pHip2B + calcPEe2H(q);
+}
+
+// Derivative Forward Kinematics
+Vec3 QuadrupedLeg::calcVEe(Vec3 q, Vec3 qd){
+    return calcJaco(q) * qd;
+}
+
+// Inverse Kinematics
+Vec3 QuadrupedLeg::calcQ(Vec3 pEe, FrameType frame){
+    Vec3 pEe2H;
+    if(frame == FrameType::HIP)
+        pEe2H = pEe;
+    else if(frame == FrameType::BODY)
+        pEe2H = pEe - _pHip2B;
+    else{
+        std::cout << "[ERROR] The frame of QuadrupedLeg::calcQ can only be HIP or BODY!" << std::endl;
+        exit(-1);
+    }
+
+    float q1, q2, q3;
+    Vec3 qResult;
+    float px, py, pz;
+    float b2y, b3z, b4z, a, b, c;
+
+    px = pEe2H(0);
+    py = pEe2H(1);
+    pz = pEe2H(2);
+
+    b2y = _abadLinkLength * _sideSign;
+    b3z = -_hipLinkLength;
+    b4z = -_kneeLinkLength;
+    a = _abadLinkLength;
+    c = sqrt(pow(px, 2) + pow(py, 2) + pow(pz, 2)); // whole length
+    b = sqrt(pow(c, 2) - pow(a, 2)); // distance between shoulder and footpoint
+
+    q1 = q1_ik(py, pz, b2y);
+    q3 = q3_ik(b3z, b4z, b);
+    q2 = q2_ik(q1, q3, px, py, pz, b3z, b4z);
+
+    qResult(0) = q1;
+    qResult(1) = q2;
+    qResult(2) = q3;
+
+    return qResult;
+}
+
+// Derivative Inverse Kinematics
+Vec3 QuadrupedLeg::calcQd(Vec3 q, Vec3 vEe){
+    return calcJaco(q).inverse() * vEe;
+}
+
+// Derivative Inverse Kinematics
+Vec3 QuadrupedLeg::calcQd(Vec3 pEe, Vec3 vEe, FrameType frame){
+    Vec3 q = calcQ(pEe, frame);
+    return calcJaco(q).inverse() * vEe;
+}
+
+// Inverse Dynamics
+Vec3 QuadrupedLeg::calcTau(Vec3 q, Vec3 force){
+    return calcJaco(q).transpose() * force;
+}
+
+// Jacobian Matrix
+Mat3 QuadrupedLeg::calcJaco(Vec3 q){
+    Mat3 jaco;
+
+    float l1 = _abadLinkLength * _sideSign;
+    float l2 = -_hipLinkLength;
+    float l3 = -_kneeLinkLength;
+
+    float s1 = std::sin(q(0));
+    float s2 = std::sin(q(1));
+    float s3 = std::sin(q(2));
+
+    float c1 = std::cos(q(0));
+    float c2 = std::cos(q(1));
+    float c3 = std::cos(q(2));
+
+    float c23 = c2 * c3 - s2 * s3;
+    float s23 = s2 * c3 + c2 * s3;
+    jaco(0, 0) = 0;
+    jaco(1, 0) = -l3 * c1 * c23 - l2 * c1 * c2 - l1 * s1;
+    jaco(2, 0) = -l3 * s1 * c23 - l2 * c2 * s1 + l1 * c1;
+    jaco(0, 1) = l3 * c23 + l2 * c2;
+    jaco(1, 1) = l3 * s1 * s23 + l2 * s1 * s2;
+    jaco(2, 1) = -l3 * c1 * s23 - l2 * c1 * s2;
+    jaco(0, 2) = l3 * c23;
+    jaco(1, 2) = l3 * s1 * s23;
+    jaco(2, 2) = -l3 * c1 * s23;
+
+    return jaco;
+}
+
+float QuadrupedLeg::q1_ik(float py, float pz, float l1){
+    float q1;
+    float L = sqrt(pow(py,2)+pow(pz,2)-pow(l1,2));
+    q1 = atan2(pz*l1+py*L, py*l1-pz*L);
+    return q1;
+}
+
+float QuadrupedLeg::q3_ik(float b3z, float b4z, float b){
+    float q3, temp;
+    temp = (pow(b3z, 2) + pow(b4z, 2) - pow(b, 2))/(2*fabs(b3z*b4z));
+    if(temp>1) temp = 1;
+    if(temp<-1) temp = -1;
+    q3 = acos(temp);
+    q3 = -(M_PI - q3); //0~180
+    return q3;
+}
+
+float QuadrupedLeg::q2_ik(float q1, float q3, float px, float py, float pz, float b3z, float b4z){
+    float q2, a1, a2, m1, m2;
+    
+    a1 = py*sin(q1) - pz*cos(q1);
+    a2 = px;
+    m1 = b4z*sin(q3);
+    m2 = b3z + b4z*cos(q3);
+    q2 = atan2(m1*a1+m2*a2, m1*a2-m2*a1);
+    return q2;
+}

User/component/user_math.c

@@ -130,3 +130,4 @@ void VerifyFailed(const char *file, uint32_t line) {
     __NOP();
   }
 }
+

User/device/ai.c

@@ -0,0 +1,152 @@
+/*
+AI
+*/
+
+/* Includes ----------------------------------------------------------------- */
+#include "ai.h"
+
+#include <string.h>
+
+#include "bsp\delay.h"
+#include "bsp\uart.h"
+#include "component\crc16.h"
+#include "component\crc8.h"
+#include "component\user_math.h"
+#include "component\filter.h"
+
+
+/* Private define ----------------------------------------------------------- */
+#define AI_LEN_RX_BUFF (sizeof(Protocol_DownPackage_t))
+
+/* Private macro ------------------------------------------------------------ */
+/* Private typedef ---------------------------------------------------------- */
+/* Private variables -------------------------------------------------------- */
+static volatile uint32_t drop_message = 0;
+
+static uint8_t rxbuf[AI_LEN_RX_BUFF];
+
+static bool inited = false;
+
+static osThreadId_t thread_alert;
+
+/* Private function  -------------------------------------------------------- */
+
+static void Ai_RxCpltCallback(void) {
+  osThreadFlagsSet(thread_alert, SIGNAL_AI_RAW_REDY);
+}
+
+static void Ai_IdleLineCallback(void) {
+  osThreadFlagsSet(thread_alert, SIGNAL_AI_RAW_REDY);
+}
+
+/* Exported functions ------------------------------------------------------- */
+int8_t AI_Init(AI_t *ai) {
+  UNUSED(ai);
+  ASSERT(ai);
+  if (inited) return DEVICE_ERR_INITED;
+  VERIFY((thread_alert = osThreadGetId()) != NULL);
+
+  BSP_UART_RegisterCallback(BSP_UART_AI, BSP_UART_RX_CPLT_CB,
+                            Ai_RxCpltCallback);
+  BSP_UART_RegisterCallback(BSP_UART_AI, BSP_UART_IDLE_LINE_CB,
+                            Ai_IdleLineCallback);
+  
+  inited = true;
+  return 0;
+}
+
+int8_t AI_Restart(void) {
+  __HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_AI));
+  __HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_AI));
+  return DEVICE_OK;
+}
+
+int8_t AI_StartReceiving(AI_t *ai) {
+  UNUSED(ai);
+  if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_AI), rxbuf,
+                           AI_LEN_RX_BUFF) == HAL_OK)
+    return DEVICE_OK;
+  return DEVICE_ERR;
+}
+
+bool AI_WaitDmaCplt(void) {
+  return (osThreadFlagsWait(SIGNAL_AI_RAW_REDY, osFlagsWaitAll, 0) ==
+          SIGNAL_AI_RAW_REDY);
+}
+
+int8_t AI_ParseHost(AI_t *ai) {
+  if (!CRC16_Verify((const uint8_t *)&(rxbuf), sizeof(ai->from_host)))
+    goto error;
+  ai->ai_online = true;
+  memcpy(&(ai->from_host), rxbuf, sizeof(ai->from_host));
+  memset(rxbuf, 0, AI_LEN_RX_BUFF);
+  return DEVICE_OK;
+
+error:
+  drop_message++;
+  return DEVICE_ERR;
+}
+
+void AI_PackCmd(AI_t *ai, CMD_Host_t *cmd_host) {
+  cmd_host->gimbal_delta.yaw = ai->from_host.data.gimbal.yaw;
+  cmd_host->gimbal_delta.pit = ai->from_host.data.gimbal.pit;
+  cmd_host->fire = (ai->from_host.data.notice & AI_NOTICE_FIRE);
+  cmd_host->chassis_move_vec.vx = ai->from_host.data.chassis_move_vec.vx;
+  cmd_host->chassis_move_vec.vy = ai->from_host.data.chassis_move_vec.vy;
+  cmd_host->chassis_move_vec.wz = ai->from_host.data.chassis_move_vec.wz;
+}
+
+int8_t AI_HandleOffline(AI_t *ai, CMD_Host_t *cmd_host) {
+  if (ai == NULL) return DEVICE_ERR_NULL;
+  if (cmd_host == NULL) return DEVICE_ERR_NULL;
+  ai->ai_online = false;
+  memset(&(ai->from_host), 0, sizeof(ai->from_host));
+  memset(cmd_host, 0, sizeof(*cmd_host));
+  return 0;
+}
+
+int8_t AI_PackMCU(AI_t *ai, const AHRS_Quaternion_t *quat) {
+  ai->to_host.mcu.id = AI_ID_MCU;
+  memcpy((void *)&(ai->to_host.mcu.package.data.quat), (const void *)quat,
+         sizeof(*quat));
+  ai->to_host.mcu.package.data.notice = 0;
+  if (ai->status == AI_STATUS_AUTOAIM)
+    ai->to_host.mcu.package.data.notice |= AI_NOTICE_AUTOAIM;
+  else if (ai->status == AI_STATUS_HITSWITCH)
+    ai->to_host.mcu.package.data.notice |= AI_NOTICE_HITBUFF;
+  else if (ai->status == AI_STATUS_AUTOMATIC)
+    ai->to_host.mcu.package.data.notice |= AI_NOTICE_AUTOMATIC;
+
+  ai->to_host.mcu.package.crc16 = CRC16_Calc(
+      (const uint8_t *)&(ai->to_host.mcu.package),
+      sizeof(ai->to_host.mcu.package) - sizeof(uint16_t), CRC16_INIT);
+  return DEVICE_OK;
+}
+
+int8_t AI_PackRef(AI_t *ai, const Referee_ForAI_t *ref) {
+  (void)ref;
+  ai->to_host.ref.id = AI_ID_REF;
+  ai->to_host.ref.package.crc16 = CRC16_Calc(
+      (const uint8_t *)&(ai->to_host.ref.package),
+      sizeof(ai->to_host.ref.package) - sizeof(uint16_t), CRC16_INIT);
+  return DEVICE_OK;
+}
+
+int8_t AI_StartSend(AI_t *ai, bool ref_update) {
+  if (ref_update) {
+    if (HAL_UART_Transmit_DMA(
+            BSP_UART_GetHandle(BSP_UART_AI), (uint8_t *)&(ai->to_host),
+            sizeof(ai->to_host.ref) + sizeof(ai->to_host.mcu)) == HAL_OK)
+      return DEVICE_OK;
+    else
+      return DEVICE_ERR;
+  } else {
+    if (HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_AI),
+                              (uint8_t *)&(ai->to_host.mcu),
+                              sizeof(ai->to_host.mcu)) == HAL_OK)
+      return DEVICE_OK;
+    else
+      return DEVICE_ERR;
+  }
+}
+

User/device/ai.h

@@ -0,0 +1,69 @@
+/*
+  AI
+*/
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include <cmsis_os2.h>
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "component\ahrs.h"
+#include "component\cmd.h"
+#include "component\user_math.h"
+#include "component\filter.h"
+#include "device\device.h"
+#include "device\referee.h"
+#include "protocol.h"
+
+/* Exported constants ------------------------------------------------------- */
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+
+typedef struct __packed {
+  uint8_t id;
+  Protocol_UpPackageReferee_t package;
+} AI_UpPackageReferee_t;
+
+typedef struct __packed {
+  uint8_t id;
+  Protocol_UpPackageMCU_t package;
+} AI_UpPackageMCU_t;
+
+typedef struct __packed {
+  osThreadId_t thread_alert;
+
+  Protocol_DownPackage_t from_host;
+
+  struct {
+    AI_UpPackageReferee_t ref;
+    AI_UpPackageMCU_t mcu;
+  } to_host;
+
+  CMD_AI_Status_t status;
+  bool ai_online;
+} AI_t;
+
+/* Exported functions prototypes -------------------------------------------- */
+int8_t AI_Init(AI_t *ai);
+int8_t AI_Restart(void);
+
+int8_t AI_StartReceiving(AI_t *ai);
+bool AI_WaitDmaCplt(void);
+int8_t AI_ParseHost(AI_t *ai);
+int8_t AI_HandleOffline(AI_t *ai, CMD_Host_t *cmd_host);
+int8_t AI_PackMCU(AI_t *ai, const AHRS_Quaternion_t *quat);
+int8_t AI_PackRef(AI_t *ai, const Referee_ForAI_t *ref);
+int8_t AI_StartSend(AI_t *ai, bool option);
+void AI_PackCmd(AI_t *ai, CMD_Host_t *cmd_host);
+#ifdef __cplusplus
+}
+#endif
+
+
+

User/device/bmi088.c

@@ -365,5 +365,6 @@ int8_t BMI088_ParseGyro(BMI088_t *bmi088) {
 
 float BMI088_GetUpdateFreq(BMI088_t *bmi088) {
   (void)bmi088;
-  return 400.0f;
+  return 200.0f;
 }
+

User/device/device.h

@@ -19,19 +19,12 @@ extern "C" {
 #define SIGNAL_BMI088_GYRO_RAW_REDY (1u << 2)
 #define SIGNAL_BMI088_ACCL_RAW_REDY (1u << 3)
 
-#define SIGNAL_DR16_RAW_REDY (1u << 4)
+#define SIGNAL_N100_NEW_DATA (1u << 4)
+#define SIGNAL_DR16_RAW_REDY (1u << 5)
 
-#define SIGNAL_GO_MOTOR_RECV (1u << 5)
+#define SIGNAL_GO_MOTOR_RECV (1u << 6)
 
 
-
-#define SIGNAL_IST8310_MAGN_NEW_DATA (1u << 8)
-#define SIGNAL_IST8310_MAGN_RAW_REDY (1u << 9)
-
-#define SIGNAL_REFEREE_RAW_REDY (1u << 10)
-#define SIGNAL_REFEREE_FAST_REFRESH_UI (1u << 11)
-#define SIGNAL_REFEREE_SLOW_REFRESH_UI (1u << 12)
-
 #define SIGNAL_AI_RAW_REDY (1u << 14)
 
 #ifdef __cplusplus

User/device/go copy.c

@@ -1,118 +0,0 @@
-/*
-  四足机器人12个关节驱动
-*/
-
-/* Includes ----------------------------------------------------------------- */
-#include "bsp/uart.h"
-#include "go.h"
-
-#include <stdbool.h>
-#include <string.h>
-
-
-#include "component/user_math.h"
-
-#include "cmsis_os2.h"
-
-static osSemaphoreId_t sem_left_rx = NULL;
-static osSemaphoreId_t sem_right_rx = NULL;
-
-/* Private define ----------------------------------------------------------- */
-
-
-/* Private macro ------------------------------------------------------------ */
-/* Private typedef ---------------------------------------------------------- */
-/* Private variables -------------------------------------------------------- */
-// static RIS_MotorData_t raw_left __attribute__ ((section(".AXI_SRAM")));
-static uint8_t raw_left_buff[16] __attribute__ ((section(".AXI_SRAM")));
-static uint8_t raw_right_buff[16] __attribute__ ((section(".AXI_SRAM")));
-// static RIS_MotorData_t raw_right __attribute__ ((section(".AXI_SRAM")));
-static MotorCmd_t motor_cmd __attribute__ ((section(".AXI_SRAM")));
-
-static osThreadId_t thread_alert;
-static GO_t *go_rx;
-static bool inited = false;
-
-/* Private function  -------------------------------------------------------- */
-
-// static void GO_LeftRxCpltCallback(void) {
-//     osMessageQueuePut(go_rx->msgq_raw, &raw_left_buff, 0, 0);
-//     osSemaphoreRelease(sem_left_rx); // 通知接收完成
-    // memset(raw_left_buff, 0, sizeof(raw_left_buff)); // 清空缓存区
-    // HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_LEFT_LEG),
-    //                     (uint8_t *)&raw_left, sizeof(RIS_MotorData_t));
-// }
-// static void GO_RightRxCpltCallback(void) {
-//     osMessageQueuePut(go_rx->msgq_raw, &raw_right_buff, 0, 0);
-//     osSemaphoreRelease(sem_right_rx); // 通知接收完成
-    // memset(raw_right_buff, 0, sizeof(raw_right_buff)); // 清空缓存区
-    // HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_RIGHT_LEG),
-    //                     (uint8_t *)&raw_right, sizeof(RIS_MotorData_t));
-// }
-
-/* Exported functions ------------------------------------------------------- */
-
-int8_t GO_Init(GO_t *go, const GO_UART_Params_t *param){
-    if (go == NULL) return DEVICE_ERR_NULL;
-    if (inited) return DEVICE_ERR_INITED;
-    if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;
-    
-    go_rx->msgq_raw = osMessageQueueNew(36, 16u, NULL);
-    
-    go->param = param;
-    sem_left_rx = osSemaphoreNew(1, 0, NULL);
-    sem_right_rx = osSemaphoreNew(1, 0, NULL);
-    // BSP_UART_RegisterCallback(BSP_UART_LEFT_LEG, BSP_UART_RX_CPLT_CB,
-    //                             GO_LeftRxCpltCallback);
-    // BSP_UART_RegisterCallback(BSP_UART_RIGHT_LEG, BSP_UART_RX_CPLT_CB,
-    //                             GO_RightRxCpltCallback);
-    
-    inited = true;
-    return DEVICE_OK;
-}
-
-int8_t GO_Motor_Control(GO_MotorOutput_t *output, GO_t *go){
-    if (go == NULL) return DEVICE_ERR_NULL;
-    if (output == NULL) return DEVICE_ERR_NULL;
-    memset(raw_right_buff, 0, sizeof(raw_right_buff));
-    memset(raw_left_buff, 0, sizeof(raw_left_buff));
-    // HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_LEFT_LEG),
-    //                     (uint8_t *)&raw_left_buff, 16);
-    // HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_RIGHT_LEG),
-    //                     (uint8_t *)&raw_right_buff, 16);
-
-    for (uint8_t i = 0; i < GO_MOTOR_NUM; i++) {
-        motor_cmd.id = i;
-        motor_cmd.mode = GO_MOTOR_MODE_FOC;
-        motor_cmd.T = output->as_array[i].tor_des;
-        motor_cmd.W = output->as_array[i].spd_des;
-        motor_cmd.Pos = output->as_array[i].pos_des;
-        motor_cmd.K_P = output->as_array[i].k_pos;
-        motor_cmd.K_W = output->as_array[i].k_spd;
-
-        modify_data(&motor_cmd);
-
-        if (i <= GO_MOTOR_LF_CALF || (i >= GO_MOTOR_LR_HIP && i <= GO_MOTOR_LR_CALF)) {
-            HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_LEFT_LEG),
-                              (uint8_t *)&motor_cmd.motor_send_data,
-                              sizeof(RIS_ControlData_t));
-
-            HAL_UART_Receive(BSP_UART_GetHandle(BSP_UART_LEFT_LEG),
-                            (uint8_t *)&raw_left_buff, 16, 5);
-        } else {
-            HAL_UART_Transmit_DMA(BSP_UART_GetHandle(BSP_UART_RIGHT_LEG),
-                              (uint8_t *)&motor_cmd.motor_send_data,
-                              sizeof(RIS_ControlData_t));
-
-            HAL_UART_Receive(BSP_UART_GetHandle(BSP_UART_RIGHT_LEG),
-                            (uint8_t *)&raw_right_buff, 16, 5);
-        }
-        
-    }
-    return DEVICE_OK;
-}
-
-int8_t GO_StoreMsg(GO_t *go, RIS_MotorData_t *go_rx){
-
-    return DEVICE_OK;
-}

User/device/go.c

@@ -47,6 +47,7 @@ int8_t GO_Motor_Control(GO_t *go){
         go->motor.output.id[i].mode = GO_MOTOR_MODE_FOC; // 设置电机模式
         modify_data(&go->motor.output.id[i]); 
         if (i < GO_MOTOR_NUM / 4 || i >= GO_MOTOR_NUM/2 && i < GO_MOTOR_NUM * 3 / 4) {
+        // if (i < GO_MOTOR_NUM / 4) {
             HAL_UART_Transmit(BSP_UART_GetHandle(go->uart_param->left_leg), 
                               (uint8_t *)&go->motor.output.id[i].motor_send_data, 
                               sizeof(go->motor.output.id[i].motor_send_data), 1);

User/device/n100.c

@@ -0,0 +1,156 @@
+/*
+    N100 陀螺仪数据
+*/
+
+/* Includes ----------------------------------------------------------------- */
+#include "n100.h"
+
+#include <string.h>
+#include "device/device.h"
+
+#include "bsp/uart.h"
+#include "component/user_math.h"
+#include "component/crc8.h"
+#include "component/crc16.h"
+/* Private define ----------------------------------------------------------- */
+#define RX_BUFF_SIZE 56
+#define N100_BAG_HEAD 0xFC
+#define N100_BAG_END 0xFD
+// #define RX_BUFF_SIZE (sizeof(N100_AHRS_t))
+/* Private macro ------------------------------------------------------------ */
+/* Private typedef ---------------------------------------------------------- */
+/* Private variables -------------------------------------------------------- */
+static volatile uint32_t drop_message = 0;
+
+static uint8_t rxbuf[RX_BUFF_SIZE] __attribute__ ((section(".AXI_SRAM")));
+
+// static N100_AHRS_t rxbuf __attribute__((section(".AXI_SRAM")));
+
+static bool inited = false;
+
+static osThreadId_t thread_alert;
+
+/* Private function  -------------------------------------------------------- */
+
+static void N100_RxCpltCallback(void) {
+  osThreadFlagsSet(thread_alert, SIGNAL_N100_NEW_DATA);
+}
+
+static void N100_ErrorCallback(void) {
+  N100_Restart();
+}
+
+static void N100_Decode(N100_t *n100, const uint8_t *rxbuf) {
+    if (n100 == NULL || rxbuf == NULL) return;
+
+    size_t offset = 0;
+
+    // header
+    n100->rx_raw.header.head   = rxbuf[offset++];
+    n100->rx_raw.header.cmd    = rxbuf[offset++];
+    n100->rx_raw.header.len    = rxbuf[offset++];
+    n100->rx_raw.header.count  = rxbuf[offset++];
+    n100->rx_raw.header.crc8   = rxbuf[offset++];
+
+    // data.crc16
+    n100->rx_raw.data.crc16 = (uint16_t)(rxbuf[offset] | (rxbuf[offset+1] << 8));
+    offset += 2;
+
+    //都是float32
+    memcpy(&n100->rx_raw.data.gyro.x, rxbuf + offset, 4);
+    offset += 4;
+    memcpy(&n100->rx_raw.data.gyro.y, rxbuf + offset, 4);
+    offset += 4;
+    memcpy(&n100->rx_raw.data.gyro.z, rxbuf + offset, 4);
+    offset += 4;
+
+    memcpy(&n100->rx_raw.data.eulr.rol, rxbuf + offset, 4);
+    offset += 4;
+    memcpy(&n100->rx_raw.data.eulr.pit, rxbuf + offset, 4);
+    offset += 4;
+    memcpy(&n100->rx_raw.data.eulr.yaw, rxbuf + offset, 4);
+    offset += 4;
+    // data.quat
+    memcpy(&n100->rx_raw.data.quat, rxbuf + offset, sizeof(AHRS_Quaternion_t));
+    offset += sizeof(AHRS_Quaternion_t);
+
+    // data.time
+    memcpy(&n100->rx_raw.data.time, rxbuf + offset, sizeof(int64_t));
+    offset += sizeof(int64_t);
+
+    // end
+    n100->rx_raw.end = rxbuf[offset];
+}
+
+// 直接对原始数据 rxbuf 校验
+static bool N100_Verify(const uint8_t *rxbuf) {
+    if (rxbuf == NULL) return false;
+    // 检查包头包尾
+    if (rxbuf[0] != N100_BAG_HEAD || rxbuf[RX_BUFF_SIZE - 1] != N100_BAG_END) {
+        drop_message++;
+        return false;
+    }
+    // // CRC8 校验（包头4字节）
+    // if (rxbuf[4] != crc8_calc(rxbuf, 4)) {
+    //     drop_message++;
+    //     return false;
+    // }
+    // // CRC16 校验（数据段，从第7字节开始，长度为RX_BUFF_SIZE-8）
+    // uint16_t crc16 = (uint16_t)(rxbuf[5] | (rxbuf[6] << 8));
+    // if (crc16 != crc16_calc(rxbuf + 7, RX_BUFF_SIZE - 8)) {
+    //     drop_message++;
+    //     return false;
+    // }
+    return true;
+}
+
+/* Exported functions ------------------------------------------------------- */
+int8_t N100_Init(N100_t *n100){
+    ASSERT(n100);
+    if (inited) return DEVICE_ERR_INITED;
+    if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;
+    
+    BSP_UART_RegisterCallback(BSP_UART_IMU_N100, BSP_UART_RX_CPLT_CB, N100_RxCpltCallback);
+    BSP_UART_RegisterCallback(BSP_UART_IMU_N100, BSP_UART_ERROR_CB, N100_ErrorCallback);
+    inited = true;
+    return DEVICE_OK;
+}
+
+int8_t N100_Restart(void) {
+  __HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_IMU_N100));
+  __HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_IMU_N100));
+  return DEVICE_OK;
+}
+
+int8_t N100_StartReceiving(N100_t *n100) {
+  if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_IMU_N100), rxbuf, RX_BUFF_SIZE) != HAL_OK)
+    return DEVICE_ERR;
+  return DEVICE_OK;
+}
+
+bool N100_WaitDmaCplt(void) {
+  return (osThreadFlagsWait(SIGNAL_N100_NEW_DATA, osFlagsWaitAll, 200) ==
+          SIGNAL_N100_NEW_DATA);
+}
+
+int8_t N100_ParseData (N100_t *n100) {
+    if (n100 == NULL) return DEVICE_ERR_NULL;
+    // 先校验原始数据
+    if (!N100_Verify(rxbuf)) {
+        return DEVICE_ERR;
+    }
+    // 校验通过后再解码
+    N100_Decode(n100, rxbuf);
+    n100->eulr = n100->rx_raw.data.eulr;
+    n100->gyro = n100->rx_raw.data.gyro;
+    n100->quat = n100->rx_raw.data.quat;
+
+    return DEVICE_OK;
+}
+
+int8_t N100_HandleOffline(N100_t *n100) {
+  if (n100 == NULL) return DEVICE_ERR_NULL;
+    /*清空n100结构体*/
+    memset(n100, 0, sizeof(*n100));
+  return DEVICE_OK;
+}

User/device/n100.h

@@ -0,0 +1,63 @@
+/*
+  N100 陀螺仪数据
+*/
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include <cmsis_os2.h>
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "component/ahrs.h"
+
+/* Exported constants ------------------------------------------------------- */
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+
+typedef struct {
+    struct {
+        uint8_t head;
+        uint8_t cmd;
+        uint8_t len;
+        uint8_t count;
+        uint8_t crc8;
+    } header; /* 数据包头部 */
+    struct {
+        uint16_t crc16;
+        AHRS_Gyro_t gyro; /* 陀螺仪数据 */
+        AHRS_Eulr_t eulr; /* 欧拉角数据 */
+        AHRS_Quaternion_t quat; /* 四元数数据 */
+        int64_t time; /* 时间戳 */
+    } data; /* 数据包内容 */
+    uint8_t end;
+} N100_AHRS_t;
+
+
+typedef struct {
+  osThreadId_t thread_alert;
+  N100_AHRS_t rx_raw;
+  AHRS_Gyro_t gyro; /* 陀螺仪数据 */
+  AHRS_Eulr_t eulr; /* 欧拉角数据 */
+  AHRS_Quaternion_t quat; /* 四元数数据 */
+} N100_t;
+
+/* Exported functions prototypes -------------------------------------------- */
+int8_t N100_Init(N100_t *n100);
+int8_t N100_Restart(void);
+
+int8_t N100_StartReceiving(N100_t *n100);
+bool N100_WaitDmaCplt(void);
+int8_t N100_ParseData (N100_t *n100);
+int8_t N100_HandleOffline(N100_t *n100);
+
+#ifdef __cplusplus
+}
+#endif
+
+
+

User/module/chassis.c

@@ -10,12 +10,13 @@
 #include "cmsis_os2.h"
 // #include "component/limiter.h"
 #include "device/go.h"
-
+#include "component/limiter.h"
 /* Private typedef ---------------------------------------------------------- */
 /* Private define ----------------------------------------------------------- */
 
 /*底盘默认高度*/
 #define CHASSIS_DEFAULT_HEIGHT (0.2f) /* 底盘默认高度，单位：米 */
+#define CHASSIS_MAX_SPEED (0.03f)
 /* Private macro ------------------------------------------------------------ */
 
 
@@ -151,6 +152,7 @@ int8_t Chassis_Control(Chassis_t *c, const CMD_ChassisCmd_t *c_cmd, uint32_t now
             // c->output.id[i] = damping_torque; /* 设置阻尼力矩 */
             c->output.id[i] = position_mode; /* 设置位置模式 */
           }
+
           // c->output.named.lf_thigh.Pos = 1.0f; /* 左前腿大腿位置 */
           // c->output.named.lf_calf.Pos = 2.0f; /* 左前腿小腿位置 */
           // c->output.named.rf_thigh.Pos = -1.0f; /* 右前腿大腿位置 */
@@ -171,6 +173,11 @@ int8_t Chassis_Control(Chassis_t *c, const CMD_ChassisCmd_t *c_cmd, uint32_t now
         }
         break;
       }
+      c->output.named.lf_hip.Pos = 0.0f; /* 左前腿髋关节位置 */
+      c->output.named.lr_hip.Pos = 0.0f; /* 左后腿髋关节位置 */
+      c->output.named.rf_hip.Pos = 0.0f; /* 右前腿髋关节位置 */
+      c->output.named.rr_hip.Pos = 0.0f; /* 右后腿髋关节位置 */
+
       c->height = c->height + c_cmd->delta_hight * c->dt; /* 更新底盘高度 */
       c->output.named.lf_thigh.Pos = c->height * 5;
       c->output.named.lf_calf.Pos = c->height * 10;
@@ -181,8 +188,35 @@ int8_t Chassis_Control(Chassis_t *c, const CMD_ChassisCmd_t *c_cmd, uint32_t now
       c->output.named.rr_thigh.Pos = -c->height * 5;
       c->output.named.rr_calf.Pos = -c->height * 10;
       chassis_mode_states = CHASSIS_MODE_RECOVER; /* 更新状态 */
+
+      for (uint8_t i = 0; i < 4; i++) {
+        float target_pose[3] = {0.0,0.0,-0.2}; /* 目标位置 */
+        float angle_pose[3];
+        Kinematics_InverseKinematics(target_pose, &c->param->mech_param.length, angle_pose, &c->param->mech_param.sign.leg[i]); /* 逆运动学计算 */
+        c->output.id[i * 3].Pos = angle_pose[0]; /* 髋关节位置 */
+        c->output.id[i * 3 + 1].Pos = angle_pose[1]; /* 大腿位置 */
+        c->output.id[i * 3 + 2].Pos = angle_pose[2]; /* 小腿位置 */
+      }
+      // float target_pose[3] = {c_cmd->ctrl_vec.vx/10.0f, c_cmd->ctrl_vec.vy/10.0f, -0.2f}; /* 目标位置 */
+      // float target_pose[3] = {0.0,0.0,-0.2}; /* 目标位置 */
+      // float angle_pose[3];
+      // Kinematics_InverseKinematics(target_pose, &c->param->mech_param.length, angle_pose, &c->param->mech_param.sign.leg[0]); /* 逆运动学计算 */
+      // c->output.named.lf_hip.Pos = angle_pose[0]; /* 左前腿髋关节位置 */
+      // c->output.named.lf_thigh.Pos = angle_pose[1]; /* 左前腿大腿位置 */
+      // c->output.named.lf_calf.Pos = angle_pose[2]; /* 左前腿小腿位置 */
+
       break;
   }
+
+  // Limit_ChassicOutput(&c->feedback, &c->output, 0.5, 0.0, 0.0);
+  for (uint8_t i = 0; i < GO_MOTOR_NUM; i++) {
+    /* 限制输出 */
+    Limit_ChassicOutput(c->feedback.id[i].Pos, &c->output.id[i].Pos,
+                        c->param->mech_param.ratio.id[i] * CHASSIS_MAX_SPEED,
+                        c->param->mech_param.limit.max.id[i],
+                        c->param->mech_param.limit.min.id[i]);
+  }
+
 }
 
 void Chassis_DumpOutput(const Chassis_t *c, GO_ChassisCMD_t *out){

User/module/chassis.h

@@ -11,6 +11,7 @@ extern "C" {
 /* Includes ----------------------------------------------------------------- */
 #include "component/cmd.h"
 #include "component/filter.h"
+#include "component/kinematics.h"
 #include "component/pid.h"
 #include "device/go.h"
 
@@ -30,6 +31,13 @@ typedef enum {
   CHASSIS_TYPE_QUADRUPED,   /* 四足机器人 */
 } Chassis_Type_t;
 
+typedef enum {
+  IMU_BOARD_BMI088,
+  IMU_WHEELREC_N100,
+  IMU_NUM,
+  IMU_ERROR,
+} Chassis_ImuType_t;
+
 // typedef union {
 //     float id[GO_MOTOR_NUM]; /* 零点角度，单位：弧度 */
 //     struct {
@@ -66,46 +74,32 @@ typedef union {
   } named;
 } joint_params;
 
-// /*关节减速比*/
+typedef struct {
-// typedef union {
+  joint_params min;
-//   float id[GO_MOTOR_NUM];
+  joint_params max; /* 关节的最小和最大角度，单位：弧度 */
-//   struct{
+} joint_limits;
-//     float lf_hip;   /* 左前腿髋关节长度，单位：米 */
+
-//     float lf_thigh; /* 左前腿大腿长度，单位：米 */
+typedef union {
-//     float lf_calf;  /* 左前腿小腿长度，单位：米 */
+  Kinematics_Sign_t leg[GO_MOTOR_NUM/3];
-//     float rf_hip;   /* 右前腿髋关节长度，单位：米 */
+  struct {
-//     float rf_thigh; /* 右前腿大腿长度，单位：米 */
+    Kinematics_Sign_t lf; /* 左前腿 */
-//     float rf_calf;  /* 右前腿小腿长度，单位：米 */
+    Kinematics_Sign_t rf; /* 右前腿 */
-//     float lr_hip;   /* 左后腿髋关节长度，单位：米 */
+    Kinematics_Sign_t lr; /* 左后腿 */
-//     float lr_thigh; /* 左后腿大腿长度，单位：米 */
+    Kinematics_Sign_t rr; /* 右后腿 */
-//     float lr_calf;  /* 左后腿小腿长度，单位：米 */
+  } named;
-//     float rr_hip;   /* 右后腿髋关节长度，单位：米 */
+} Kinematics_DirectionSign_t;
-//     float rr_thigh; /* 右后腿大腿长度，单位：米 */
-//     float rr_calf;  /* 右后腿小腿长度，单位：米 */
-//   } named;
-// } joint_ratio;
 
 
-
-    // struct {
-    //     float x; /* 机械中心X坐标，单位：米 */
-    //     float y; /* 机械中心Y坐标，单位：米 */
-    //     float z; /* 机械中心Z坐标，单位：米 */
-    // } mech_center; /* 机械中心位置 */
-
-
-
-    // typedef struct {
-    // /*三个关节距离*/
 typedef struct{
   joint_params zero_point; /* 零点角度，单位：弧度 */
-  joint_params length;     /* 关节长度，单位：米 */
+
+  // joint_params length;     /* 关节长度，单位：米 */
+ 
   joint_params ratio;      /* 关节减速比 */
-  struct {
+  joint_limits limit; /* 关节的最小和最大角度，单位：弧度 */
-    float x; /* 机械中心X坐标，单位：米 */
+  Kinematics_LinkLength_t length; /* 关节长度，单位：米 */
-    float y; /* 机械中心Y坐标，单位：米 */
+  Kinematics_LegOffset_t offset; /* 关节偏移，单位：米 */
-    float z; /* 机械中心Z坐标，单位：米 */
+  Kinematics_DirectionSign_t sign; /* 关节侧向标志（左前/左后为-1，右前/右后为1） */
-  } mech_center; /* 机械中心位置 */
     
 }Chassis_Mech_Params_t;
 

User/module/config.c

@@ -1,116 +0,0 @@
-/*
- * 配置相关
- */
-
-#include "config.h"
-
-#include <stdint.h>
-#include <string.h>
-
-#include "bsp/flash.h"
-
-#define CONFIG_BASE_ADDRESS (ADDR_FLASH_SECTOR_11)
-
-Config_t param_default = {
-    
-    .uart = {
-        .left_leg = BSP_UART_LEFT_LEG,
-        .right_leg = BSP_UART_RIGHT_LEG,
-    },
-    .cali = {
-        .bmi088 = {
-            .gyro_offset = {0.0f, 0.0f, 0.0f},
-        },
-    },
-    .chassis = {
-        .type = CHASSIS_TYPE_QUADRUPED,
-
-        .torque_pid_param = {
-            .k = 1.0f, /* 控制器增益 */
-            .p = 1.0f, /* 比例项增益 */
-            .i = 0.0f, /* 积分项增益 */
-            .d = 0.0f, /* 微分项增益 */
-            .i_limit = 0.0f, /* 积分项上限 */
-            .out_limit = 0.0f, /* 输出绝对值限制 */
-            .d_cutoff_freq = 10.0f, /* D项低通截止频率 */
-            .range = -1.0f, /* 计算循环误差时使用，大于0时启用 */
-        },
-        
-        .low_pass_cutoff_freq = {
-            .in = -1.0f,
-            .out = -1.0f,
-        },
-
-        .mech_param = {
-            .length = {
-                .named = {
-                    .lf_hip = 0.0861f,   /* 左前腿髋关节长度，单位：米 */
-                    .lf_thigh = 0.20f, /* 左前腿大腿长度，单位：米 */
-                    .lf_calf = 0.20f,  /* 左前腿小腿长度，单位：米 */
-                    .rf_hip = 0.0861f,   /* 右前腿髋关节长度，单位：米 */
-                    .rf_thigh = 0.20f, /* 右前腿大腿长度，单位：米 */
-                    .rf_calf = 0.20f,  /* 右前腿小腿长度，单位：米 */
-                    .lr_hip = 0.0861f,   /* 左后腿髋关节长度，单位：米 */
-                    .lr_thigh = 0.20f, /* 左后腿大腿长度，单位：米 */
-                    .lr_calf = 0.20f,  /* 左后腿小腿长度，单位：米 */
-                    .rr_hip = 0.0861f,   /* 右后腿髋关节长度，单位：米 */
-                    .rr_thigh = 0.20f, /* 右后腿大腿长度，单位：米 */
-                    .rr_calf = 0.20f,  /* 右后腿小腿长度，单位：米 */
-                }
-            },
-            .zero_point = {
-                .named = {
-                    .lf_hip = 1.98f,   /* 左前腿髋关节零点角度，单位：弧度 */
-                    .lf_thigh = -2.86f, /* 左前腿大腿零点角度，单位：弧度 */
-                    .lf_calf = -32.367f,  /* 左前腿小腿零点角度，单位：弧度 */
-                    .rf_hip = 5.56f,   /* 右前腿髋关节零点角度，单位：弧度 */
-                    .rf_thigh = 12.92f, /* 右前腿大腿零点角度，单位：弧度 */
-                    .rf_calf = 37.047f,  /* 右前腿小腿零点角度，单位：弧度 */
-                    .lr_hip = 4.76f,   /* 左后腿髋关节零点角度，单位：弧度 */
-                    .lr_thigh = -4.57f, /* 左后腿大腿零点角度，单位：弧度 */
-                    .lr_calf = -35.847f,  /* 左后腿小腿零点角度，单位：弧度 */
-                    .rr_hip = 2.42f,   /* 右后腿髋关节零点角度，单位：弧度 */
-                    .rr_thigh = 8.85f, /* 右后腿大腿零点角度，单位：弧度 */
-                    .rr_calf = 38.55f,  /* 右后腿小腿零点角度，单位：弧度 */
-                }
-            },
-            .ratio = {
-                .named = {
-                    .lf_hip = 6.33f,   /* 左前腿髋关节减速比 */
-                    .lf_thigh = 6.33f, /* 左前腿大腿减速比 */
-                    .lf_calf = 12.66f, /* 左前腿小腿减速比 */
-                    .rf_hip = 6.33f,   /* 右前腿髋关节减速比 */
-                    .rf_thigh = 6.33f, /* 右前腿大腿减速比 */
-                    .rf_calf = 12.66f, /* 右前腿小腿减速比 */
-                    .lr_hip = 6.33f,   /* 左后腿髋关节减速比 */
-                    .lr_thigh = 6.33f, /* 左后腿大腿减速比 */
-                    .lr_calf = 12.66f, /* 左后腿小腿减速比 */
-                    .rr_hip = 6.33f,   /* 右后腿髋关节减速比 */
-                    .rr_thigh = 6.33f, /* 右后腿大腿减速比 */
-                    .rr_calf = 12.66f, /* 右后腿小腿减速比 */
-                }
-            },
-            .mech_center = {
-                .x = 0.0f, /* 机械中心X坐标，单位：米 */
-                .y = 0.0f, /* 机械中心Y坐标，单位：米 */
-                .z = 0.0f, /* 机械中心Z坐标，单位：米 */
-            }, /* 机械中心位置 */
-
-        },
-
-
-    },
-}; /* param_default */
-
-
-
-/**
- * \brief 获取配置信息
- *
- * \param cfg 配置信息
- */
-void Config_Get(Config_t *cfg) {
-    //直接把param_default传进去
-    if (cfg == NULL) return; // 检查参数是否为NULL
-    memcpy(cfg, &param_default, sizeof(Config_t)); // 复制默认配置到cfg
-}

User/module/config.h

@@ -37,6 +37,7 @@ typedef struct {
   struct {
     BMI088_Cali_t bmi088;
   } cali; /* 校准 */
+  Chassis_ImuType_t chassis_imu_type; /* 底盘IMU类型 */
   
 } Config_t;
 

User/task/atti_esti.c

@@ -7,9 +7,11 @@
 #include "task/user_task.h"
 /* USER INCLUDE BEGIN*/
 #include "bsp/pwm.h"
+#include "bsp/gpio.h"
 #include "component/ahrs.h"
 #include "component/pid.h"
 #include "device/bmi088.h"
+#include "device/n100.h"
 #include "task/user_task.h"
 /* USER INCLUDE END*/
 
@@ -19,6 +21,8 @@
 /* Private variables -------------------------------------------------------- */
 /* USER STRUCT BEGIN*/
 BMI088_t bmi088;
+N100_t n100;
+
 BMI088_Cali_t bmi088_cali = {
   .gyro_offset = {0.0f, 0.0f, 0.0f},
 }; /* BMI088校准数据 */
@@ -48,44 +52,81 @@ void Task_atti_esti(void *argument) {
   osDelay(ATTI_ESTI_INIT_DELAY); /* 延时一段时间再开启任务 */
 
   /* USER CODE INIT BEGIN*/
-  BMI088_Init(&bmi088, &bmi088_cali); /* 初始化BMI088传感器 */
+  switch (task_runtime.config.chassis_imu_type) {
-  AHRS_Init(&gimbal_ahrs, &mage, BMI088_GetUpdateFreq(&bmi088)); /* 初始化姿态解算算法 */
+    case IMU_BOARD_BMI088:
-    /* 初始化IMU温度控制PID，防止温漂 */
+      BMI088_Init(&bmi088, &bmi088_cali); /* 初始化BMI088传感器 */
-  PID_Init(&imu_temp_ctrl_pid, KPID_MODE_NO_D,
+      AHRS_Init(&gimbal_ahrs, &mage, BMI088_GetUpdateFreq(&bmi088)); /* 初始化姿态解算算法 */
-           1.0f / BMI088_GetUpdateFreq(&bmi088), &imu_temp_ctrl_pid_param);
+      /* 初始化IMU温度控制PID，防止温漂 */
-  // BSP_PWM_Start(BSP_PWM_IMU_HEAT);
+      // 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);
+      break;
+    case IMU_WHEELREC_N100:
+      N100_Init(&n100);
+      BSP_GPIO_SetPin(BSP_GPIO_5V_EN, BSP_GPIO_ON); /* 打开5V电源 */
+      break;
+    default:
+      break;
+  }
   /* USER CODE INIT END*/
 
   while (1) {
     /* USER CODE BEGIN */
-    /* 等待IMU新数据 */
+    switch (task_runtime.config.chassis_imu_type) {
-    BMI088_WaitNew();
+      case IMU_BOARD_BMI088:
+              /* 等待IMU新数据 */
+        BMI088_WaitNew();
+        /* 开始数据接收DMA，加速度计和陀螺仪共用同一个SPI接口，
+        * 一次只能开启一个DMA
+        */
+        BMI088_AcclStartDmaRecv();
+        BMI088_AcclWaitDmaCplt();
 
-    /* 开始数据接收DMA，加速度计和陀螺仪共用同一个SPI接口，
+        BMI088_GyroStartDmaRecv();
-     * 一次只能开启一个DMA
+        BMI088_GyroWaitDmaCplt();
-     */
-    BMI088_AcclStartDmaRecv();
-    BMI088_AcclWaitDmaCplt();
 
-    BMI088_GyroStartDmaRecv();
+        /* 锁住RTOS内核防止数据解析过程中断，造成错误 */
-    BMI088_GyroWaitDmaCplt();
+        osKernelLock();
+        /* 接收完所有数据后，把数据从原始字节加工成方便计算的数据 */
+        BMI088_ParseAccl(&bmi088);
+        BMI088_ParseGyro(&bmi088);
+
+        /* 根据设备接收到的数据进行姿态解析 */
+        AHRS_Update(&gimbal_ahrs, &bmi088.accl, &bmi088.gyro, &mage);
+
+        /* 根据解析出来的四元数计算欧拉角 */
+        AHRS_GetEulr(&eulr_to_send, &gimbal_ahrs);
+        osKernelUnlock();
+        /* 将欧拉角数据放入消息队列 */
+        osMessageQueueReset(task_runtime.msgq.body.eulr_imu);
+        osMessageQueuePut(task_runtime.msgq.body.eulr_imu, &eulr_to_send, 0, 0); /* 将欧拉角数据放入消息队列 */
+
+        /* PID控制IMU温度，PWM输出 */
+        // BSP_PWM_Set(BSP_PWM_IMU_HEAT,
+        //             PID_Calc(&imu_temp_ctrl_pid, 40.0f, bmi088.temp, 0.0f, 0.01f));
+        break;
+      case IMU_WHEELREC_N100:
+        N100_StartReceiving(&n100);
+        if (N100_WaitDmaCplt()) {
+          osKernelLock();
+          N100_ParseData(&n100);
+          osKernelUnlock();
+        } else {
+          N100_HandleOffline(&n100);
+        }
+        osMessageQueueReset(task_runtime.msgq.body.eulr_imu); /* 重置消息队列 */
+        osMessageQueuePut(task_runtime.msgq.body.eulr_imu, &n100.eulr, 0, 0); /* 将欧拉角数据放入消息队列 */
+        
+        break;
+      default:
+        
+        break;
+        
+    }
 
-    /* 锁住RTOS内核防止数据解析过程中断，造成错误 */
-    osKernelLock();
-    /* 接收完所有数据后，把数据从原始字节加工成方便计算的数据 */
-    BMI088_ParseAccl(&bmi088);
-    BMI088_ParseGyro(&bmi088);
 
-    /* 根据设备接收到的数据进行姿态解析 */
-    AHRS_Update(&gimbal_ahrs, &bmi088.accl, &bmi088.gyro, &mage);
 
-    /* 根据解析出来的四元数计算欧拉角 */
-    AHRS_GetEulr(&eulr_to_send, &gimbal_ahrs);
-    osKernelUnlock();
 
-    /* PID控制IMU温度，PWM输出 */
-    // BSP_PWM_Set(BSP_PWM_IMU_HEAT,
-    //             PID_Calc(&imu_temp_ctrl_pid, 40.0f, bmi088.temp, 0.0f, 0.01f));
     /* USER CODE END */
   }
   

User/task/config.yaml

@@ -37,14 +37,14 @@
   function: Task_cmd
   name: cmd
   stack: 256
-- delay: 0
+- delay: 200
   description: 四足控制
   freq_control: true
   frequency: 800
   function: Task_ctrl_leg
   name: ctrl_leg
   stack: 512
-- delay: 0
+- delay: 200
   description: 底盘控制，接受反馈和命令，计算输出给到两测腿控制
   freq_control: true
   frequency: 500

User/task/ctrl_leg.c

@@ -40,9 +40,7 @@ void Task_ctrl_leg(void *argument) {
   uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
   while (1) {
     tick += delay_tick; /* 计算下一个唤醒时刻 */
-
     /* USER CODE BEGIN */
-    // osMessageQueueGet(task_runtime.msgq.chassis.output, &cmd, NULL, 0); /* 从消息队列中获取底盘控制命令 */
     if (osMessageQueueGet(task_runtime.msgq.chassis.output, &go_cmd, NULL, 0) == osOK) { /* 从消息队列中获取底盘控制命令 */
       GO_ParseCmd(&go, &go_cmd); /* 解析底盘控制命令 */
     } 

User/task/init.c

@@ -45,6 +45,7 @@ void Task_Init(void *argument) {
   task_runtime.msgq.cmd.chassis = osMessageQueueNew(2u, sizeof(CMD_ChassisCmd_t), NULL);
   task_runtime.msgq.chassis.feefback = osMessageQueueNew(2u, sizeof(GO_ChassisFeedback_t), NULL);
   task_runtime.msgq.chassis.output = osMessageQueueNew(2u, sizeof(GO_ChassisCMD_t), NULL);
+  task_runtime.msgq.body.eulr_imu = osMessageQueueNew(2u, sizeof(AHRS_Eulr_t), NULL);
   /* USER MESSAGE END */
 
   osKernelUnlock(); // 解锁内核

User/task/user_task.h

@@ -16,7 +16,7 @@ extern "C" {
 #define TEST_FREQ (1000)
 #define MONITOR_FREQ (100)
 #define CMD_FREQ (500)
-#define CTRL_LEG_FREQ (500)
+#define CTRL_LEG_FREQ (800)
 #define CTRL_CHASSIS_FREQ (500)
 
 /* 任务初始化延时ms */
@@ -27,8 +27,8 @@ extern "C" {
 #define CLI_INIT_DELAY (0)
 #define MONITOR_INIT_DELAY (0)
 #define CMD_INIT_DELAY (0)
-#define CTRL_LEG_INIT_DELAY (0)
+#define CTRL_LEG_INIT_DELAY (200)
-#define CTRL_CHASSIS_INIT_DELAY (0)
+#define CTRL_CHASSIS_INIT_DELAY (200)
 
 /* Exported defines --------------------------------------------------------- */
 /* Exported macro ----------------------------------------------------------- */
@@ -51,6 +51,10 @@ typedef struct {
     /* USER MESSAGE BEGIN */
     struct {
         // osMessageQueueId_t user_msg; /* 用户自定义任务消息队列 */
+
+        struct {
+            osMessageQueueId_t eulr_imu;
+        } body;
         struct {
             struct {
                 osMessageQueueId_t rc; /* 原始数据消息队列 */

Utils/Kinematics.m

@@ -0,0 +1,221 @@
+function quadruped_leg_visualization()
+    % 四足机器人单腿运动学可视化
+    
+    % 清除之前的图形
+    close all;
+    clc;
+    
+    % 定义腿部参数 (单位: 米)
+    leg_params.upper_leg_length = 0.15;
+    leg_params.lower_leg_length = 0.15;
+    leg_params.hip_offset_x = 0.05;
+    leg_params.hip_offset_z = -0.02;
+    leg_params.hip_yaw_offset = 0.01;
+    leg_params.knee_sign = 1;  % 1或-1，取决于机械设计
+    
+    % 目标位置 (在髋关节坐标系中)
+    target_pos = [0.2, 0.1, -0.25];  % x, y, z
+    
+    % 计算逆运动学 (右腿hip_yaw_sign=1，左腿=-1)
+    joint_angles = leg_inverse_kinematics(target_pos, leg_params, 1);
+    
+    % 显示计算结果
+    disp('关节角度 (弧度):');
+    disp(['髋关节偏航 (hip yaw): ', num2str(joint_angles(1)), ' rad']);
+    disp(['髋关节滚动 (hip roll): ', num2str(joint_angles(2)), ' rad']);
+    disp(['髋关节俯仰 (hip pitch): ', num2str(joint_angles(3)), ' rad']);
+    disp(['膝关节 (knee): ', num2str(joint_angles(4)), ' rad']);
+    
+    disp('关节角度 (度):');
+    disp(['髋关节偏航 (hip yaw): ', num2str(rad2deg(joint_angles(1))), '°']);
+    disp(['髋关节滚动 (hip roll): ', num2str(rad2deg(joint_angles(2))), '°']);
+    disp(['髋关节俯仰 (hip pitch): ', num2str(rad2deg(joint_angles(3))), '°']);
+    disp(['膝关节 (knee): ', num2str(rad2deg(joint_angles(4))), '°']);
+    
+    % 可视化腿部
+    visualize_leg(joint_angles, leg_params, target_pos);
+end
+
+function joint_angles = leg_inverse_kinematics(target_pos, leg_params, hip_yaw_sign)
+    % 逆运动学计算函数
+    
+    % 从参数字典中提取参数
+    L1 = leg_params.upper_leg_length;
+    L2 = leg_params.lower_leg_length;
+    offset_x = leg_params.hip_offset_x;
+    offset_z = leg_params.hip_offset_z;
+    yaw_offset = leg_params.hip_yaw_offset;
+    knee_sign = leg_params.knee_sign;
+    
+    % 第一步：计算髋关节偏航角 (hip yaw)
+    x = target_pos(1);
+    y = target_pos(2);
+    z = target_pos(3);
+    
+    x = x - offset_x;
+    z = z - offset_z;
+    
+    % 使用迭代法处理yaw offset
+    theta_yaw = atan2(hip_yaw_sign * y, x);
+    for i = 1:3
+        % 计算yaw旋转后的位置
+        x_rot = x * cos(theta_yaw) + hip_yaw_sign * y * sin(theta_yaw);
+        y_rot = -x * sin(theta_yaw) + hip_yaw_sign * y * cos(theta_yaw);
+        
+        % 考虑yaw offset
+        x_rot = x_rot - yaw_offset * (1 - cos(theta_yaw));
+        y_rot = y_rot + yaw_offset * sin(theta_yaw);
+        
+        theta_yaw = atan2(hip_yaw_sign * y_rot, x_rot);
+    end
+    
+    % 第二步：计算髋关节滚转角 (hip roll) 和俯仰角 (hip pitch)
+    x_rot = x * cos(theta_yaw) + hip_yaw_sign * y * sin(theta_yaw) - yaw_offset;
+    z_rot = z;
+    
+    % 计算髋关节滚转角
+    L = sqrt(x_rot^2 + z_rot^2);
+    theta_roll = atan2(z_rot, x_rot);
+    
+    % 计算髋关节俯仰角和膝关节角度
+    D = (L^2 - L1^2 - L2^2) / (2 * L1 * L2);
+    D = max(min(D, 1.0), -1.0);  % 确保数值在有效范围内
+    
+    theta_knee = knee_sign * acos(D);
+    
+    alpha = atan2(L2 * sin(theta_knee), L1 + L2 * cos(theta_knee));
+    theta_pitch = alpha + theta_roll;
+    
+    % 返回关节角度 (hip_yaw, hip_roll, hip_pitch, knee)
+    joint_angles = [theta_yaw, theta_roll, theta_pitch, theta_knee];
+end
+
+function visualize_leg(joint_angles, leg_params, target_pos)
+    % 可视化腿部运动学
+    
+    % 提取参数
+    L1 = leg_params.upper_leg_length;
+    L2 = leg_params.lower_leg_length;
+    offset_x = leg_params.hip_offset_x;
+    offset_z = leg_params.hip_offset_z;
+    yaw_offset = leg_params.hip_yaw_offset;
+    
+    % 提取关节角度
+    theta_yaw = joint_angles(1);
+    theta_roll = joint_angles(2);
+    theta_pitch = joint_angles(3);
+    theta_knee = joint_angles(4);
+    
+    % 创建图形窗口
+    figure('Name', '四足机器人单腿运动学可视化', 'NumberTitle', 'off', 'Position', [100, 100, 1200, 800]);
+    
+    % 1. 3D视图
+    subplot(2,2,[1,3]);
+    hold on;
+    grid on;
+    axis equal;
+    view(3);
+    xlabel('X (m)');
+    ylabel('Y (m)');
+    zlabel('Z (m)');
+    title('3D视图');
+    
+    % 绘制坐标系
+    plot3([0, 0.1], [0, 0], [0, 0], 'r', 'LineWidth', 2); % X轴
+    plot3([0, 0], [0, 0.1], [0, 0], 'g', 'LineWidth', 2); % Y轴
+    plot3([0, 0], [0, 0], [0, 0.1], 'b', 'LineWidth', 2); % Z轴
+    
+    % 计算关节位置
+    % 髋关节位置 (考虑了offset)
+    hip_pos = [offset_x, 0, offset_z];
+    
+    % 应用yaw旋转
+    R_yaw = [cos(theta_yaw), -sin(theta_yaw), 0;
+             sin(theta_yaw), cos(theta_yaw), 0;
+             0, 0, 1];
+    
+    % 应用roll旋转 (在yaw旋转后的坐标系中)
+    R_roll = [cos(theta_roll), 0, sin(theta_roll);
+              0, 1, 0;
+              -sin(theta_roll), 0, cos(theta_roll)];
+    
+    % 应用pitch旋转
+    R_pitch = [1, 0, 0;
+               0, cos(theta_pitch), -sin(theta_pitch);
+               0, sin(theta_pitch), cos(theta_pitch)];
+    
+    % 计算大腿末端位置
+    upper_leg_end = hip_pos + (R_yaw * R_roll * R_pitch * [L1; 0; 0])';
+    
+    % 计算小腿末端位置
+    R_knee = [1, 0, 0;
+              0, cos(theta_knee), -sin(theta_knee);
+              0, sin(theta_knee), cos(theta_knee)];
+    
+    lower_leg_end = upper_leg_end + (R_yaw * R_roll * R_pitch * R_knee * [L2; 0; 0])';
+    
+    % 绘制连杆
+    plot3([hip_pos(1), upper_leg_end(1)], ...
+          [hip_pos(2), upper_leg_end(2)], ...
+          [hip_pos(3), upper_leg_end(3)], 'b-o', 'LineWidth', 3, 'MarkerSize', 8);
+    
+    plot3([upper_leg_end(1), lower_leg_end(1)], ...
+          [upper_leg_end(2), lower_leg_end(2)], ...
+          [upper_leg_end(3), lower_leg_end(3)], 'r-o', 'LineWidth', 3, 'MarkerSize', 8);
+    
+    % 绘制目标位置
+    plot3(target_pos(1), target_pos(2), target_pos(3), 'g*', 'MarkerSize', 15);
+    
+    % 绘制yaw offset
+    plot3([hip_pos(1), hip_pos(1)+yaw_offset*sin(theta_yaw)], ...
+          [0, yaw_offset*cos(theta_yaw)], ...
+          [hip_pos(3), hip_pos(3)], 'm--', 'LineWidth', 2);
+    
+    % 设置视图范围
+    axis_range = max([L1+L2, abs(offset_x)+0.1, abs(offset_z)+0.1]);
+    xlim([-axis_range, axis_range]);
+    ylim([-axis_range, axis_range]);
+    zlim([-axis_range, axis_range]);
+    
+    % 2. X-Y平面视图
+    subplot(2,2,2);
+    hold on;
+    grid on;
+    axis equal;
+    xlabel('X (m)');
+    ylabel('Y (m)');
+    title('X-Y平面视图');
+    
+    plot([0, 0.1], [0, 0], 'r', 'LineWidth', 2); % X轴
+    plot([0, 0], [0, 0.1], 'g', 'LineWidth', 2); % Y轴
+    
+    plot([hip_pos(1), upper_leg_end(1)], [hip_pos(2), upper_leg_end(2)], 'b-o', 'LineWidth', 3, 'MarkerSize', 8);
+    plot([upper_leg_end(1), lower_leg_end(1)], [upper_leg_end(2), lower_leg_end(2)], 'r-o', 'LineWidth', 3, 'MarkerSize', 8);
+    plot(target_pos(1), target_pos(2), 'g*', 'MarkerSize', 15);
+    plot([hip_pos(1), hip_pos(1)+yaw_offset*sin(theta_yaw)], [0, yaw_offset*cos(theta_yaw)], 'm--', 'LineWidth', 2);
+    
+    xlim([-axis_range, axis_range]);
+    ylim([-axis_range, axis_range]);
+    
+    % 3. X-Z平面视图
+    subplot(2,2,4);
+    hold on;
+    grid on;
+    axis equal;
+    xlabel('X (m)');
+    ylabel('Z (m)');
+    title('X-Z平面视图');
+    
+    plot([0, 0.1], [0, 0], 'r', 'LineWidth', 2); % X轴
+    plot([0, 0], [0, 0.1], 'b', 'LineWidth', 2); % Z轴
+    
+    plot([hip_pos(1), upper_leg_end(1)], [hip_pos(3), upper_leg_end(3)], 'b-o', 'LineWidth', 3, 'MarkerSize', 8);
+    plot([upper_leg_end(1), lower_leg_end(1)], [upper_leg_end(3), lower_leg_end(3)], 'r-o', 'LineWidth', 3, 'MarkerSize', 8);
+    plot(target_pos(1), target_pos(3), 'g*', 'MarkerSize', 15);
+    
+    xlim([-axis_range, axis_range]);
+    ylim([-axis_range, axis_range]);
+    
+    % 添加图例
+    legend({'大腿', '小腿', '目标位置', 'Yaw偏移'}, 'Location', 'best');
+end

Utils/Kinematics.py

@@ -0,0 +1,78 @@
+# test_kinematics.py
+
+import numpy as np
+
+def forward_kinematics(theta, link, side_sign):
+    l1 = link['hip'] * side_sign
+    l2 = -link['thigh']
+    l3 = -link['calf']
+    q1, q2, q3 = theta
+
+    s1, s2, s3 = np.sin([q1, q2, q3])
+    c1, c2, c3 = np.cos([q1, q2, q3])
+
+    c23 = c2 * c3 - s2 * s3
+    s23 = s2 * c3 + c2 * s3
+
+    x = l3 * s23 + l2 * s2
+    y = -l3 * s1 * c23 + l1 * c1 - l2 * c2 * s1
+    z =  l3 * c1 * c23 + l1 * s1 + l2 * c1 * c2
+    return np.array([x, y, z])
+
+def q1_ik(py, pz, l1):
+    L = np.sqrt(py*py + pz*pz - l1*l1)
+    return np.arctan2(pz*l1 + py*L, py*l1 - pz*L)
+
+def q3_ik(b3z, b4z, b):
+    temp = (b3z**2 + b4z**2 - b**2) / (2.0 * np.abs(b3z * b4z))
+    temp = np.clip(temp, -1.0, 1.0)
+    q3 = np.arccos(temp)
+    return -(np.pi - q3)
+
+def q2_ik(q1, q3, px, py, pz, b3z, b4z):
+    a1 = py * np.sin(q1) - pz * np.cos(q1)
+    a2 = px
+    m1 = b4z * np.sin(q3)
+    m2 = b3z + b4z * np.cos(q3)
+    return np.arctan2(m1 * a1 + m2 * a2, m1 * a2 - m2 * a1)
+
+def inverse_kinematics(foot, link, side_sign):
+    px, py, pz = foot
+    b2y = link['hip'] * side_sign
+    b3z = -link['thigh']
+    b4z = -link['calf']
+    a = link['hip']
+    c = np.sqrt(px*px + py*py + pz*pz)
+    b = np.sqrt(c*c - a*a)
+    if np.isnan(b) or b > abs(b3z) + abs(b4z):
+        return None
+    q1 = q1_ik(py, pz, b2y)
+    q3 = q3_ik(b3z, b4z, b)
+    q2 = q2_ik(q1, q3, px, py, pz, b3z, b4z)
+    return np.array([q1, q2, q3])
+
+# ...existing code...
+
+def test():
+    link = {'hip': 0.05, 'thigh': 0.2, 'calf': 0.2}
+    side_sign = 1  # 左前腿
+
+    # 输入一个目标点
+    foot_target = np.array([0.0, 0.06, -0.2])
+    print("目标足端位置:", foot_target)
+
+    # 逆运动学求解关节角度
+    theta = inverse_kinematics(foot_target, link, side_sign)
+    if theta is None:
+        print("目标点不可达！")
+        return
+    print("逆运动学解（关节角度，单位：弧度）:", theta)
+    print("逆运动学解（关节角度，单位：度）:", np.degrees(theta))
+
+    # 正运动学验算
+    foot_check = forward_kinematics(theta, link, side_sign)
+    print("正运动学验算足端位置:", foot_check)
+    print("位置误差:", np.abs(foot_target - foot_check))
+
+if __name__ == "__main__":
+    test()