Merge branch 'madao'

Final
add Encoder and PID control and Abstacle control
2025-07-08 23:32:52 +08:00 · 2025-07-08 23:30:16 +08:00 · 2025-07-03 00:29:57 +08:00 · 2025-07-02 00:08:05 +08:00 · 2025-07-02 00:06:16 +08:00 · 2025-07-01 22:12:49 +08:00
90 changed files with 27403 additions and 9135 deletions
--- a/.mxproject
+++ b/.mxproject
--- a/AZURE_RTOS/App/app_azure_rtos.c
+++ b/AZURE_RTOS/App/app_azure_rtos.c
@@ -22,7 +22,7 @@
 #include "app_azure_rtos.h"
 /* Private includes ----------------------------------------------------------*/
 /* USER CODE BEGIN Includes */
-
+#include "headfile.h"
 /* USER CODE END Includes */

 /* Private typedef -----------------------------------------------------------*/
@@ -95,11 +95,12 @@ VOID tx_application_define(VOID *first_unused_memory)
      /* USER CODE BEGIN  App_ThreadX_Init_Error */
      while(1)
      {
+		  
      }
      /* USER CODE END  App_ThreadX_Init_Error */
    }
    /* USER CODE BEGIN  App_ThreadX_Init_Success */
-
+//	App_ThreadX_Init(first_unused_memory);  //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> //app_thread_init()<29>ſ<EFBFBD><C5BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
    /* USER CODE END  App_ThreadX_Init_Success */

  }
--- a/AutoGuideStick.ioc
+++ b/AutoGuideStick.ioc
@@ -7,18 +7,21 @@ CAD.pinconfig=
 CAD.provider=
 CORTEX_M33_NS.userName=CORTEX_M33
 File.Version=6
+GPDMA1.CIRCULARMODE_GPDMACH3=ENABLE
 GPDMA1.CIRCULARMODE_GPDMACH5=ENABLE
 GPDMA1.DIRECTION_GPDMACH4=DMA_MEMORY_TO_PERIPH
+GPDMA1.IPHANDLE_GPDMACH3-SIMPLEREQUEST_GPDMACH3=__NULL
 GPDMA1.IPHANDLE_GPDMACH4-SIMPLEREQUEST_GPDMACH4=__NULL
 GPDMA1.IPHANDLE_GPDMACH5-SIMPLEREQUEST_GPDMACH5=__NULL
-GPDMA1.IPParameters=IPHANDLE_GPDMACH4-SIMPLEREQUEST_GPDMACH4,REQUEST_GPDMACH4,DIRECTION_GPDMACH4,SRCINC_GPDMACH4,CIRCULARMODE_GPDMACH5,LINKALLOCATEDPORT_CIRCULAR_GPDMACH5,IPHANDLE_GPDMACH5-SIMPLEREQUEST_GPDMACH5,REQUEST_GPDMACH5,TRANSFERALLOCATEDPORTSRC_GPDMACH5,TRANSFERALLOCATEDPORTDEST_GPDMACH5
+GPDMA1.IPParameters=REQUEST_GPDMACH4,DIRECTION_GPDMACH4,SRCINC_GPDMACH4,CIRCULARMODE_GPDMACH5,LINKALLOCATEDPORT_CIRCULAR_GPDMACH5,REQUEST_GPDMACH5,TRANSFERALLOCATEDPORTSRC_GPDMACH5,TRANSFERALLOCATEDPORTDEST_GPDMACH5,IPHANDLE_GPDMACH5-SIMPLEREQUEST_GPDMACH5,IPHANDLE_GPDMACH4-SIMPLEREQUEST_GPDMACH4,CIRCULARMODE_GPDMACH3,IPHANDLE_GPDMACH3-SIMPLEREQUEST_GPDMACH3,REQUEST_GPDMACH3
 GPDMA1.LINKALLOCATEDPORT_CIRCULAR_GPDMACH5=DMA_LINK_ALLOCATED_PORT1
-GPDMA1.REQUEST_GPDMACH4=GPDMA1_REQUEST_USART1_TX
-GPDMA1.REQUEST_GPDMACH5=GPDMA1_REQUEST_USART1_RX
+GPDMA1.REQUEST_GPDMACH3=GPDMA1_REQUEST_USART2_RX
+GPDMA1.REQUEST_GPDMACH4=GPDMA1_REQUEST_UART4_TX
+GPDMA1.REQUEST_GPDMACH5=GPDMA1_REQUEST_UART4_RX
 GPDMA1.SRCINC_GPDMACH4=DMA_SINC_INCREMENTED
 GPDMA1.TRANSFERALLOCATEDPORTDEST_GPDMACH5=DMA_DEST_ALLOCATED_PORT1
 GPDMA1.TRANSFERALLOCATEDPORTSRC_GPDMACH5=DMA_SRC_ALLOCATED_PORT1
-GPIO.groupedBy=
+GPIO.groupedBy=Group By Peripherals
 KeepUserPlacement=false
 MMTAppRegionsCount=0
 MMTConfigApplied=false
@@ -27,30 +30,65 @@ Mcu.ContextProject=TrustZoneDisabled
 Mcu.Family=STM32H5
 Mcu.IP0=BOOTPATH
 Mcu.IP1=CORTEX_M33_NS
-Mcu.IP2=GPDMA1
-Mcu.IP3=MEMORYMAP
-Mcu.IP4=NVIC
-Mcu.IP5=PWR
-Mcu.IP6=RCC
-Mcu.IP7=SYS
-Mcu.IP8=THREADX
-Mcu.IP9=USART1
-Mcu.IPNb=10
+Mcu.IP10=TIM1
+Mcu.IP11=TIM2
+Mcu.IP12=TIM3
+Mcu.IP13=TIM4
+Mcu.IP14=TIM5
+Mcu.IP15=TIM8
+Mcu.IP16=UART4
+Mcu.IP17=USART2
+Mcu.IP18=USART3
+Mcu.IP2=DEBUG
+Mcu.IP3=GPDMA1
+Mcu.IP4=MEMORYMAP
+Mcu.IP5=NVIC
+Mcu.IP6=PWR
+Mcu.IP7=RCC
+Mcu.IP8=SYS
+Mcu.IP9=THREADX
+Mcu.IPNb=19
 Mcu.Name=STM32H563ZITx
 Mcu.Package=LQFP144
 Mcu.Pin0=PH0-OSC_IN(PH0)
-Mcu.Pin1=PB14
-Mcu.Pin10=VP_BOOTPATH_VS_BOOTPATH
-Mcu.Pin11=VP_MEMORYMAP_VS_MEMORYMAP
-Mcu.Pin2=PB15
-Mcu.Pin3=VP_CORTEX_M33_NS_VS_Hclk
-Mcu.Pin4=VP_GPDMA1_VS_GPDMACH4
-Mcu.Pin5=VP_GPDMA1_VS_GPDMACH5
-Mcu.Pin6=VP_PWR_VS_SECSignals
-Mcu.Pin7=VP_PWR_VS_LPOM
-Mcu.Pin8=VP_SYS_VS_tim1
-Mcu.Pin9=VP_THREADX_VS_RTOSJjThreadXJjCoreJjDefault
-Mcu.PinsNb=12
+Mcu.Pin1=PH1-OSC_OUT(PH1)
+Mcu.Pin10=PG2
+Mcu.Pin11=PG3
+Mcu.Pin12=PC6
+Mcu.Pin13=PC7
+Mcu.Pin14=PC9
+Mcu.Pin15=PA13(JTMS/SWDIO)
+Mcu.Pin16=PA14(JTCK/SWCLK)
+Mcu.Pin17=PC11
+Mcu.Pin18=PC12
+Mcu.Pin19=PD2
+Mcu.Pin2=PC2
+Mcu.Pin20=PD5
+Mcu.Pin21=PD6
+Mcu.Pin22=PB8
+Mcu.Pin23=PB9
+Mcu.Pin24=VP_CORTEX_M33_NS_VS_Hclk
+Mcu.Pin25=VP_GPDMA1_VS_GPDMACH3
+Mcu.Pin26=VP_GPDMA1_VS_GPDMACH4
+Mcu.Pin27=VP_GPDMA1_VS_GPDMACH5
+Mcu.Pin28=VP_PWR_VS_SECSignals
+Mcu.Pin29=VP_PWR_VS_LPOM
+Mcu.Pin3=PA0
+Mcu.Pin30=VP_SYS_VS_tim6
+Mcu.Pin31=VP_THREADX_VS_RTOSJjThreadXJjCoreJjDefault
+Mcu.Pin32=VP_TIM2_VS_ClockSourceINT
+Mcu.Pin33=VP_TIM3_VS_ClockSourceINT
+Mcu.Pin34=VP_TIM4_VS_ClockSourceINT
+Mcu.Pin35=VP_TIM5_VS_ClockSourceINT
+Mcu.Pin36=VP_BOOTPATH_VS_BOOTPATH
+Mcu.Pin37=VP_MEMORYMAP_VS_MEMORYMAP
+Mcu.Pin4=PB1
+Mcu.Pin5=PG0
+Mcu.Pin6=PG1
+Mcu.Pin7=PE9
+Mcu.Pin8=PE11
+Mcu.Pin9=PB10
+Mcu.PinsNb=38
 Mcu.ThirdPartyNb=0
 Mcu.UserConstants=
 Mcu.UserName=STM32H563ZITx
@@ -59,6 +97,7 @@ MxDb.Version=DB.6.0.141
 NVIC.BusFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
 NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
 NVIC.ForceEnableDMAVector=true
+NVIC.GPDMA1_Channel3_IRQn=true\:0\:0\:false\:false\:true\:true\:false\:true\:true
 NVIC.GPDMA1_Channel4_IRQn=true\:0\:0\:false\:false\:true\:true\:false\:true\:true
 NVIC.GPDMA1_Channel5_IRQn=true\:0\:0\:false\:false\:true\:true\:false\:true\:true
 NVIC.HardFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
@@ -71,17 +110,104 @@ NVIC.SavedPendsvIrqHandlerGenerated=true
 NVIC.SavedSvcallIrqHandlerGenerated=true
 NVIC.SavedSystickIrqHandlerGenerated=true
 NVIC.SysTick_IRQn=true\:14\:0\:false\:false\:false\:false\:false\:true\:false
-NVIC.TIM1_UP_IRQn=true\:15\:0\:false\:false\:true\:false\:false\:true\:true
-NVIC.TimeBase=TIM1_UP_IRQn
-NVIC.TimeBaseIP=TIM1
-NVIC.USART1_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true\:true
+NVIC.TIM2_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true\:true
+NVIC.TIM3_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true\:true
+NVIC.TIM5_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true\:true
+NVIC.TIM6_IRQn=true\:15\:0\:false\:false\:true\:false\:false\:true\:true
+NVIC.TimeBase=TIM6_IRQn
+NVIC.TimeBaseIP=TIM6
+NVIC.UART4_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true\:true
+NVIC.USART2_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true\:true
+NVIC.USART3_IRQn=true\:0\:0\:false\:false\:true\:false\:true\:true\:true
 NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
-PB14.Mode=Asynchronous
-PB14.Signal=USART1_TX
-PB15.Mode=Asynchronous
-PB15.Signal=USART1_RX
-PH0-OSC_IN(PH0).Mode=HSE-External-Clock-Source
+PA0.GPIOParameters=GPIO_Label
+PA0.GPIO_Label=HC_Echo
+PA0.Signal=S_TIM5_CH1
+PA13(JTMS/SWDIO).Mode=Serial_Wire
+PA13(JTMS/SWDIO).Signal=DEBUG_JTMS-SWDIO
+PA14(JTCK/SWCLK).Mode=Serial_Wire
+PA14(JTCK/SWCLK).Signal=DEBUG_JTCK-SWCLK
+PB1.GPIOParameters=GPIO_Label
+PB1.GPIO_Label=PWMB
+PB1.Locked=true
+PB1.Signal=S_TIM3_CH4
+PB10.Locked=true
+PB10.Mode=Asynchronous
+PB10.Signal=USART3_TX
+PB8.Locked=true
+PB8.Mode=Asynchronous
+PB8.Signal=UART4_RX
+PB9.Locked=true
+PB9.Mode=Asynchronous
+PB9.Signal=UART4_TX
+PC11.Locked=true
+PC11.Mode=Asynchronous
+PC11.Signal=USART3_RX
+PC12.GPIOParameters=GPIO_PuPd,GPIO_Label
+PC12.GPIO_Label=Shake_Motor
+PC12.GPIO_PuPd=GPIO_PULLDOWN
+PC12.Locked=true
+PC12.Signal=GPIO_Output
+PC2.GPIOParameters=GPIO_Label
+PC2.GPIO_Label=PWMA
+PC2.Locked=true
+PC2.Signal=S_TIM4_CH4
+PC6.GPIOParameters=GPIO_PuPd,GPIO_Label
+PC6.GPIO_Label=E2A
+PC6.GPIO_PuPd=GPIO_PULLUP
+PC6.Locked=true
+PC6.Signal=S_TIM8_CH1
+PC7.GPIOParameters=GPIO_PuPd,GPIO_Label
+PC7.GPIO_Label=E2B
+PC7.GPIO_PuPd=GPIO_PULLUP
+PC7.Locked=true
+PC7.Signal=S_TIM8_CH2
+PC9.GPIOParameters=GPIO_PuPd,GPIO_Label
+PC9.GPIO_Label=HC_Trig
+PC9.GPIO_PuPd=GPIO_NOPULL
+PC9.Locked=true
+PC9.Signal=GPIO_Output
+PD2.GPIOParameters=GPIO_PuPd,GPIO_Label
+PD2.GPIO_Label=Buzzer
+PD2.GPIO_PuPd=GPIO_PULLDOWN
+PD2.Locked=true
+PD2.Signal=GPIO_Output
+PD5.Locked=true
+PD5.Mode=Asynchronous
+PD5.Signal=USART2_TX
+PD6.Locked=true
+PD6.Mode=Asynchronous
+PD6.Signal=USART2_RX
+PE11.GPIOParameters=GPIO_PuPd,GPIO_Label
+PE11.GPIO_Label=E1B
+PE11.GPIO_PuPd=GPIO_PULLUP
+PE11.Locked=true
+PE11.Signal=S_TIM1_CH2
+PE9.GPIOParameters=GPIO_PuPd,GPIO_Label
+PE9.GPIO_Label=E1A
+PE9.GPIO_PuPd=GPIO_PULLUP
+PE9.Locked=true
+PE9.Signal=S_TIM1_CH1
+PG0.GPIOParameters=GPIO_Label
+PG0.GPIO_Label=AIN1
+PG0.Locked=true
+PG0.Signal=GPIO_Output
+PG1.GPIOParameters=GPIO_Label
+PG1.GPIO_Label=AIN2
+PG1.Locked=true
+PG1.Signal=GPIO_Output
+PG2.GPIOParameters=GPIO_Label
+PG2.GPIO_Label=BIN1
+PG2.Locked=true
+PG2.Signal=GPIO_Output
+PG3.GPIOParameters=GPIO_Label
+PG3.GPIO_Label=BIN2
+PG3.Locked=true
+PG3.Signal=GPIO_Output
+PH0-OSC_IN(PH0).Mode=HSE-External-Oscillator
 PH0-OSC_IN(PH0).Signal=RCC_OSC_IN
+PH1-OSC_OUT(PH1).Mode=HSE-External-Oscillator
+PH1-OSC_OUT(PH1).Signal=RCC_OSC_OUT
 PinOutPanel.RotationAngle=0
 ProjectManager.AskForMigrate=true
 ProjectManager.BackupPrevious=false
@@ -114,7 +240,7 @@ ProjectManager.ToolChainLocation=
 ProjectManager.UAScriptAfterPath=
 ProjectManager.UAScriptBeforePath=
 ProjectManager.UnderRoot=false
-ProjectManager.functionlistsort=1-SystemClock_Config-RCC-false-HAL-false,2-MX_GPIO_Init-GPIO-false-HAL-true,3-MX_GPDMA1_Init-GPDMA1-false-HAL-true,4-MX_USART1_UART_Init-USART1-false-HAL-true,0-MX_CORTEX_M33_NS_Init-CORTEX_M33_NS-false-HAL-true,0-MX_PWR_Init-PWR-false-HAL-true
+ProjectManager.functionlistsort=1-SystemClock_Config-RCC-false-HAL-false,2-MX_GPIO_Init-GPIO-false-HAL-true,3-MX_GPDMA1_Init-GPDMA1-false-HAL-true,4-MX_USART2_UART_Init-USART2-false-HAL-true,5-MX_TIM2_Init-TIM2-false-HAL-true,6-MX_TIM3_Init-TIM3-false-HAL-true,7-MX_USART3_UART_Init-USART3-false-HAL-true,8-MX_TIM4_Init-TIM4-false-HAL-true,9-MX_UART4_Init-UART4-false-HAL-true,10-MX_TIM1_Init-TIM1-false-HAL-true,11-MX_TIM8_Init-TIM8-false-HAL-true,0-MX_CORTEX_M33_NS_Init-CORTEX_M33_NS-false-HAL-true,0-MX_PWR_Init-PWR-false-HAL-true
 RCC.ADCFreq_Value=250000000
 RCC.AHBFreq_Value=250000000
 RCC.APB1Freq_Value=250000000
@@ -200,13 +326,54 @@ RCC.VCOInputFreq_Value=4000000
 RCC.VCOOutputFreq_Value=500000000
 RCC.VCOPLL2OutputFreq_Value=516000000
 RCC.VCOPLL3OutputFreq_Value=516000000
-USART1.BaudRate=9600
-USART1.IPParameters=VirtualMode-Asynchronous,BaudRate
-USART1.VirtualMode-Asynchronous=VM_ASYNC
+SH.S_TIM1_CH1.0=TIM1_CH1,Encoder_Interface
+SH.S_TIM1_CH1.ConfNb=1
+SH.S_TIM1_CH2.0=TIM1_CH2,Encoder_Interface
+SH.S_TIM1_CH2.ConfNb=1
+SH.S_TIM3_CH4.0=TIM3_CH4,PWM Generation4 CH4
+SH.S_TIM3_CH4.ConfNb=1
+SH.S_TIM4_CH4.0=TIM4_CH4,PWM Generation4 CH4
+SH.S_TIM4_CH4.ConfNb=1
+SH.S_TIM5_CH1.0=TIM5_CH1,Input_Capture1_from_TI1
+SH.S_TIM5_CH1.ConfNb=1
+SH.S_TIM8_CH1.0=TIM8_CH1,Encoder_Interface
+SH.S_TIM8_CH1.ConfNb=1
+SH.S_TIM8_CH2.0=TIM8_CH2,Encoder_Interface
+SH.S_TIM8_CH2.ConfNb=1
+THREADX.IPParameters=TX_APP_GENERATE_INIT_CODE,TX_MINIMUM_STACK
+THREADX.TX_APP_GENERATE_INIT_CODE=false
+THREADX.TX_MINIMUM_STACK=400
+TIM1.EncoderMode=TIM_ENCODERMODE_TI12
+TIM1.IPParameters=EncoderMode
+TIM2.IPParameters=Prescaler
+TIM2.Prescaler=250 - 1
+TIM3.Channel-PWM\ Generation4\ CH4=TIM_CHANNEL_4
+TIM3.IPParameters=Prescaler,Channel-PWM Generation4 CH4,PeriodNoDither
+TIM3.PeriodNoDither=255
+TIM3.Prescaler=48
+TIM4.Channel-PWM\ Generation4\ CH4=TIM_CHANNEL_4
+TIM4.IPParameters=Channel-PWM Generation4 CH4,Prescaler,PeriodNoDither
+TIM4.PeriodNoDither=255
+TIM4.Prescaler=48
+TIM5.Channel-Input_Capture1_from_TI1=TIM_CHANNEL_1
+TIM5.IPParameters=Channel-Input_Capture1_from_TI1,Prescaler
+TIM5.Prescaler=250-1
+TIM8.EncoderMode=TIM_ENCODERMODE_TI12
+TIM8.IPParameters=EncoderMode
+UART4.BaudRate=9600
+UART4.IPParameters=BaudRate
+USART2.BaudRate=9600
+USART2.IPParameters=VirtualMode-Asynchronous,BaudRate
+USART2.VirtualMode-Asynchronous=VM_ASYNC
+USART3.BaudRate=115200
+USART3.IPParameters=VirtualMode-Asynchronous,BaudRate
+USART3.VirtualMode-Asynchronous=VM_ASYNC
 VP_BOOTPATH_VS_BOOTPATH.Mode=BP_Activate
 VP_BOOTPATH_VS_BOOTPATH.Signal=BOOTPATH_VS_BOOTPATH
 VP_CORTEX_M33_NS_VS_Hclk.Mode=Hclk_Mode
 VP_CORTEX_M33_NS_VS_Hclk.Signal=CORTEX_M33_NS_VS_Hclk
+VP_GPDMA1_VS_GPDMACH3.Mode=SIMPLEREQUEST_GPDMACH3
+VP_GPDMA1_VS_GPDMACH3.Signal=GPDMA1_VS_GPDMACH3
 VP_GPDMA1_VS_GPDMACH4.Mode=SIMPLEREQUEST_GPDMACH4
 VP_GPDMA1_VS_GPDMACH4.Signal=GPDMA1_VS_GPDMACH4
 VP_GPDMA1_VS_GPDMACH5.Mode=SIMPLEREQUEST_GPDMACH5
@@ -217,8 +384,16 @@ VP_PWR_VS_LPOM.Mode=PowerOptimisation
 VP_PWR_VS_LPOM.Signal=PWR_VS_LPOM
 VP_PWR_VS_SECSignals.Mode=Security/Privilege
 VP_PWR_VS_SECSignals.Signal=PWR_VS_SECSignals
-VP_SYS_VS_tim1.Mode=TIM1
-VP_SYS_VS_tim1.Signal=SYS_VS_tim1
+VP_SYS_VS_tim6.Mode=TIM6
+VP_SYS_VS_tim6.Signal=SYS_VS_tim6
 VP_THREADX_VS_RTOSJjThreadXJjCoreJjDefault.Mode=Core_Default
 VP_THREADX_VS_RTOSJjThreadXJjCoreJjDefault.Signal=THREADX_VS_RTOSJjThreadXJjCoreJjDefault
+VP_TIM2_VS_ClockSourceINT.Mode=Internal
+VP_TIM2_VS_ClockSourceINT.Signal=TIM2_VS_ClockSourceINT
+VP_TIM3_VS_ClockSourceINT.Mode=Internal
+VP_TIM3_VS_ClockSourceINT.Signal=TIM3_VS_ClockSourceINT
+VP_TIM4_VS_ClockSourceINT.Mode=Internal
+VP_TIM4_VS_ClockSourceINT.Signal=TIM4_VS_ClockSourceINT
+VP_TIM5_VS_ClockSourceINT.Mode=Internal
+VP_TIM5_VS_ClockSourceINT.Signal=TIM5_VS_ClockSourceINT
 board=custom
--- a/Core/Inc/app_threadx.h
+++ b/Core/Inc/app_threadx.h
@@ -30,12 +30,13 @@ extern "C" {

 /* Private includes ----------------------------------------------------------*/
 /* USER CODE BEGIN Includes */
-
+#include "headfile.h"
 /* USER CODE END Includes */

 /* Exported types ------------------------------------------------------------*/
 /* USER CODE BEGIN ET */

+//extern MotorCommand current_motor_cmd;
 /* USER CODE END ET */

 /* Exported constants --------------------------------------------------------*/
@@ -46,6 +47,23 @@ extern "C" {
 /* Private defines -----------------------------------------------------------*/

 /* USER CODE BEGIN PD */
+// <20>¼<EFBFBD><C2BC><EFBFBD>־<EFBFBD><D6BE><EFBFBD><EFBFBD>
+#define EVENT_OBSTACLE_DETECTED    0x01
+#define EVENT_BLE_COMMAND_RECEIVED 0x04
+#define IMU_UPDATE_EVENT           0x08
+#define EVENT_LOCATION_UPDATED     0x10
+
+
+#define TEST 			1
+extern TX_QUEUE ble_tx_queue;
+extern TX_EVENT_FLAGS_GROUP system_events;
+extern TX_EVENT_FLAGS_GROUP sensor_events; //传感器事件组
+extern TX_EVENT_FLAGS_GROUP response_events; //避障
+//typedef struct
+//{
+////	uint32_t msg_type;  // 应该使用 int --- 4字节
+//	char data[128];
+//}BLE_Message;  // 

 /* USER CODE END PD */

--- a/Core/Inc/main.h
+++ b/Core/Inc/main.h
@@ -57,6 +57,34 @@ void Error_Handler(void);
 /* USER CODE END EFP */

 /* Private defines -----------------------------------------------------------*/
+#define PWMA_Pin GPIO_PIN_2
+#define PWMA_GPIO_Port GPIOC
+#define HC_Echo_Pin GPIO_PIN_0
+#define HC_Echo_GPIO_Port GPIOA
+#define PWMB_Pin GPIO_PIN_1
+#define PWMB_GPIO_Port GPIOB
+#define AIN1_Pin GPIO_PIN_0
+#define AIN1_GPIO_Port GPIOG
+#define AIN2_Pin GPIO_PIN_1
+#define AIN2_GPIO_Port GPIOG
+#define E1A_Pin GPIO_PIN_9
+#define E1A_GPIO_Port GPIOE
+#define E1B_Pin GPIO_PIN_11
+#define E1B_GPIO_Port GPIOE
+#define BIN1_Pin GPIO_PIN_2
+#define BIN1_GPIO_Port GPIOG
+#define BIN2_Pin GPIO_PIN_3
+#define BIN2_GPIO_Port GPIOG
+#define E2A_Pin GPIO_PIN_6
+#define E2A_GPIO_Port GPIOC
+#define E2B_Pin GPIO_PIN_7
+#define E2B_GPIO_Port GPIOC
+#define HC_Trig_Pin GPIO_PIN_9
+#define HC_Trig_GPIO_Port GPIOC
+#define Shake_Motor_Pin GPIO_PIN_12
+#define Shake_Motor_GPIO_Port GPIOC
+#define Buzzer_Pin GPIO_PIN_2
+#define Buzzer_GPIO_Port GPIOD

 /* USER CODE BEGIN Private defines */

--- a/Core/Inc/stm32h5xx_it.h
+++ b/Core/Inc/stm32h5xx_it.h
@@ -52,10 +52,16 @@ void MemManage_Handler(void);
 void BusFault_Handler(void);
 void UsageFault_Handler(void);
 void DebugMon_Handler(void);
+void GPDMA1_Channel3_IRQHandler(void);
 void GPDMA1_Channel4_IRQHandler(void);
 void GPDMA1_Channel5_IRQHandler(void);
-void TIM1_UP_IRQHandler(void);
-void USART1_IRQHandler(void);
+void TIM2_IRQHandler(void);
+void TIM3_IRQHandler(void);
+void TIM5_IRQHandler(void);
+void TIM6_IRQHandler(void);
+void USART2_IRQHandler(void);
+void USART3_IRQHandler(void);
+void UART4_IRQHandler(void);
 /* USER CODE BEGIN EFP */

 /* USER CODE END EFP */
--- a/Core/Inc/tim.h
+++ b/Core/Inc/tim.h
@@ -0,0 +1,69 @@
+/* USER CODE BEGIN Header */
+/**
+  ******************************************************************************
+  * @file    tim.h
+  * @brief   This file contains all the function prototypes for
+  *          the tim.c file
+  ******************************************************************************
+  * @attention
+  *
+  * Copyright (c) 2025 STMicroelectronics.
+  * All rights reserved.
+  *
+  * This software is licensed under terms that can be found in the LICENSE file
+  * in the root directory of this software component.
+  * If no LICENSE file comes with this software, it is provided AS-IS.
+  *
+  ******************************************************************************
+  */
+/* USER CODE END Header */
+/* Define to prevent recursive inclusion -------------------------------------*/
+#ifndef __TIM_H__
+#define __TIM_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ------------------------------------------------------------------*/
+#include "main.h"
+
+/* USER CODE BEGIN Includes */
+
+/* USER CODE END Includes */
+
+extern TIM_HandleTypeDef htim1;
+
+extern TIM_HandleTypeDef htim2;
+
+extern TIM_HandleTypeDef htim3;
+
+extern TIM_HandleTypeDef htim4;
+
+extern TIM_HandleTypeDef htim5;
+
+extern TIM_HandleTypeDef htim8;
+
+/* USER CODE BEGIN Private defines */
+
+/* USER CODE END Private defines */
+
+void MX_TIM1_Init(void);
+void MX_TIM2_Init(void);
+void MX_TIM3_Init(void);
+void MX_TIM4_Init(void);
+void MX_TIM5_Init(void);
+void MX_TIM8_Init(void);
+
+void HAL_TIM_MspPostInit(TIM_HandleTypeDef *htim);
+
+/* USER CODE BEGIN Prototypes */
+
+/* USER CODE END Prototypes */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __TIM_H__ */
+
--- a/Core/Inc/tx_user.h
+++ b/Core/Inc/tx_user.h
@@ -115,7 +115,7 @@
 /*#define TX_TIMER_THREAD_STACK_SIZE                1024*/
 /*#define TX_TIMER_THREAD_PRIORITY                0*/

-/*#define TX_MINIMUM_STACK                200*/
+#define TX_MINIMUM_STACK                400

 /* Determine if timer expirations (application timers, timeouts, and tx_thread_sleep) calls
   should be processed within the a system timer thread or directly in the timer ISR.
--- a/Core/Inc/usart.h
+++ b/Core/Inc/usart.h
@@ -32,13 +32,19 @@ extern "C" {

 /* USER CODE END Includes */

-extern UART_HandleTypeDef huart1;
+extern UART_HandleTypeDef huart4;
+
+extern UART_HandleTypeDef huart2;
+
+extern UART_HandleTypeDef huart3;

 /* USER CODE BEGIN Private defines */

 /* USER CODE END Private defines */

-void MX_USART1_UART_Init(void);
+void MX_UART4_Init(void);
+void MX_USART2_UART_Init(void);
+void MX_USART3_UART_Init(void);

 /* USER CODE BEGIN Prototypes */

--- a/Core/Src/app_threadx.c
+++ b/Core/Src/app_threadx.c
@@ -34,6 +34,18 @@
 /* Private define ------------------------------------------------------------*/
 /* USER CODE BEGIN PD */

+// BLE define 
+#define BLE_RX_THREAD_STACK_SIZE 2048
+#define BLE_RX_THREAD_PRIORITY    9
+#define BLE_TX_THREAD_STACK_SIZE 2048
+#define BLE_TX_THREAD_PRIORITY    10
+// IMU thread config
+// IMU thread config
+#define IMU_ANGLE_THREAD_STACK_SIZE  1024
+#define IMU_ANGLE_THREAD_PRIORITY    11
+
+
+
 /* USER CODE END PD */

 /* Private macro -------------------------------------------------------------*/
@@ -43,11 +55,57 @@

 /* Private variables ---------------------------------------------------------*/
 /* USER CODE BEGIN PV */
+/* ȫ<>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> */

+TX_EVENT_FLAGS_GROUP system_events;
+TX_EVENT_FLAGS_GROUP sensor_events;
+TX_EVENT_FLAGS_GROUP response_events; // 用于避障事件实现
+
+MotorCommand current_motor_cmd = {0,0};
+_GPSData gps_data;
+
+// Ble thread vd
+TX_THREAD ble_rx_thread;
+TX_THREAD ble_tx_thread;
+UCHAR ble_rx_stack[BLE_RX_THREAD_STACK_SIZE];
+UCHAR ble_tx_stack[BLE_TX_THREAD_STACK_SIZE];
+
+TX_QUEUE ble_tx_queue;
+#define BLE_TX_QUEUE_LEN 10
+//ULONG ble_tx_queue_buffer[BLE_TX_QUEUE_LEN]; // 一定要是ULONG类型 之前使用的是BLE_Message
+
+#define BLE_TX_MSG_LEN 64
+#define BLE_TX_QUEUE_LEN 10
+
+__attribute__((aligned(4)))
+ULONG ble_tx_queue_buffer[BLE_TX_QUEUE_LEN * ((BLE_TX_MSG_LEN + sizeof(ULONG) - 1) / sizeof(ULONG))];
+
+// imu
+TX_THREAD imu_angle_thread;
+UCHAR imu_angle_stack[IMU_ANGLE_THREAD_STACK_SIZE];
+
+#define IM948_RX_QUEUE_SIZE 64
+ULONG im948_rx_queue_buffer[IM948_RX_QUEUE_SIZE];
+TX_QUEUE im948_uart_rx_queue;
 /* USER CODE END PV */

 /* Private function prototypes -----------------------------------------------*/
 /* USER CODE BEGIN PFP */
+#define GPS_TASK_STACK_SIZE   2048
+#define GPS_TASK_PRIORITY     11
+
+TX_THREAD gps_task;
+ULONG gps_task_stack[GPS_TASK_STACK_SIZE / sizeof(ULONG)];
+
+static TX_THREAD obstacle_thread;
+static UCHAR obstacle_stack[512];
+//超声波
+static UCHAR ultrasonic_stack[512];
+static void obstacle_thread_entry(ULONG);
+TX_THREAD ultrasonic_task_thread;
+
+/* USER CODE END 1 */
+/* USER CODE END 1 */

 /* USER CODE END PFP */

@@ -58,16 +116,126 @@
  */
 UINT App_ThreadX_Init(VOID *memory_ptr)
 {
-  UINT ret = TX_SUCCESS;
-  /* USER CODE BEGIN App_ThreadX_MEM_POOL */
+    HCBle_SendData("进入 App_ThreadX_Init\r\n");
+//	HCBle_SendData("%d",sizeof(ULONG));
+	UINT status;

-  /* USER CODE END App_ThreadX_MEM_POOL */
-  /* USER CODE BEGIN App_ThreadX_Init */
-  /* USER CODE END App_ThreadX_Init */
+	// === 创建 BLE RX 线程 ===
+    status = tx_thread_create(&ble_rx_thread, "BLE RX Thread",
+                              ble_rx_task_entry, 0,
+                              ble_rx_stack, BLE_RX_THREAD_STACK_SIZE,
+                              BLE_RX_THREAD_PRIORITY, BLE_RX_THREAD_PRIORITY,
+                              TX_NO_TIME_SLICE, TX_AUTO_START);
+	
+	if(status != TX_SUCCESS)
+	{
+		return status;
+	}
+	
+    // === 创建 BLE TX 线程 ===
+    status = tx_thread_create(&ble_tx_thread, "BLE TX Thread",
+                              ble_tx_task_entry, 0,
+                              ble_tx_stack, BLE_TX_THREAD_STACK_SIZE,
+                              BLE_TX_THREAD_PRIORITY, BLE_TX_THREAD_PRIORITY,
+                              TX_NO_TIME_SLICE, TX_AUTO_START);
+	
+    if (status != TX_SUCCESS) {
+        HCBle_SendData("❌ BLE TX 线程创建失败，错误码=%d\r\n", status);
+        return status;
+    }
+	
+	    // === 创建 BLE TX 消息队列 ===
+	status = tx_queue_create(&ble_tx_queue, "BLE TX Queue",
+							 (BLE_TX_MSG_LEN + sizeof(ULONG) - 1) / sizeof(ULONG),
+							 ble_tx_queue_buffer,
+							 sizeof(ble_tx_queue_buffer));

-  return ret;
+
+     if (status != TX_SUCCESS) {
+        HCBle_SendData("❌ BLE TX 消息队列创建失败，错误码=%d\r\n", status);
+        return status;
+    }
+
+	status = tx_thread_create(&imu_angle_thread, "IMU Angle Thread",
+                          imu_angle_ble_task_entry, 0,
+                          imu_angle_stack, IMU_ANGLE_THREAD_STACK_SIZE,
+                          IMU_ANGLE_THREAD_PRIORITY, IMU_ANGLE_THREAD_PRIORITY,
+                          TX_NO_TIME_SLICE, TX_AUTO_START);
+
+	if (status != TX_SUCCESS) {
+		return status;
+	}
+
+	
+	status = tx_queue_create(&im948_uart_rx_queue, "IM948_RX_QUEUE",
+                               TX_1_ULONG, // sizeof(ULONG) bytes per entry
+                               im948_rx_queue_buffer,
+                               IM948_RX_QUEUE_SIZE * sizeof(ULONG));
+	if(status != TX_SUCCESS)
+	{
+		return status;
+	}
+	
+	status = tx_thread_create(&gps_task,
+                  "GPS Task",
+                  gps_thread_entry,
+                  0,
+                  gps_task_stack,
+                  GPS_TASK_STACK_SIZE,
+                  GPS_TASK_PRIORITY,
+                  GPS_TASK_PRIORITY,
+                  TX_NO_TIME_SLICE,
+                  TX_AUTO_START);
+	
+	if(status != TX_SUCCESS)
+	{
+		return status;
+	}
+	
+	// obstacle_thread create
+	status = tx_thread_create(&obstacle_thread,
+							"obstacle",
+							obstacle_thread_entry,
+							0,obstacle_stack,sizeof(obstacle_stack),8,8,TX_NO_TIME_SLICE,TX_AUTO_START);
+	
+	if(status != TX_SUCCESS)
+	{
+		return status;
+	}
+	
+	 status = tx_thread_create(&ultrasonic_task_thread,
+                     "Ultrasonic",
+                     ultrasonic_task_entry,
+                     0,
+                     ultrasonic_stack,
+                     sizeof(ultrasonic_stack),
+                     7, 7,    // 这里的优先级
+                     TX_NO_TIME_SLICE,
+                     TX_AUTO_START);
+		
+		if(status != TX_SUCCESS)
+	{
+		return status;
+	}
+	
+	//
+//    HCBle_SendData("✅ BLE RX/TX 线程和队列初始化完成\r\n");
+					 
+//	status = ControlThreadCreate();
+//	if(status != TX_SUCCESS)
+//	{
+//		return status;
+//	}			 
+//	status = Encoder_ThreadCreate();	
+//	if(status != TX_SUCCESS)
+//	{
+//		return status;
+//	}	
+	
+    return TX_SUCCESS;
 }

+
  /**
  * @brief  Function that implements the kernel's initialization.
  * @param  None
@@ -76,9 +244,8 @@ UINT App_ThreadX_Init(VOID *memory_ptr)
 void MX_ThreadX_Init(void)
 {
  /* USER CODE BEGIN Before_Kernel_Start */
-
+	HCBle_InitEventFlags();  // 这必须在任何使用前调用一次
  /* USER CODE END Before_Kernel_Start */
-
  tx_kernel_enter();

  /* USER CODE BEGIN Kernel_Start_Error */
@@ -87,5 +254,48 @@ void MX_ThreadX_Init(void)
 }

 /* USER CODE BEGIN 1 */
+//新加入的 obstacle 
+static void obstacle_thread_entry(ULONG arg)
+{
+	while(1)
+	{
+		ULONG evt; // 这个应该是用来接收数据的
+		tx_event_flags_get(&response_events,EVENT_OBSTACLE_DETECTED,TX_OR_CLEAR,&evt,TX_WAIT_FOREVER);
+		
+		switch(obstacle_level)
+		{
+			case 1: // 远
+				Buzzer_Open();
+				Shake_Motor_Open();
+				tx_thread_sleep(30);
+				Buzzer_Close();
+				Shake_Motor_Close();
+				break;
+			
+			case 2:
+				for(int i = 0; i < 2;i++)
+				{
+					Buzzer_Open();
+					Shake_Motor_Open();
+					tx_thread_sleep(50);
+					Buzzer_Close();
+                    Shake_Motor_Close();
+                    tx_thread_sleep(30);
+				}
+				break;
+				
+			case 3: // 近
+				Buzzer_Open();
+				Shake_Motor_Open();
+				tx_thread_sleep(150);
+				Buzzer_Close();
+				Shake_Motor_Close();
+				break;
+			default:
+				break;
+		}
+		tx_thread_sleep(20);
+	}
+}

 /* USER CODE END 1 */
--- a/Core/Src/gpdma.c
+++ b/Core/Src/gpdma.c
@@ -36,6 +36,8 @@ void MX_GPDMA1_Init(void)
  __HAL_RCC_GPDMA1_CLK_ENABLE();

  /* GPDMA1 interrupt Init */
+    HAL_NVIC_SetPriority(GPDMA1_Channel3_IRQn, 0, 0);
+    HAL_NVIC_EnableIRQ(GPDMA1_Channel3_IRQn);
    HAL_NVIC_SetPriority(GPDMA1_Channel4_IRQn, 0, 0);
    HAL_NVIC_EnableIRQ(GPDMA1_Channel4_IRQn);
    HAL_NVIC_SetPriority(GPDMA1_Channel5_IRQn, 0, 0);
--- a/Core/Src/gpio.c
+++ b/Core/Src/gpio.c
@@ -34,13 +34,60 @@

 /** Configure pins
     PH0-OSC_IN(PH0)   ------> RCC_OSC_IN
+     PH1-OSC_OUT(PH1)   ------> RCC_OSC_OUT
+     PA13(JTMS/SWDIO)   ------> DEBUG_JTMS-SWDIO
+     PA14(JTCK/SWCLK)   ------> DEBUG_JTCK-SWCLK
 */
 void MX_GPIO_Init(void)
 {

+  GPIO_InitTypeDef GPIO_InitStruct = {0};
+
  /* GPIO Ports Clock Enable */
  __HAL_RCC_GPIOH_CLK_ENABLE();
+  __HAL_RCC_GPIOC_CLK_ENABLE();
+  __HAL_RCC_GPIOA_CLK_ENABLE();
  __HAL_RCC_GPIOB_CLK_ENABLE();
+  __HAL_RCC_GPIOG_CLK_ENABLE();
+  __HAL_RCC_GPIOE_CLK_ENABLE();
+  __HAL_RCC_GPIOD_CLK_ENABLE();
+
+  /*Configure GPIO pin Output Level */
+  HAL_GPIO_WritePin(GPIOG, AIN1_Pin|AIN2_Pin|BIN1_Pin|BIN2_Pin, GPIO_PIN_RESET);
+
+  /*Configure GPIO pin Output Level */
+  HAL_GPIO_WritePin(GPIOC, HC_Trig_Pin|Shake_Motor_Pin, GPIO_PIN_RESET);
+
+  /*Configure GPIO pin Output Level */
+  HAL_GPIO_WritePin(Buzzer_GPIO_Port, Buzzer_Pin, GPIO_PIN_RESET);
+
+  /*Configure GPIO pins : AIN1_Pin AIN2_Pin BIN1_Pin BIN2_Pin */
+  GPIO_InitStruct.Pin = AIN1_Pin|AIN2_Pin|BIN1_Pin|BIN2_Pin;
+  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
+  GPIO_InitStruct.Pull = GPIO_NOPULL;
+  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+  HAL_GPIO_Init(GPIOG, &GPIO_InitStruct);
+
+  /*Configure GPIO pin : HC_Trig_Pin */
+  GPIO_InitStruct.Pin = HC_Trig_Pin;
+  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
+  GPIO_InitStruct.Pull = GPIO_NOPULL;
+  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+  HAL_GPIO_Init(HC_Trig_GPIO_Port, &GPIO_InitStruct);
+
+  /*Configure GPIO pin : Shake_Motor_Pin */
+  GPIO_InitStruct.Pin = Shake_Motor_Pin;
+  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
+  GPIO_InitStruct.Pull = GPIO_PULLDOWN;
+  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+  HAL_GPIO_Init(Shake_Motor_GPIO_Port, &GPIO_InitStruct);
+
+  /*Configure GPIO pin : Buzzer_Pin */
+  GPIO_InitStruct.Pin = Buzzer_Pin;
+  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
+  GPIO_InitStruct.Pull = GPIO_PULLDOWN;
+  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+  HAL_GPIO_Init(Buzzer_GPIO_Port, &GPIO_InitStruct);

 }

--- a/Core/Src/main.c
+++ b/Core/Src/main.c
@@ -21,6 +21,7 @@
 #include "main.h"
 #include "gpdma.h"
 #include "memorymap.h"
+#include "tim.h"
 #include "usart.h"
 #include "gpio.h"

@@ -47,7 +48,7 @@
 /* Private variables ---------------------------------------------------------*/

 /* USER CODE BEGIN PV */
-
+uint8_t rx_byte; // imu_<75><5F><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
 /* USER CODE END PV */

 /* Private function prototypes -----------------------------------------------*/
@@ -91,9 +92,26 @@ int main(void)
  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_GPDMA1_Init();
-  MX_USART1_UART_Init();
+  MX_USART2_UART_Init();
+  MX_TIM2_Init();
+  MX_TIM3_Init();
+  MX_USART3_UART_Init();
+  MX_TIM4_Init();
+  MX_UART4_Init();
+  MX_TIM1_Init();
+  MX_TIM8_Init();
+  MX_TIM5_Init();
  /* USER CODE BEGIN 2 */

+   imu600_init();
+   GPS_Init();
+   DWT_Init();
+   PWM_GPIO_TIM_Init();
+   HAL_TIM_IC_Start_IT(&htim5,TIM_CHANNEL_1);
+//   				Buzzer_Open();
+//	HCBle_InitDMAReception();
+//	HAL_Delay(200);
+//	GPS_Init();
  /* USER CODE END 2 */

  MX_ThreadX_Init();
@@ -174,7 +192,7 @@ void SystemClock_Config(void)

 /**
  * @brief  Period elapsed callback in non blocking mode
-  * @note   This function is called  when TIM1 interrupt took place, inside
+  * @note   This function is called  when TIM6 interrupt took place, inside
  * HAL_TIM_IRQHandler(). It makes a direct call to HAL_IncTick() to increment
  * a global variable "uwTick" used as application time base.
  * @param  htim : TIM handle
@@ -185,7 +203,7 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
  /* USER CODE BEGIN Callback 0 */

  /* USER CODE END Callback 0 */
-  if (htim->Instance == TIM1)
+  if (htim->Instance == TIM6)
  {
    HAL_IncTick();
  }
--- a/Core/Src/stm32h5xx_hal_timebase_tim.c
+++ b/Core/Src/stm32h5xx_hal_timebase_tim.c
@@ -25,12 +25,12 @@
 /* Private define ------------------------------------------------------------*/
 /* Private macro -------------------------------------------------------------*/
 /* Private variables ---------------------------------------------------------*/
-TIM_HandleTypeDef        htim1;
+TIM_HandleTypeDef        htim6;
 /* Private function prototypes -----------------------------------------------*/
 /* Private functions ---------------------------------------------------------*/

 /**
-  * @brief  This function configures the TIM1 as a time base source.
+  * @brief  This function configures the TIM6 as a time base source.
  *         The time source is configured  to have 1ms time base with a dedicated
  *         Tick interrupt priority.
  * @note   This function is called  automatically at the beginning of program after
@@ -41,49 +41,58 @@ TIM_HandleTypeDef        htim1;
 HAL_StatusTypeDef HAL_InitTick(uint32_t TickPriority)
 {
  RCC_ClkInitTypeDef    clkconfig;
-  uint32_t              uwTimclock;
+  uint32_t              uwTimclock, uwAPB1Prescaler;
  uint32_t              uwPrescalerValue;
  uint32_t              pFLatency;

  HAL_StatusTypeDef     status;

-  /* Enable TIM1 clock */
-  __HAL_RCC_TIM1_CLK_ENABLE();
+  /* Enable TIM6 clock */
+  __HAL_RCC_TIM6_CLK_ENABLE();

  /* Get clock configuration */
  HAL_RCC_GetClockConfig(&clkconfig, &pFLatency);

-  /* Compute TIM1 clock */
-      uwTimclock = HAL_RCC_GetPCLK2Freq();
+  /* Get APB1 prescaler */
+  uwAPB1Prescaler = clkconfig.APB1CLKDivider;
+  /* Compute TIM6 clock */
+  if (uwAPB1Prescaler == RCC_HCLK_DIV1)
+  {
+    uwTimclock = HAL_RCC_GetPCLK1Freq();
+  }
+  else
+  {
+    uwTimclock = 2UL * HAL_RCC_GetPCLK1Freq();
+  }

-  /* Compute the prescaler value to have TIM1 counter clock equal to 100KHz */
+  /* Compute the prescaler value to have TIM6 counter clock equal to 100KHz */
  uwPrescalerValue = (uint32_t) ((uwTimclock / 100000U) - 1U);

-  /* Initialize TIM1 */
-  htim1.Instance = TIM1;
+  /* Initialize TIM6 */
+  htim6.Instance = TIM6;

  /* Initialize TIMx peripheral as follow:
-   * Period = [(TIM1CLK/1000) - 1]. to have a (1/1000) s time base.
+   * Period = [(TIM6CLK/1000) - 1]. to have a (1/1000) s time base.
   * Prescaler = (uwTimclock/100000 - 1) to have a 100KHz counter clock.
   * ClockDivision = 0
   * Counter direction = Up
   */
-  htim1.Init.Period = (100000U / 1000U) - 1U;
-  htim1.Init.Prescaler = uwPrescalerValue;
-  htim1.Init.ClockDivision = 0;
-  htim1.Init.CounterMode = TIM_COUNTERMODE_UP;
+  htim6.Init.Period = (100000U / 1000U) - 1U;
+  htim6.Init.Prescaler = uwPrescalerValue;
+  htim6.Init.ClockDivision = 0;
+  htim6.Init.CounterMode = TIM_COUNTERMODE_UP;

-  status = HAL_TIM_Base_Init(&htim1);
+  status = HAL_TIM_Base_Init(&htim6);
  if (status == HAL_OK)
  {
    /* Start the TIM time Base generation in interrupt mode */
-    status = HAL_TIM_Base_Start_IT(&htim1);
+    status = HAL_TIM_Base_Start_IT(&htim6);
    if (status == HAL_OK)
    {
      if (TickPriority < (1UL << __NVIC_PRIO_BITS))
      {
-        /* Enable the TIM1 global Interrupt */
-        HAL_NVIC_SetPriority(TIM1_UP_IRQn, TickPriority, 0U);
+        /* Enable the TIM6 global Interrupt */
+        HAL_NVIC_SetPriority(TIM6_IRQn, TickPriority, 0U);
        uwTickPrio = TickPriority;
      }
      else
@@ -93,8 +102,8 @@ HAL_StatusTypeDef HAL_InitTick(uint32_t TickPriority)
    }
 }

-  /* Enable the TIM1 global Interrupt */
-  HAL_NVIC_EnableIRQ(TIM1_UP_IRQn);
+  /* Enable the TIM6 global Interrupt */
+  HAL_NVIC_EnableIRQ(TIM6_IRQn);

  /* Return function status */
  return status;
@@ -102,25 +111,25 @@ HAL_StatusTypeDef HAL_InitTick(uint32_t TickPriority)

 /**
  * @brief  Suspend Tick increment.
-  * @note   Disable the tick increment by disabling TIM1 update interrupt.
+  * @note   Disable the tick increment by disabling TIM6 update interrupt.
  * @param  None
  * @retval None
  */
 void HAL_SuspendTick(void)
 {
-  /* Disable TIM1 update Interrupt */
-  __HAL_TIM_DISABLE_IT(&htim1, TIM_IT_UPDATE);
+  /* Disable TIM6 update Interrupt */
+  __HAL_TIM_DISABLE_IT(&htim6, TIM_IT_UPDATE);
 }

 /**
  * @brief  Resume Tick increment.
-  * @note   Enable the tick increment by Enabling TIM1 update interrupt.
+  * @note   Enable the tick increment by Enabling TIM6 update interrupt.
  * @param  None
  * @retval None
  */
 void HAL_ResumeTick(void)
 {
-  /* Enable TIM1 Update interrupt */
-  __HAL_TIM_ENABLE_IT(&htim1, TIM_IT_UPDATE);
+  /* Enable TIM6 Update interrupt */
+  __HAL_TIM_ENABLE_IT(&htim6, TIM_IT_UPDATE);
 }

--- a/Core/Src/stm32h5xx_it.c
+++ b/Core/Src/stm32h5xx_it.c
@@ -56,12 +56,20 @@
 /* USER CODE END 0 */

 /* External variables --------------------------------------------------------*/
+extern TIM_HandleTypeDef htim2;
+extern TIM_HandleTypeDef htim3;
+extern TIM_HandleTypeDef htim5;
 extern DMA_NodeTypeDef Node_GPDMA1_Channel5;
 extern DMA_QListTypeDef List_GPDMA1_Channel5;
 extern DMA_HandleTypeDef handle_GPDMA1_Channel5;
 extern DMA_HandleTypeDef handle_GPDMA1_Channel4;
-extern UART_HandleTypeDef huart1;
-extern TIM_HandleTypeDef htim1;
+extern DMA_NodeTypeDef Node_GPDMA1_Channel3;
+extern DMA_QListTypeDef List_GPDMA1_Channel3;
+extern DMA_HandleTypeDef handle_GPDMA1_Channel3;
+extern UART_HandleTypeDef huart4;
+extern UART_HandleTypeDef huart2;
+extern UART_HandleTypeDef huart3;
+extern TIM_HandleTypeDef htim6;

 /* USER CODE BEGIN EV */

@@ -165,6 +173,20 @@ void DebugMon_Handler(void)
 /* please refer to the startup file (startup_stm32h5xx.s).                    */
 /******************************************************************************/

+/**
+  * @brief This function handles GPDMA1 Channel 3 global interrupt.
+  */
+void GPDMA1_Channel3_IRQHandler(void)
+{
+  /* USER CODE BEGIN GPDMA1_Channel3_IRQn 0 */
+
+  /* USER CODE END GPDMA1_Channel3_IRQn 0 */
+  HAL_DMA_IRQHandler(&handle_GPDMA1_Channel3);
+  /* USER CODE BEGIN GPDMA1_Channel3_IRQn 1 */
+
+  /* USER CODE END GPDMA1_Channel3_IRQn 1 */
+}
+
 /**
  * @brief This function handles GPDMA1 Channel 4 global interrupt.
  */
@@ -194,45 +216,101 @@ void GPDMA1_Channel5_IRQHandler(void)
 }

 /**
-  * @brief This function handles TIM1 Update interrupt.
+  * @brief This function handles TIM2 global interrupt.
  */
-void TIM1_UP_IRQHandler(void)
+void TIM2_IRQHandler(void)
 {
-  /* USER CODE BEGIN TIM1_UP_IRQn 0 */
+  /* USER CODE BEGIN TIM2_IRQn 0 */

-  /* USER CODE END TIM1_UP_IRQn 0 */
-  HAL_TIM_IRQHandler(&htim1);
-  /* USER CODE BEGIN TIM1_UP_IRQn 1 */
+  /* USER CODE END TIM2_IRQn 0 */
+  HAL_TIM_IRQHandler(&htim2);
+  /* USER CODE BEGIN TIM2_IRQn 1 */

-  /* USER CODE END TIM1_UP_IRQn 1 */
+  /* USER CODE END TIM2_IRQn 1 */
 }

 /**
-  * @brief This function handles USART1 global interrupt.
+  * @brief This function handles TIM3 global interrupt.
  */
-void USART1_IRQHandler(void)
+void TIM3_IRQHandler(void)
 {
-  /* USER CODE BEGIN USART1_IRQn 0 */
+  /* USER CODE BEGIN TIM3_IRQn 0 */

-  /* USER CODE END USART1_IRQn 0 */
-  HAL_UART_IRQHandler(&huart1);
-  /* USER CODE BEGIN USART1_IRQn 1 */
-  if(__HAL_UART_GET_FLAG(&huart1,UART_FLAG_IDLE))
-  {
-	  __HAL_UART_CLEAR_IDLEFLAG(&huart1);
-	  //<2F><><EFBFBD><EFBFBD><EFBFBD>Ǽ<EFBFBD><C7BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ַ<EFBFBD><D6B7><EFBFBD><EFBFBD><EFBFBD>
-	  uint16_t len = UART_DMA_RX_BUF_SIZE - __HAL_DMA_GET_COUNTER(huart1.hdmarx);
-	  
-	  for(uint16_t i = 0;i < len; i++)
-	  {
-		  ble_rx_ring.buffer[ble_rx_ring.head] = uart_dma_rx_buf[i];
-		  ble_rx_ring.head = (ble_rx_ring.head + 1) %RING_BUFFER_SIZE;
-	  }
-	  
-	  HAL_UART_AbortReceive(&huart1);  //ֹͣ<CDA3><D6B9>ǰDMA
-	  HCBle_InitDMAReception();
-  }
-  /* USER CODE END USART1_IRQn 1 */
+  /* USER CODE END TIM3_IRQn 0 */
+  HAL_TIM_IRQHandler(&htim3);
+  /* USER CODE BEGIN TIM3_IRQn 1 */
+
+  /* USER CODE END TIM3_IRQn 1 */
+}
+
+/**
+  * @brief This function handles TIM5 global interrupt.
+  */
+void TIM5_IRQHandler(void)
+{
+  /* USER CODE BEGIN TIM5_IRQn 0 */
+
+  /* USER CODE END TIM5_IRQn 0 */
+  HAL_TIM_IRQHandler(&htim5);
+  /* USER CODE BEGIN TIM5_IRQn 1 */
+
+  /* USER CODE END TIM5_IRQn 1 */
+}
+
+/**
+  * @brief This function handles TIM6 global interrupt.
+  */
+void TIM6_IRQHandler(void)
+{
+  /* USER CODE BEGIN TIM6_IRQn 0 */
+
+  /* USER CODE END TIM6_IRQn 0 */
+  HAL_TIM_IRQHandler(&htim6);
+  /* USER CODE BEGIN TIM6_IRQn 1 */
+
+  /* USER CODE END TIM6_IRQn 1 */
+}
+
+/**
+  * @brief This function handles USART2 global interrupt.
+  */
+void USART2_IRQHandler(void)
+{
+  /* USER CODE BEGIN USART2_IRQn 0 */
+
+  /* USER CODE END USART2_IRQn 0 */
+  HAL_UART_IRQHandler(&huart2);
+  /* USER CODE BEGIN USART2_IRQn 1 */
+
+  /* USER CODE END USART2_IRQn 1 */
+}
+
+/**
+  * @brief This function handles USART3 global interrupt.
+  */
+void USART3_IRQHandler(void)
+{
+  /* USER CODE BEGIN USART3_IRQn 0 */
+
+  /* USER CODE END USART3_IRQn 0 */
+  HAL_UART_IRQHandler(&huart3);
+  /* USER CODE BEGIN USART3_IRQn 1 */
+
+  /* USER CODE END USART3_IRQn 1 */
+}
+
+/**
+  * @brief This function handles UART4 global interrupt.
+  */
+void UART4_IRQHandler(void)
+{
+  /* USER CODE BEGIN UART4_IRQn 0 */
+
+  /* USER CODE END UART4_IRQn 0 */
+  HAL_UART_IRQHandler(&huart4);
+  /* USER CODE BEGIN UART4_IRQn 1 */
+
+  /* USER CODE END UART4_IRQn 1 */
 }

 /* USER CODE BEGIN 1 */
--- a/Core/Src/tim.c
+++ b/Core/Src/tim.c
@@ -0,0 +1,608 @@
+/* USER CODE BEGIN Header */
+/**
+  ******************************************************************************
+  * @file    tim.c
+  * @brief   This file provides code for the configuration
+  *          of the TIM instances.
+  ******************************************************************************
+  * @attention
+  *
+  * Copyright (c) 2025 STMicroelectronics.
+  * All rights reserved.
+  *
+  * This software is licensed under terms that can be found in the LICENSE file
+  * in the root directory of this software component.
+  * If no LICENSE file comes with this software, it is provided AS-IS.
+  *
+  ******************************************************************************
+  */
+/* USER CODE END Header */
+/* Includes ------------------------------------------------------------------*/
+#include "tim.h"
+
+/* USER CODE BEGIN 0 */
+
+/* USER CODE END 0 */
+
+TIM_HandleTypeDef htim1;
+TIM_HandleTypeDef htim2;
+TIM_HandleTypeDef htim3;
+TIM_HandleTypeDef htim4;
+TIM_HandleTypeDef htim5;
+TIM_HandleTypeDef htim8;
+
+/* TIM1 init function */
+void MX_TIM1_Init(void)
+{
+
+  /* USER CODE BEGIN TIM1_Init 0 */
+
+  /* USER CODE END TIM1_Init 0 */
+
+  TIM_Encoder_InitTypeDef sConfig = {0};
+  TIM_MasterConfigTypeDef sMasterConfig = {0};
+
+  /* USER CODE BEGIN TIM1_Init 1 */
+
+  /* USER CODE END TIM1_Init 1 */
+  htim1.Instance = TIM1;
+  htim1.Init.Prescaler = 0;
+  htim1.Init.CounterMode = TIM_COUNTERMODE_UP;
+  htim1.Init.Period = 65535;
+  htim1.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
+  htim1.Init.RepetitionCounter = 0;
+  htim1.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
+  sConfig.EncoderMode = TIM_ENCODERMODE_TI12;
+  sConfig.IC1Polarity = TIM_ICPOLARITY_RISING;
+  sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI;
+  sConfig.IC1Prescaler = TIM_ICPSC_DIV1;
+  sConfig.IC1Filter = 0;
+  sConfig.IC2Polarity = TIM_ICPOLARITY_RISING;
+  sConfig.IC2Selection = TIM_ICSELECTION_DIRECTTI;
+  sConfig.IC2Prescaler = TIM_ICPSC_DIV1;
+  sConfig.IC2Filter = 0;
+  if (HAL_TIM_Encoder_Init(&htim1, &sConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
+  sMasterConfig.MasterOutputTrigger2 = TIM_TRGO2_RESET;
+  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
+  if (HAL_TIMEx_MasterConfigSynchronization(&htim1, &sMasterConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  /* USER CODE BEGIN TIM1_Init 2 */
+
+  /* USER CODE END TIM1_Init 2 */
+
+}
+/* TIM2 init function */
+void MX_TIM2_Init(void)
+{
+
+  /* USER CODE BEGIN TIM2_Init 0 */
+
+  /* USER CODE END TIM2_Init 0 */
+
+  TIM_ClockConfigTypeDef sClockSourceConfig = {0};
+  TIM_MasterConfigTypeDef sMasterConfig = {0};
+
+  /* USER CODE BEGIN TIM2_Init 1 */
+
+  /* USER CODE END TIM2_Init 1 */
+  htim2.Instance = TIM2;
+  htim2.Init.Prescaler = 250 - 1;
+  htim2.Init.CounterMode = TIM_COUNTERMODE_UP;
+  htim2.Init.Period = 4294967295;
+  htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
+  htim2.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
+  if (HAL_TIM_Base_Init(&htim2) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
+  if (HAL_TIM_ConfigClockSource(&htim2, &sClockSourceConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
+  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
+  if (HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  /* USER CODE BEGIN TIM2_Init 2 */
+
+  /* USER CODE END TIM2_Init 2 */
+
+}
+/* TIM3 init function */
+void MX_TIM3_Init(void)
+{
+
+  /* USER CODE BEGIN TIM3_Init 0 */
+
+  /* USER CODE END TIM3_Init 0 */
+
+  TIM_ClockConfigTypeDef sClockSourceConfig = {0};
+  TIM_MasterConfigTypeDef sMasterConfig = {0};
+  TIM_OC_InitTypeDef sConfigOC = {0};
+
+  /* USER CODE BEGIN TIM3_Init 1 */
+
+  /* USER CODE END TIM3_Init 1 */
+  htim3.Instance = TIM3;
+  htim3.Init.Prescaler = 48;
+  htim3.Init.CounterMode = TIM_COUNTERMODE_UP;
+  htim3.Init.Period = 255;
+  htim3.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
+  htim3.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
+  if (HAL_TIM_Base_Init(&htim3) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
+  if (HAL_TIM_ConfigClockSource(&htim3, &sClockSourceConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  if (HAL_TIM_PWM_Init(&htim3) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
+  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
+  if (HAL_TIMEx_MasterConfigSynchronization(&htim3, &sMasterConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  sConfigOC.OCMode = TIM_OCMODE_PWM1;
+  sConfigOC.Pulse = 0;
+  sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
+  sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
+  if (HAL_TIM_PWM_ConfigChannel(&htim3, &sConfigOC, TIM_CHANNEL_4) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  /* USER CODE BEGIN TIM3_Init 2 */
+
+  /* USER CODE END TIM3_Init 2 */
+  HAL_TIM_MspPostInit(&htim3);
+
+}
+/* TIM4 init function */
+void MX_TIM4_Init(void)
+{
+
+  /* USER CODE BEGIN TIM4_Init 0 */
+
+  /* USER CODE END TIM4_Init 0 */
+
+  TIM_ClockConfigTypeDef sClockSourceConfig = {0};
+  TIM_MasterConfigTypeDef sMasterConfig = {0};
+  TIM_OC_InitTypeDef sConfigOC = {0};
+
+  /* USER CODE BEGIN TIM4_Init 1 */
+
+  /* USER CODE END TIM4_Init 1 */
+  htim4.Instance = TIM4;
+  htim4.Init.Prescaler = 48;
+  htim4.Init.CounterMode = TIM_COUNTERMODE_UP;
+  htim4.Init.Period = 255;
+  htim4.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
+  htim4.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
+  if (HAL_TIM_Base_Init(&htim4) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
+  if (HAL_TIM_ConfigClockSource(&htim4, &sClockSourceConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  if (HAL_TIM_PWM_Init(&htim4) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
+  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
+  if (HAL_TIMEx_MasterConfigSynchronization(&htim4, &sMasterConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  sConfigOC.OCMode = TIM_OCMODE_PWM1;
+  sConfigOC.Pulse = 0;
+  sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
+  sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
+  if (HAL_TIM_PWM_ConfigChannel(&htim4, &sConfigOC, TIM_CHANNEL_4) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  /* USER CODE BEGIN TIM4_Init 2 */
+
+  /* USER CODE END TIM4_Init 2 */
+  HAL_TIM_MspPostInit(&htim4);
+
+}
+/* TIM5 init function */
+void MX_TIM5_Init(void)
+{
+
+  /* USER CODE BEGIN TIM5_Init 0 */
+
+  /* USER CODE END TIM5_Init 0 */
+
+  TIM_ClockConfigTypeDef sClockSourceConfig = {0};
+  TIM_MasterConfigTypeDef sMasterConfig = {0};
+  TIM_IC_InitTypeDef sConfigIC = {0};
+
+  /* USER CODE BEGIN TIM5_Init 1 */
+
+  /* USER CODE END TIM5_Init 1 */
+  htim5.Instance = TIM5;
+  htim5.Init.Prescaler = 250-1;
+  htim5.Init.CounterMode = TIM_COUNTERMODE_UP;
+  htim5.Init.Period = 4294967295;
+  htim5.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
+  htim5.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
+  if (HAL_TIM_Base_Init(&htim5) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
+  if (HAL_TIM_ConfigClockSource(&htim5, &sClockSourceConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  if (HAL_TIM_IC_Init(&htim5) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
+  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
+  if (HAL_TIMEx_MasterConfigSynchronization(&htim5, &sMasterConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  sConfigIC.ICPolarity = TIM_INPUTCHANNELPOLARITY_RISING;
+  sConfigIC.ICSelection = TIM_ICSELECTION_DIRECTTI;
+  sConfigIC.ICPrescaler = TIM_ICPSC_DIV1;
+  sConfigIC.ICFilter = 0;
+  if (HAL_TIM_IC_ConfigChannel(&htim5, &sConfigIC, TIM_CHANNEL_1) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  /* USER CODE BEGIN TIM5_Init 2 */
+
+  /* USER CODE END TIM5_Init 2 */
+
+}
+/* TIM8 init function */
+void MX_TIM8_Init(void)
+{
+
+  /* USER CODE BEGIN TIM8_Init 0 */
+
+  /* USER CODE END TIM8_Init 0 */
+
+  TIM_Encoder_InitTypeDef sConfig = {0};
+  TIM_MasterConfigTypeDef sMasterConfig = {0};
+
+  /* USER CODE BEGIN TIM8_Init 1 */
+
+  /* USER CODE END TIM8_Init 1 */
+  htim8.Instance = TIM8;
+  htim8.Init.Prescaler = 0;
+  htim8.Init.CounterMode = TIM_COUNTERMODE_UP;
+  htim8.Init.Period = 65535;
+  htim8.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
+  htim8.Init.RepetitionCounter = 0;
+  htim8.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
+  sConfig.EncoderMode = TIM_ENCODERMODE_TI12;
+  sConfig.IC1Polarity = TIM_ICPOLARITY_RISING;
+  sConfig.IC1Selection = TIM_ICSELECTION_DIRECTTI;
+  sConfig.IC1Prescaler = TIM_ICPSC_DIV1;
+  sConfig.IC1Filter = 0;
+  sConfig.IC2Polarity = TIM_ICPOLARITY_RISING;
+  sConfig.IC2Selection = TIM_ICSELECTION_DIRECTTI;
+  sConfig.IC2Prescaler = TIM_ICPSC_DIV1;
+  sConfig.IC2Filter = 0;
+  if (HAL_TIM_Encoder_Init(&htim8, &sConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
+  sMasterConfig.MasterOutputTrigger2 = TIM_TRGO2_RESET;
+  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
+  if (HAL_TIMEx_MasterConfigSynchronization(&htim8, &sMasterConfig) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  /* USER CODE BEGIN TIM8_Init 2 */
+
+  /* USER CODE END TIM8_Init 2 */
+
+}
+
+void HAL_TIM_Encoder_MspInit(TIM_HandleTypeDef* tim_encoderHandle)
+{
+
+  GPIO_InitTypeDef GPIO_InitStruct = {0};
+  if(tim_encoderHandle->Instance==TIM1)
+  {
+  /* USER CODE BEGIN TIM1_MspInit 0 */
+
+  /* USER CODE END TIM1_MspInit 0 */
+    /* TIM1 clock enable */
+    __HAL_RCC_TIM1_CLK_ENABLE();
+
+    __HAL_RCC_GPIOE_CLK_ENABLE();
+    /**TIM1 GPIO Configuration
+    PE9     ------> TIM1_CH1
+    PE11     ------> TIM1_CH2
+    */
+    GPIO_InitStruct.Pin = E1A_Pin|E1B_Pin;
+    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
+    GPIO_InitStruct.Pull = GPIO_PULLUP;
+    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+    GPIO_InitStruct.Alternate = GPIO_AF1_TIM1;
+    HAL_GPIO_Init(GPIOE, &GPIO_InitStruct);
+
+  /* USER CODE BEGIN TIM1_MspInit 1 */
+
+  /* USER CODE END TIM1_MspInit 1 */
+  }
+  else if(tim_encoderHandle->Instance==TIM8)
+  {
+  /* USER CODE BEGIN TIM8_MspInit 0 */
+
+  /* USER CODE END TIM8_MspInit 0 */
+    /* TIM8 clock enable */
+    __HAL_RCC_TIM8_CLK_ENABLE();
+
+    __HAL_RCC_GPIOC_CLK_ENABLE();
+    /**TIM8 GPIO Configuration
+    PC6     ------> TIM8_CH1
+    PC7     ------> TIM8_CH2
+    */
+    GPIO_InitStruct.Pin = E2A_Pin|E2B_Pin;
+    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
+    GPIO_InitStruct.Pull = GPIO_PULLUP;
+    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+    GPIO_InitStruct.Alternate = GPIO_AF3_TIM8;
+    HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
+
+  /* USER CODE BEGIN TIM8_MspInit 1 */
+
+  /* USER CODE END TIM8_MspInit 1 */
+  }
+}
+
+void HAL_TIM_Base_MspInit(TIM_HandleTypeDef* tim_baseHandle)
+{
+
+  GPIO_InitTypeDef GPIO_InitStruct = {0};
+  if(tim_baseHandle->Instance==TIM2)
+  {
+  /* USER CODE BEGIN TIM2_MspInit 0 */
+
+  /* USER CODE END TIM2_MspInit 0 */
+    /* TIM2 clock enable */
+    __HAL_RCC_TIM2_CLK_ENABLE();
+
+    /* TIM2 interrupt Init */
+    HAL_NVIC_SetPriority(TIM2_IRQn, 0, 0);
+    HAL_NVIC_EnableIRQ(TIM2_IRQn);
+  /* USER CODE BEGIN TIM2_MspInit 1 */
+
+  /* USER CODE END TIM2_MspInit 1 */
+  }
+  else if(tim_baseHandle->Instance==TIM3)
+  {
+  /* USER CODE BEGIN TIM3_MspInit 0 */
+
+  /* USER CODE END TIM3_MspInit 0 */
+    /* TIM3 clock enable */
+    __HAL_RCC_TIM3_CLK_ENABLE();
+
+    /* TIM3 interrupt Init */
+    HAL_NVIC_SetPriority(TIM3_IRQn, 0, 0);
+    HAL_NVIC_EnableIRQ(TIM3_IRQn);
+  /* USER CODE BEGIN TIM3_MspInit 1 */
+
+  /* USER CODE END TIM3_MspInit 1 */
+  }
+  else if(tim_baseHandle->Instance==TIM4)
+  {
+  /* USER CODE BEGIN TIM4_MspInit 0 */
+
+  /* USER CODE END TIM4_MspInit 0 */
+    /* TIM4 clock enable */
+    __HAL_RCC_TIM4_CLK_ENABLE();
+  /* USER CODE BEGIN TIM4_MspInit 1 */
+
+  /* USER CODE END TIM4_MspInit 1 */
+  }
+  else if(tim_baseHandle->Instance==TIM5)
+  {
+  /* USER CODE BEGIN TIM5_MspInit 0 */
+
+  /* USER CODE END TIM5_MspInit 0 */
+    /* TIM5 clock enable */
+    __HAL_RCC_TIM5_CLK_ENABLE();
+
+    __HAL_RCC_GPIOA_CLK_ENABLE();
+    /**TIM5 GPIO Configuration
+    PA0     ------> TIM5_CH1
+    */
+    GPIO_InitStruct.Pin = HC_Echo_Pin;
+    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
+    GPIO_InitStruct.Pull = GPIO_NOPULL;
+    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+    GPIO_InitStruct.Alternate = GPIO_AF2_TIM5;
+    HAL_GPIO_Init(HC_Echo_GPIO_Port, &GPIO_InitStruct);
+
+    /* TIM5 interrupt Init */
+    HAL_NVIC_SetPriority(TIM5_IRQn, 0, 0);
+    HAL_NVIC_EnableIRQ(TIM5_IRQn);
+  /* USER CODE BEGIN TIM5_MspInit 1 */
+
+  /* USER CODE END TIM5_MspInit 1 */
+  }
+}
+void HAL_TIM_MspPostInit(TIM_HandleTypeDef* timHandle)
+{
+
+  GPIO_InitTypeDef GPIO_InitStruct = {0};
+  if(timHandle->Instance==TIM3)
+  {
+  /* USER CODE BEGIN TIM3_MspPostInit 0 */
+
+  /* USER CODE END TIM3_MspPostInit 0 */
+    __HAL_RCC_GPIOB_CLK_ENABLE();
+    /**TIM3 GPIO Configuration
+    PB1     ------> TIM3_CH4
+    */
+    GPIO_InitStruct.Pin = PWMB_Pin;
+    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
+    GPIO_InitStruct.Pull = GPIO_NOPULL;
+    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+    GPIO_InitStruct.Alternate = GPIO_AF2_TIM3;
+    HAL_GPIO_Init(PWMB_GPIO_Port, &GPIO_InitStruct);
+
+  /* USER CODE BEGIN TIM3_MspPostInit 1 */
+
+  /* USER CODE END TIM3_MspPostInit 1 */
+  }
+  else if(timHandle->Instance==TIM4)
+  {
+  /* USER CODE BEGIN TIM4_MspPostInit 0 */
+
+  /* USER CODE END TIM4_MspPostInit 0 */
+
+    __HAL_RCC_GPIOC_CLK_ENABLE();
+    /**TIM4 GPIO Configuration
+    PC2     ------> TIM4_CH4
+    */
+    GPIO_InitStruct.Pin = PWMA_Pin;
+    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
+    GPIO_InitStruct.Pull = GPIO_NOPULL;
+    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+    GPIO_InitStruct.Alternate = GPIO_AF2_TIM4;
+    HAL_GPIO_Init(PWMA_GPIO_Port, &GPIO_InitStruct);
+
+  /* USER CODE BEGIN TIM4_MspPostInit 1 */
+
+  /* USER CODE END TIM4_MspPostInit 1 */
+  }
+
+}
+
+void HAL_TIM_Encoder_MspDeInit(TIM_HandleTypeDef* tim_encoderHandle)
+{
+
+  if(tim_encoderHandle->Instance==TIM1)
+  {
+  /* USER CODE BEGIN TIM1_MspDeInit 0 */
+
+  /* USER CODE END TIM1_MspDeInit 0 */
+    /* Peripheral clock disable */
+    __HAL_RCC_TIM1_CLK_DISABLE();
+
+    /**TIM1 GPIO Configuration
+    PE9     ------> TIM1_CH1
+    PE11     ------> TIM1_CH2
+    */
+    HAL_GPIO_DeInit(GPIOE, E1A_Pin|E1B_Pin);
+
+  /* USER CODE BEGIN TIM1_MspDeInit 1 */
+
+  /* USER CODE END TIM1_MspDeInit 1 */
+  }
+  else if(tim_encoderHandle->Instance==TIM8)
+  {
+  /* USER CODE BEGIN TIM8_MspDeInit 0 */
+
+  /* USER CODE END TIM8_MspDeInit 0 */
+    /* Peripheral clock disable */
+    __HAL_RCC_TIM8_CLK_DISABLE();
+
+    /**TIM8 GPIO Configuration
+    PC6     ------> TIM8_CH1
+    PC7     ------> TIM8_CH2
+    */
+    HAL_GPIO_DeInit(GPIOC, E2A_Pin|E2B_Pin);
+
+  /* USER CODE BEGIN TIM8_MspDeInit 1 */
+
+  /* USER CODE END TIM8_MspDeInit 1 */
+  }
+}
+
+void HAL_TIM_Base_MspDeInit(TIM_HandleTypeDef* tim_baseHandle)
+{
+
+  if(tim_baseHandle->Instance==TIM2)
+  {
+  /* USER CODE BEGIN TIM2_MspDeInit 0 */
+
+  /* USER CODE END TIM2_MspDeInit 0 */
+    /* Peripheral clock disable */
+    __HAL_RCC_TIM2_CLK_DISABLE();
+
+    /* TIM2 interrupt Deinit */
+    HAL_NVIC_DisableIRQ(TIM2_IRQn);
+  /* USER CODE BEGIN TIM2_MspDeInit 1 */
+
+  /* USER CODE END TIM2_MspDeInit 1 */
+  }
+  else if(tim_baseHandle->Instance==TIM3)
+  {
+  /* USER CODE BEGIN TIM3_MspDeInit 0 */
+
+  /* USER CODE END TIM3_MspDeInit 0 */
+    /* Peripheral clock disable */
+    __HAL_RCC_TIM3_CLK_DISABLE();
+
+    /* TIM3 interrupt Deinit */
+    HAL_NVIC_DisableIRQ(TIM3_IRQn);
+  /* USER CODE BEGIN TIM3_MspDeInit 1 */
+
+  /* USER CODE END TIM3_MspDeInit 1 */
+  }
+  else if(tim_baseHandle->Instance==TIM4)
+  {
+  /* USER CODE BEGIN TIM4_MspDeInit 0 */
+
+  /* USER CODE END TIM4_MspDeInit 0 */
+    /* Peripheral clock disable */
+    __HAL_RCC_TIM4_CLK_DISABLE();
+  /* USER CODE BEGIN TIM4_MspDeInit 1 */
+
+  /* USER CODE END TIM4_MspDeInit 1 */
+  }
+  else if(tim_baseHandle->Instance==TIM5)
+  {
+  /* USER CODE BEGIN TIM5_MspDeInit 0 */
+
+  /* USER CODE END TIM5_MspDeInit 0 */
+    /* Peripheral clock disable */
+    __HAL_RCC_TIM5_CLK_DISABLE();
+
+    /**TIM5 GPIO Configuration
+    PA0     ------> TIM5_CH1
+    */
+    HAL_GPIO_DeInit(HC_Echo_GPIO_Port, HC_Echo_Pin);
+
+    /* TIM5 interrupt Deinit */
+    HAL_NVIC_DisableIRQ(TIM5_IRQn);
+  /* USER CODE BEGIN TIM5_MspDeInit 1 */
+
+  /* USER CODE END TIM5_MspDeInit 1 */
+  }
+}
+
+/* USER CODE BEGIN 1 */
+
+/* USER CODE END 1 */
--- a/Core/Src/usart.c
+++ b/Core/Src/usart.c
@@ -24,54 +24,146 @@

 /* USER CODE END 0 */

-UART_HandleTypeDef huart1;
+UART_HandleTypeDef huart4;
+UART_HandleTypeDef huart2;
+UART_HandleTypeDef huart3;
 DMA_NodeTypeDef Node_GPDMA1_Channel5;
 DMA_QListTypeDef List_GPDMA1_Channel5;
 DMA_HandleTypeDef handle_GPDMA1_Channel5;
 DMA_HandleTypeDef handle_GPDMA1_Channel4;
+DMA_NodeTypeDef Node_GPDMA1_Channel3;
+DMA_QListTypeDef List_GPDMA1_Channel3;
+DMA_HandleTypeDef handle_GPDMA1_Channel3;

-/* USART1 init function */
-
-void MX_USART1_UART_Init(void)
+/* UART4 init function */
+void MX_UART4_Init(void)
 {

-  /* USER CODE BEGIN USART1_Init 0 */
+  /* USER CODE BEGIN UART4_Init 0 */

-  /* USER CODE END USART1_Init 0 */
+  /* USER CODE END UART4_Init 0 */

-  /* USER CODE BEGIN USART1_Init 1 */
+  /* USER CODE BEGIN UART4_Init 1 */

-  /* USER CODE END USART1_Init 1 */
-  huart1.Instance = USART1;
-  huart1.Init.BaudRate = 9600;
-  huart1.Init.WordLength = UART_WORDLENGTH_8B;
-  huart1.Init.StopBits = UART_STOPBITS_1;
-  huart1.Init.Parity = UART_PARITY_NONE;
-  huart1.Init.Mode = UART_MODE_TX_RX;
-  huart1.Init.HwFlowCtl = UART_HWCONTROL_NONE;
-  huart1.Init.OverSampling = UART_OVERSAMPLING_16;
-  huart1.Init.OneBitSampling = UART_ONE_BIT_SAMPLE_DISABLE;
-  huart1.Init.ClockPrescaler = UART_PRESCALER_DIV1;
-  huart1.AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_NO_INIT;
-  if (HAL_UART_Init(&huart1) != HAL_OK)
+  /* USER CODE END UART4_Init 1 */
+  huart4.Instance = UART4;
+  huart4.Init.BaudRate = 9600;
+  huart4.Init.WordLength = UART_WORDLENGTH_8B;
+  huart4.Init.StopBits = UART_STOPBITS_1;
+  huart4.Init.Parity = UART_PARITY_NONE;
+  huart4.Init.Mode = UART_MODE_TX_RX;
+  huart4.Init.HwFlowCtl = UART_HWCONTROL_NONE;
+  huart4.Init.OverSampling = UART_OVERSAMPLING_16;
+  huart4.Init.OneBitSampling = UART_ONE_BIT_SAMPLE_DISABLE;
+  huart4.Init.ClockPrescaler = UART_PRESCALER_DIV1;
+  huart4.AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_NO_INIT;
+  if (HAL_UART_Init(&huart4) != HAL_OK)
  {
    Error_Handler();
  }
-  if (HAL_UARTEx_SetTxFifoThreshold(&huart1, UART_TXFIFO_THRESHOLD_1_8) != HAL_OK)
+  if (HAL_UARTEx_SetTxFifoThreshold(&huart4, UART_TXFIFO_THRESHOLD_1_8) != HAL_OK)
  {
    Error_Handler();
  }
-  if (HAL_UARTEx_SetRxFifoThreshold(&huart1, UART_RXFIFO_THRESHOLD_1_8) != HAL_OK)
+  if (HAL_UARTEx_SetRxFifoThreshold(&huart4, UART_RXFIFO_THRESHOLD_1_8) != HAL_OK)
  {
    Error_Handler();
  }
-  if (HAL_UARTEx_DisableFifoMode(&huart1) != HAL_OK)
+  if (HAL_UARTEx_DisableFifoMode(&huart4) != HAL_OK)
  {
    Error_Handler();
  }
-  /* USER CODE BEGIN USART1_Init 2 */
+  /* USER CODE BEGIN UART4_Init 2 */

-  /* USER CODE END USART1_Init 2 */
+  /* USER CODE END UART4_Init 2 */
+
+}
+/* USART2 init function */
+
+void MX_USART2_UART_Init(void)
+{
+
+  /* USER CODE BEGIN USART2_Init 0 */
+
+  /* USER CODE END USART2_Init 0 */
+
+  /* USER CODE BEGIN USART2_Init 1 */
+
+  /* USER CODE END USART2_Init 1 */
+  huart2.Instance = USART2;
+  huart2.Init.BaudRate = 9600;
+  huart2.Init.WordLength = UART_WORDLENGTH_8B;
+  huart2.Init.StopBits = UART_STOPBITS_1;
+  huart2.Init.Parity = UART_PARITY_NONE;
+  huart2.Init.Mode = UART_MODE_TX_RX;
+  huart2.Init.HwFlowCtl = UART_HWCONTROL_NONE;
+  huart2.Init.OverSampling = UART_OVERSAMPLING_16;
+  huart2.Init.OneBitSampling = UART_ONE_BIT_SAMPLE_DISABLE;
+  huart2.Init.ClockPrescaler = UART_PRESCALER_DIV1;
+  huart2.AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_NO_INIT;
+  if (HAL_UART_Init(&huart2) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  if (HAL_UARTEx_SetTxFifoThreshold(&huart2, UART_TXFIFO_THRESHOLD_1_8) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  if (HAL_UARTEx_SetRxFifoThreshold(&huart2, UART_RXFIFO_THRESHOLD_1_8) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  if (HAL_UARTEx_DisableFifoMode(&huart2) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  /* USER CODE BEGIN USART2_Init 2 */
+
+  /* USER CODE END USART2_Init 2 */
+
+}
+/* USART3 init function */
+
+void MX_USART3_UART_Init(void)
+{
+
+  /* USER CODE BEGIN USART3_Init 0 */
+
+  /* USER CODE END USART3_Init 0 */
+
+  /* USER CODE BEGIN USART3_Init 1 */
+
+  /* USER CODE END USART3_Init 1 */
+  huart3.Instance = USART3;
+  huart3.Init.BaudRate = 115200;
+  huart3.Init.WordLength = UART_WORDLENGTH_8B;
+  huart3.Init.StopBits = UART_STOPBITS_1;
+  huart3.Init.Parity = UART_PARITY_NONE;
+  huart3.Init.Mode = UART_MODE_TX_RX;
+  huart3.Init.HwFlowCtl = UART_HWCONTROL_NONE;
+  huart3.Init.OverSampling = UART_OVERSAMPLING_16;
+  huart3.Init.OneBitSampling = UART_ONE_BIT_SAMPLE_DISABLE;
+  huart3.Init.ClockPrescaler = UART_PRESCALER_DIV1;
+  huart3.AdvancedInit.AdvFeatureInit = UART_ADVFEATURE_NO_INIT;
+  if (HAL_UART_Init(&huart3) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  if (HAL_UARTEx_SetTxFifoThreshold(&huart3, UART_TXFIFO_THRESHOLD_1_8) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  if (HAL_UARTEx_SetRxFifoThreshold(&huart3, UART_RXFIFO_THRESHOLD_1_8) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  if (HAL_UARTEx_DisableFifoMode(&huart3) != HAL_OK)
+  {
+    Error_Handler();
+  }
+  /* USER CODE BEGIN USART3_Init 2 */
+
+  /* USER CODE END USART3_Init 2 */

 }

@@ -81,40 +173,40 @@ void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle)
  GPIO_InitTypeDef GPIO_InitStruct = {0};
  DMA_NodeConfTypeDef NodeConfig= {0};
  RCC_PeriphCLKInitTypeDef PeriphClkInitStruct = {0};
-  if(uartHandle->Instance==USART1)
+  if(uartHandle->Instance==UART4)
  {
-  /* USER CODE BEGIN USART1_MspInit 0 */
+  /* USER CODE BEGIN UART4_MspInit 0 */

-  /* USER CODE END USART1_MspInit 0 */
+  /* USER CODE END UART4_MspInit 0 */

  /** Initializes the peripherals clock
  */
-    PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_USART1;
-    PeriphClkInitStruct.Usart1ClockSelection = RCC_USART1CLKSOURCE_PCLK2;
+    PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_UART4;
+    PeriphClkInitStruct.Uart4ClockSelection = RCC_UART4CLKSOURCE_PCLK1;
    if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK)
    {
      Error_Handler();
    }

-    /* USART1 clock enable */
-    __HAL_RCC_USART1_CLK_ENABLE();
+    /* UART4 clock enable */
+    __HAL_RCC_UART4_CLK_ENABLE();

    __HAL_RCC_GPIOB_CLK_ENABLE();
-    /**USART1 GPIO Configuration
-    PB14     ------> USART1_TX
-    PB15     ------> USART1_RX
+    /**UART4 GPIO Configuration
+    PB8     ------> UART4_RX
+    PB9     ------> UART4_TX
    */
-    GPIO_InitStruct.Pin = GPIO_PIN_14|GPIO_PIN_15;
+    GPIO_InitStruct.Pin = GPIO_PIN_8|GPIO_PIN_9;
    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
-    GPIO_InitStruct.Alternate = GPIO_AF4_USART1;
+    GPIO_InitStruct.Alternate = GPIO_AF8_UART4;
    HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);

-    /* USART1 DMA Init */
-    /* GPDMA1_REQUEST_USART1_RX Init */
+    /* UART4 DMA Init */
+    /* GPDMA1_REQUEST_UART4_RX Init */
    NodeConfig.NodeType = DMA_GPDMA_LINEAR_NODE;
-    NodeConfig.Init.Request = GPDMA1_REQUEST_USART1_RX;
+    NodeConfig.Init.Request = GPDMA1_REQUEST_UART4_RX;
    NodeConfig.Init.BlkHWRequest = DMA_BREQ_SINGLE_BURST;
    NodeConfig.Init.Direction = DMA_PERIPH_TO_MEMORY;
    NodeConfig.Init.SrcInc = DMA_SINC_FIXED;
@@ -167,9 +259,9 @@ void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle)
      Error_Handler();
    }

-    /* GPDMA1_REQUEST_USART1_TX Init */
+    /* GPDMA1_REQUEST_UART4_TX Init */
    handle_GPDMA1_Channel4.Instance = GPDMA1_Channel4;
-    handle_GPDMA1_Channel4.Init.Request = GPDMA1_REQUEST_USART1_TX;
+    handle_GPDMA1_Channel4.Init.Request = GPDMA1_REQUEST_UART4_TX;
    handle_GPDMA1_Channel4.Init.BlkHWRequest = DMA_BREQ_SINGLE_BURST;
    handle_GPDMA1_Channel4.Init.Direction = DMA_MEMORY_TO_PERIPH;
    handle_GPDMA1_Channel4.Init.SrcInc = DMA_SINC_INCREMENTED;
@@ -194,41 +286,224 @@ void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle)
      Error_Handler();
    }

-    /* USART1 interrupt Init */
-    HAL_NVIC_SetPriority(USART1_IRQn, 0, 0);
-    HAL_NVIC_EnableIRQ(USART1_IRQn);
-  /* USER CODE BEGIN USART1_MspInit 1 */
+    /* UART4 interrupt Init */
+    HAL_NVIC_SetPriority(UART4_IRQn, 0, 0);
+    HAL_NVIC_EnableIRQ(UART4_IRQn);
+  /* USER CODE BEGIN UART4_MspInit 1 */

-  /* USER CODE END USART1_MspInit 1 */
+  /* USER CODE END UART4_MspInit 1 */
+  }
+  else if(uartHandle->Instance==USART2)
+  {
+  /* USER CODE BEGIN USART2_MspInit 0 */
+
+  /* USER CODE END USART2_MspInit 0 */
+
+  /** Initializes the peripherals clock
+  */
+    PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_USART2;
+    PeriphClkInitStruct.Usart2ClockSelection = RCC_USART2CLKSOURCE_PCLK1;
+    if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK)
+    {
+      Error_Handler();
+    }
+
+    /* USART2 clock enable */
+    __HAL_RCC_USART2_CLK_ENABLE();
+
+    __HAL_RCC_GPIOD_CLK_ENABLE();
+    /**USART2 GPIO Configuration
+    PD5     ------> USART2_TX
+    PD6     ------> USART2_RX
+    */
+    GPIO_InitStruct.Pin = GPIO_PIN_5|GPIO_PIN_6;
+    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
+    GPIO_InitStruct.Pull = GPIO_NOPULL;
+    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+    GPIO_InitStruct.Alternate = GPIO_AF7_USART2;
+    HAL_GPIO_Init(GPIOD, &GPIO_InitStruct);
+
+    /* USART2 DMA Init */
+    /* GPDMA1_REQUEST_USART2_RX Init */
+    NodeConfig.NodeType = DMA_GPDMA_LINEAR_NODE;
+    NodeConfig.Init.Request = GPDMA1_REQUEST_USART2_RX;
+    NodeConfig.Init.BlkHWRequest = DMA_BREQ_SINGLE_BURST;
+    NodeConfig.Init.Direction = DMA_PERIPH_TO_MEMORY;
+    NodeConfig.Init.SrcInc = DMA_SINC_FIXED;
+    NodeConfig.Init.DestInc = DMA_DINC_FIXED;
+    NodeConfig.Init.SrcDataWidth = DMA_SRC_DATAWIDTH_BYTE;
+    NodeConfig.Init.DestDataWidth = DMA_DEST_DATAWIDTH_BYTE;
+    NodeConfig.Init.SrcBurstLength = 1;
+    NodeConfig.Init.DestBurstLength = 1;
+    NodeConfig.Init.TransferAllocatedPort = DMA_SRC_ALLOCATED_PORT0|DMA_DEST_ALLOCATED_PORT0;
+    NodeConfig.Init.TransferEventMode = DMA_TCEM_BLOCK_TRANSFER;
+    NodeConfig.Init.Mode = DMA_NORMAL;
+    NodeConfig.TriggerConfig.TriggerPolarity = DMA_TRIG_POLARITY_MASKED;
+    NodeConfig.DataHandlingConfig.DataExchange = DMA_EXCHANGE_NONE;
+    NodeConfig.DataHandlingConfig.DataAlignment = DMA_DATA_RIGHTALIGN_ZEROPADDED;
+    if (HAL_DMAEx_List_BuildNode(&NodeConfig, &Node_GPDMA1_Channel3) != HAL_OK)
+    {
+      Error_Handler();
+    }
+
+    if (HAL_DMAEx_List_InsertNode(&List_GPDMA1_Channel3, NULL, &Node_GPDMA1_Channel3) != HAL_OK)
+    {
+      Error_Handler();
+    }
+
+    if (HAL_DMAEx_List_SetCircularMode(&List_GPDMA1_Channel3) != HAL_OK)
+    {
+      Error_Handler();
+    }
+
+    handle_GPDMA1_Channel3.Instance = GPDMA1_Channel3;
+    handle_GPDMA1_Channel3.InitLinkedList.Priority = DMA_LOW_PRIORITY_LOW_WEIGHT;
+    handle_GPDMA1_Channel3.InitLinkedList.LinkStepMode = DMA_LSM_FULL_EXECUTION;
+    handle_GPDMA1_Channel3.InitLinkedList.LinkAllocatedPort = DMA_LINK_ALLOCATED_PORT0;
+    handle_GPDMA1_Channel3.InitLinkedList.TransferEventMode = DMA_TCEM_BLOCK_TRANSFER;
+    handle_GPDMA1_Channel3.InitLinkedList.LinkedListMode = DMA_LINKEDLIST_CIRCULAR;
+    if (HAL_DMAEx_List_Init(&handle_GPDMA1_Channel3) != HAL_OK)
+    {
+      Error_Handler();
+    }
+
+    if (HAL_DMAEx_List_LinkQ(&handle_GPDMA1_Channel3, &List_GPDMA1_Channel3) != HAL_OK)
+    {
+      Error_Handler();
+    }
+
+    __HAL_LINKDMA(uartHandle, hdmarx, handle_GPDMA1_Channel3);
+
+    if (HAL_DMA_ConfigChannelAttributes(&handle_GPDMA1_Channel3, DMA_CHANNEL_NPRIV) != HAL_OK)
+    {
+      Error_Handler();
+    }
+
+    /* USART2 interrupt Init */
+    HAL_NVIC_SetPriority(USART2_IRQn, 0, 0);
+    HAL_NVIC_EnableIRQ(USART2_IRQn);
+  /* USER CODE BEGIN USART2_MspInit 1 */
+
+  /* USER CODE END USART2_MspInit 1 */
+  }
+  else if(uartHandle->Instance==USART3)
+  {
+  /* USER CODE BEGIN USART3_MspInit 0 */
+
+  /* USER CODE END USART3_MspInit 0 */
+
+  /** Initializes the peripherals clock
+  */
+    PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_USART3;
+    PeriphClkInitStruct.Usart3ClockSelection = RCC_USART3CLKSOURCE_PCLK1;
+    if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK)
+    {
+      Error_Handler();
+    }
+
+    /* USART3 clock enable */
+    __HAL_RCC_USART3_CLK_ENABLE();
+
+    __HAL_RCC_GPIOB_CLK_ENABLE();
+    __HAL_RCC_GPIOC_CLK_ENABLE();
+    /**USART3 GPIO Configuration
+    PB10     ------> USART3_TX
+    PC11     ------> USART3_RX
+    */
+    GPIO_InitStruct.Pin = GPIO_PIN_10;
+    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
+    GPIO_InitStruct.Pull = GPIO_NOPULL;
+    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+    GPIO_InitStruct.Alternate = GPIO_AF7_USART3;
+    HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
+
+    GPIO_InitStruct.Pin = GPIO_PIN_11;
+    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
+    GPIO_InitStruct.Pull = GPIO_NOPULL;
+    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+    GPIO_InitStruct.Alternate = GPIO_AF7_USART3;
+    HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
+
+    /* USART3 interrupt Init */
+    HAL_NVIC_SetPriority(USART3_IRQn, 0, 0);
+    HAL_NVIC_EnableIRQ(USART3_IRQn);
+  /* USER CODE BEGIN USART3_MspInit 1 */
+
+  /* USER CODE END USART3_MspInit 1 */
  }
 }

 void HAL_UART_MspDeInit(UART_HandleTypeDef* uartHandle)
 {

-  if(uartHandle->Instance==USART1)
+  if(uartHandle->Instance==UART4)
  {
-  /* USER CODE BEGIN USART1_MspDeInit 0 */
+  /* USER CODE BEGIN UART4_MspDeInit 0 */

-  /* USER CODE END USART1_MspDeInit 0 */
+  /* USER CODE END UART4_MspDeInit 0 */
    /* Peripheral clock disable */
-    __HAL_RCC_USART1_CLK_DISABLE();
+    __HAL_RCC_UART4_CLK_DISABLE();

-    /**USART1 GPIO Configuration
-    PB14     ------> USART1_TX
-    PB15     ------> USART1_RX
+    /**UART4 GPIO Configuration
+    PB8     ------> UART4_RX
+    PB9     ------> UART4_TX
    */
-    HAL_GPIO_DeInit(GPIOB, GPIO_PIN_14|GPIO_PIN_15);
+    HAL_GPIO_DeInit(GPIOB, GPIO_PIN_8|GPIO_PIN_9);

-    /* USART1 DMA DeInit */
+    /* UART4 DMA DeInit */
    HAL_DMA_DeInit(uartHandle->hdmarx);
    HAL_DMA_DeInit(uartHandle->hdmatx);

-    /* USART1 interrupt Deinit */
-    HAL_NVIC_DisableIRQ(USART1_IRQn);
-  /* USER CODE BEGIN USART1_MspDeInit 1 */
+    /* UART4 interrupt Deinit */
+    HAL_NVIC_DisableIRQ(UART4_IRQn);
+  /* USER CODE BEGIN UART4_MspDeInit 1 */

-  /* USER CODE END USART1_MspDeInit 1 */
+  /* USER CODE END UART4_MspDeInit 1 */
+  }
+  else if(uartHandle->Instance==USART2)
+  {
+  /* USER CODE BEGIN USART2_MspDeInit 0 */
+
+  /* USER CODE END USART2_MspDeInit 0 */
+    /* Peripheral clock disable */
+    __HAL_RCC_USART2_CLK_DISABLE();
+
+    /**USART2 GPIO Configuration
+    PD5     ------> USART2_TX
+    PD6     ------> USART2_RX
+    */
+    HAL_GPIO_DeInit(GPIOD, GPIO_PIN_5|GPIO_PIN_6);
+
+    /* USART2 DMA DeInit */
+    HAL_DMA_DeInit(uartHandle->hdmarx);
+
+    /* USART2 interrupt Deinit */
+    HAL_NVIC_DisableIRQ(USART2_IRQn);
+  /* USER CODE BEGIN USART2_MspDeInit 1 */
+
+  /* USER CODE END USART2_MspDeInit 1 */
+  }
+  else if(uartHandle->Instance==USART3)
+  {
+  /* USER CODE BEGIN USART3_MspDeInit 0 */
+
+  /* USER CODE END USART3_MspDeInit 0 */
+    /* Peripheral clock disable */
+    __HAL_RCC_USART3_CLK_DISABLE();
+
+    /**USART3 GPIO Configuration
+    PB10     ------> USART3_TX
+    PC11     ------> USART3_RX
+    */
+    HAL_GPIO_DeInit(GPIOB, GPIO_PIN_10);
+
+    HAL_GPIO_DeInit(GPIOC, GPIO_PIN_11);
+
+    /* USART3 interrupt Deinit */
+    HAL_NVIC_DisableIRQ(USART3_IRQn);
+  /* USER CODE BEGIN USART3_MspDeInit 1 */
+
+  /* USER CODE END USART3_MspDeInit 1 */
  }
 }

--- a/Drivers/STM32H5xx_HAL_Driver/Inc/stm32h5xx_ll_tim.h
+++ b/Drivers/STM32H5xx_HAL_Driver/Inc/stm32h5xx_ll_tim.h
--- a/MDK-ARM/.vscode/keil-assistant.log
+++ b/MDK-ARM/.vscode/keil-assistant.log
@@ -33,3 +33,11 @@

 [info] Log at : 2025/6/8|21:00:00|GMT+0800

+[info] Log at : 2025/6/9|21:24:00|GMT+0800
+
+[info] Log at : 2025/6/9|21:25:14|GMT+0800
+
+[info] Log at : 2025/6/9|21:25:19|GMT+0800
+
+[info] Log at : 2025/6/9|21:52:22|GMT+0800
+
--- a/MDK-ARM/.vscode/uv4.log
+++ b/MDK-ARM/.vscode/uv4.log
@@ -1,33 +1,34 @@
 *** Using Compiler 'V6.21', folder: 'D:\keil5\ARM\ARMCLANG\Bin'
 Build target 'AutoGuideStick'
-compiling main.c...
-compiling usart.c...
-compiling app_threadx.c...
-compiling stm32h5xx_hal_rcc.c...
-compiling gpio.c...
-compiling stm32h5xx_hal_rcc_ex.c...
-compiling stm32h5xx_hal_cortex.c...
 compiling memorymap.c...
-compiling stm32h5xx_hal_msp.c...
-compiling stm32h5xx_hal_timebase_tim.c...
-compiling stm32h5xx_it.c...
 compiling app_azure_rtos.c...
+compiling gpdma.c...
+compiling usart.c...
+compiling gpio.c...
 compiling stm32h5xx_hal_tim_ex.c...
-compiling stm32h5xx_hal_flash.c...
-compiling stm32h5xx_hal_flash_ex.c...
 compiling stm32h5xx_hal_tim.c...
-compiling stm32h5xx_hal_gpio.c...
+compiling app_threadx.c...
+compiling stm32h5xx_hal_cortex.c...
 compiling stm32h5xx_hal_dma.c...
+compiling main.c...
+compiling stm32h5xx_hal_timebase_tim.c...
+compiling stm32h5xx_hal_msp.c...
+compiling stm32h5xx_it.c...
+compiling stm32h5xx_hal_dma_ex.c...
+compiling stm32h5xx_hal_rcc.c...
+compiling stm32h5xx_hal_flash.c...
+compiling stm32h5xx_hal_rcc_ex.c...
+compiling stm32h5xx_hal_flash_ex.c...
+compiling stm32h5xx_hal_gpio.c...
 compiling stm32h5xx_hal_pwr.c...
 compiling stm32h5xx_hal_pwr_ex.c...
-compiling stm32h5xx_hal_dma_ex.c...
 compiling stm32h5xx_hal.c...
 compiling stm32h5xx_hal_exti.c...
+compiling tx_initialize_high_level.c...
 compiling stm32h5xx_hal_uart_ex.c...
 compiling system_stm32h5xx.c...
-compiling stm32h5xx_hal_uart.c...
-compiling tx_initialize_high_level.c...
 compiling tx_initialize_kernel_enter.c...
+compiling stm32h5xx_hal_uart.c...
 compiling tx_initialize_kernel_setup.c...
 compiling tx_thread_stack_error_handler.c...
 compiling tx_thread_stack_error_notify.c...
@@ -52,12 +53,12 @@ compiling tx_byte_release.c...
 compiling tx_event_flags_cleanup.c...
 compiling tx_event_flags_create.c...
 compiling tx_event_flags_delete.c...
-compiling tx_event_flags_get.c...
 compiling tx_event_flags_info_get.c...
+compiling tx_event_flags_get.c...
 compiling tx_event_flags_initialize.c...
 compiling tx_event_flags_set.c...
-compiling tx_mutex_cleanup.c...
 compiling tx_event_flags_set_notify.c...
+compiling tx_mutex_cleanup.c...
 compiling tx_mutex_create.c...
 compiling tx_mutex_delete.c...
 compiling tx_mutex_get.c...
@@ -97,8 +98,8 @@ compiling tx_thread_preemption_change.c...
 compiling tx_thread_priority_change.c...
 compiling tx_thread_relinquish.c...
 compiling tx_thread_reset.c...
-compiling tx_thread_resume.c...
 compiling tx_thread_shell_entry.c...
+compiling tx_thread_resume.c...
 compiling tx_thread_sleep.c...
 compiling tx_thread_stack_analyze.c...
 compiling tx_thread_suspend.c...
@@ -182,8 +183,9 @@ compiling txe_timer_create.c...
 compiling txe_timer_deactivate.c...
 compiling txe_timer_delete.c...
 compiling txe_timer_info_get.c...
+compiling HCBle.c...
 linking...
-Program Size: Code=40362 RO-data=726 RW-data=16 ZI-data=4536  
+Program Size: Code=54718 RO-data=726 RW-data=16 ZI-data=5296  
 FromELF: creating hex file...
 "AutoGuideStick\AutoGuideStick.axf" - 0 Error(s), 0 Warning(s).
 Build Time Elapsed:  00:00:24
--- a/MDK-ARM/.vscode/uv4.log.lock
+++ b/MDK-ARM/.vscode/uv4.log.lock
@@ -1 +1 @@
-2025/6/8 21:00:51
+2025/6/9 21:28:26
--- a/MDK-ARM/AutoGuideStick.uvguix.madao
+++ b/MDK-ARM/AutoGuideStick.uvguix.madao
--- a/MDK-ARM/AutoGuideStick.uvoptx
+++ b/MDK-ARM/AutoGuideStick.uvoptx
--- a/MDK-ARM/AutoGuideStick.uvprojx
+++ b/MDK-ARM/AutoGuideStick.uvprojx
@@ -138,7 +138,7 @@
          </Flash1>
          <bUseTDR>1</bUseTDR>
          <Flash2>BIN\UL2V8M.DLL</Flash2>
-          <Flash3></Flash3>
+          <Flash3>"" ()</Flash3>
          <Flash4></Flash4>
          <pFcarmOut></pFcarmOut>
          <pFcarmGrp></pFcarmGrp>
@@ -190,7 +190,7 @@
            <hadIRAM2>0</hadIRAM2>
            <hadIROM2>0</hadIROM2>
            <StupSel>8</StupSel>
-            <useUlib>0</useUlib>
+            <useUlib>1</useUlib>
            <EndSel>0</EndSel>
            <uLtcg>0</uLtcg>
            <nSecure>0</nSecure>
@@ -475,6 +475,62 @@
              <FileType>1</FileType>
              <FilePath>../Core/Src/app_threadx.c</FilePath>
            </File>
+            <File>
+              <FileName>tim.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>../Core/Src/tim.c</FilePath>
+              <FileOption>
+                <CommonProperty>
+                  <UseCPPCompiler>2</UseCPPCompiler>
+                  <RVCTCodeConst>0</RVCTCodeConst>
+                  <RVCTZI>0</RVCTZI>
+                  <RVCTOtherData>0</RVCTOtherData>
+                  <ModuleSelection>0</ModuleSelection>
+                  <IncludeInBuild>1</IncludeInBuild>
+                  <AlwaysBuild>2</AlwaysBuild>
+                  <GenerateAssemblyFile>2</GenerateAssemblyFile>
+                  <AssembleAssemblyFile>2</AssembleAssemblyFile>
+                  <PublicsOnly>2</PublicsOnly>
+                  <StopOnExitCode>11</StopOnExitCode>
+                  <CustomArgument></CustomArgument>
+                  <IncludeLibraryModules></IncludeLibraryModules>
+                  <ComprImg>1</ComprImg>
+                </CommonProperty>
+                <FileArmAds>
+                  <Cads>
+                    <interw>2</interw>
+                    <Optim>0</Optim>
+                    <oTime>2</oTime>
+                    <SplitLS>2</SplitLS>
+                    <OneElfS>2</OneElfS>
+                    <Strict>2</Strict>
+                    <EnumInt>2</EnumInt>
+                    <PlainCh>2</PlainCh>
+                    <Ropi>2</Ropi>
+                    <Rwpi>2</Rwpi>
+                    <wLevel>0</wLevel>
+                    <uThumb>2</uThumb>
+                    <uSurpInc>2</uSurpInc>
+                    <uC99>2</uC99>
+                    <uGnu>2</uGnu>
+                    <useXO>2</useXO>
+                    <v6Lang>0</v6Lang>
+                    <v6LangP>0</v6LangP>
+                    <vShortEn>2</vShortEn>
+                    <vShortWch>2</vShortWch>
+                    <v6Lto>2</v6Lto>
+                    <v6WtE>2</v6WtE>
+                    <v6Rtti>2</v6Rtti>
+                    <VariousControls>
+                      <MiscControls></MiscControls>
+                      <Define></Define>
+                      <Undefine></Undefine>
+                      <IncludePath></IncludePath>
+                    </VariousControls>
+                  </Cads>
+                </FileArmAds>
+              </FileOption>
+            </File>
            <File>
              <FileName>usart.c</FileName>
              <FileType>1</FileType>
@@ -576,16 +632,6 @@
              <FileType>1</FileType>
              <FilePath>../Drivers/STM32H5xx_HAL_Driver/Src/stm32h5xx_hal_cortex.c</FilePath>
            </File>
-            <File>
-              <FileName>stm32h5xx_hal_dma.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>../Drivers/STM32H5xx_HAL_Driver/Src/stm32h5xx_hal_dma.c</FilePath>
-            </File>
-            <File>
-              <FileName>stm32h5xx_hal_dma_ex.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>../Drivers/STM32H5xx_HAL_Driver/Src/stm32h5xx_hal_dma_ex.c</FilePath>
-            </File>
            <File>
              <FileName>stm32h5xx_hal_rcc.c</FileName>
              <FileType>1</FileType>
@@ -611,6 +657,16 @@
              <FileType>1</FileType>
              <FilePath>../Drivers/STM32H5xx_HAL_Driver/Src/stm32h5xx_hal_gpio.c</FilePath>
            </File>
+            <File>
+              <FileName>stm32h5xx_hal_dma.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>../Drivers/STM32H5xx_HAL_Driver/Src/stm32h5xx_hal_dma.c</FilePath>
+            </File>
+            <File>
+              <FileName>stm32h5xx_hal_dma_ex.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>../Drivers/STM32H5xx_HAL_Driver/Src/stm32h5xx_hal_dma_ex.c</FilePath>
+            </File>
            <File>
              <FileName>stm32h5xx_hal_pwr.c</FileName>
              <FileType>1</FileType>
@@ -1603,6 +1659,91 @@
              <FileType>5</FileType>
              <FilePath>..\fun\headfile.h</FilePath>
            </File>
+            <File>
+              <FileName>gps.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\fun\gps.c</FilePath>
+            </File>
+            <File>
+              <FileName>gps.h</FileName>
+              <FileType>5</FileType>
+              <FilePath>..\fun\gps.h</FilePath>
+            </File>
+            <File>
+              <FileName>Buzzer.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\fun\Buzzer.c</FilePath>
+            </File>
+            <File>
+              <FileName>Buzzer.h</FileName>
+              <FileType>5</FileType>
+              <FilePath>..\fun\Buzzer.h</FilePath>
+            </File>
+            <File>
+              <FileName>Shake_Motor.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\fun\Shake_Motor.c</FilePath>
+            </File>
+            <File>
+              <FileName>Shake_Motor.h</FileName>
+              <FileType>5</FileType>
+              <FilePath>..\fun\Shake_Motor.h</FilePath>
+            </File>
+            <File>
+              <FileName>Ultrasound.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\fun\Ultrasound.c</FilePath>
+            </File>
+            <File>
+              <FileName>Ultrasound.h</FileName>
+              <FileType>5</FileType>
+              <FilePath>..\fun\Ultrasound.h</FilePath>
+            </File>
+            <File>
+              <FileName>IMU.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\fun\IMU.c</FilePath>
+            </File>
+            <File>
+              <FileName>IMU.h</FileName>
+              <FileType>5</FileType>
+              <FilePath>..\fun\IMU.h</FilePath>
+            </File>
+            <File>
+              <FileName>Motor.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\fun\Motor.c</FilePath>
+            </File>
+            <File>
+              <FileName>Motor.h</FileName>
+              <FileType>5</FileType>
+              <FilePath>..\fun\Motor.h</FilePath>
+            </File>
+            <File>
+              <FileName>imu948.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\fun\imu948.c</FilePath>
+            </File>
+            <File>
+              <FileName>imu948.h</FileName>
+              <FileType>5</FileType>
+              <FilePath>..\fun\imu948.h</FilePath>
+            </File>
+            <File>
+              <FileName>value.h</FileName>
+              <FileType>5</FileType>
+              <FilePath>..\fun\value.h</FilePath>
+            </File>
+            <File>
+              <FileName>encoder.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\fun\encoder.c</FilePath>
+            </File>
+            <File>
+              <FileName>encoder.h</FileName>
+              <FileType>5</FileType>
+              <FilePath>..\fun\encoder.h</FilePath>
+            </File>
          </Files>
        </Group>
        <Group>
--- a/MDK-ARM/AutoGuideStick/AutoGuideStick.axf
+++ b/MDK-ARM/AutoGuideStick/AutoGuideStick.axf
--- a/MDK-ARM/AutoGuideStick/AutoGuideStick.build_log.htm
+++ b/MDK-ARM/AutoGuideStick/AutoGuideStick.build_log.htm
@@ -22,15 +22,11 @@ Dialog DLL:      TCM.DLL V1.56.4.0
 
 <h2>Project:</h2>
 D:\advance_stick\AutoGuideStick\MDK-ARM\AutoGuideStick.uvprojx
-Project File Date:  06/09/2025
+Project File Date:  07/02/2025

 <h2>Output:</h2>
 *** Using Compiler 'V6.21', folder: 'D:\keil5\ARM\ARMCLANG\Bin'
 Build target 'AutoGuideStick'
-compiling HCBle.c...
-linking...
-Program Size: Code=54718 RO-data=726 RW-data=16 ZI-data=5296  
-FromELF: creating hex file...
 "AutoGuideStick\AutoGuideStick.axf" - 0 Error(s), 0 Warning(s).

 <h2>Software Packages used:</h2>
@@ -55,7 +51,7 @@ Package Vendor: Keil

   * Component: ARM::CMSIS:CORE@5.6.0
      Include file:  CMSIS/Core/Include/tz_context.h
-Build Time Elapsed:  00:00:03
+Build Time Elapsed:  00:00:02
 </pre>
 </body>
 </html>
--- a/MDK-ARM/AutoGuideStick/AutoGuideStick.hex
+++ b/MDK-ARM/AutoGuideStick/AutoGuideStick.hex
--- a/MDK-ARM/AutoGuideStick/AutoGuideStick.htm
+++ b/MDK-ARM/AutoGuideStick/AutoGuideStick.htm
--- a/MDK-ARM/AutoGuideStick/AutoGuideStick.lnp
+++ b/MDK-ARM/AutoGuideStick/AutoGuideStick.lnp
@@ -6,6 +6,7 @@
 "autoguidestick\gpdma.o"
 "autoguidestick\memorymap.o"
 "autoguidestick\app_threadx.o"
+"autoguidestick\tim.o"
 "autoguidestick\usart.o"
 "autoguidestick\stm32h5xx_it.o"
 "autoguidestick\stm32h5xx_hal_msp.o"
@@ -14,13 +15,13 @@
 "autoguidestick\stm32h5xx_hal_tim.o"
 "autoguidestick\stm32h5xx_hal_tim_ex.o"
 "autoguidestick\stm32h5xx_hal_cortex.o"
-"autoguidestick\stm32h5xx_hal_dma.o"
-"autoguidestick\stm32h5xx_hal_dma_ex.o"
 "autoguidestick\stm32h5xx_hal_rcc.o"
 "autoguidestick\stm32h5xx_hal_rcc_ex.o"
 "autoguidestick\stm32h5xx_hal_flash.o"
 "autoguidestick\stm32h5xx_hal_flash_ex.o"
 "autoguidestick\stm32h5xx_hal_gpio.o"
+"autoguidestick\stm32h5xx_hal_dma.o"
+"autoguidestick\stm32h5xx_hal_dma_ex.o"
 "autoguidestick\stm32h5xx_hal_pwr.o"
 "autoguidestick\stm32h5xx_hal_pwr_ex.o"
 "autoguidestick\stm32h5xx_hal.o"
@@ -194,7 +195,15 @@
 "autoguidestick\txe_timer_delete.o"
 "autoguidestick\txe_timer_info_get.o"
 "autoguidestick\hcble.o"
--strict --scatter "AutoGuideStick\AutoGuideStick.sct"
+"autoguidestick\gps.o"
+"autoguidestick\buzzer.o"
+"autoguidestick\shake_motor.o"
+"autoguidestick\ultrasound.o"
+"autoguidestick\imu.o"
+"autoguidestick\motor.o"
+"autoguidestick\imu948.o"
+"autoguidestick\encoder.o"
+--library_type=microlib --strict --scatter "AutoGuideStick\AutoGuideStick.sct"
 --summary_stderr --info summarysizes --map --load_addr_map_info --xref --callgraph --symbols
 --info sizes --info totals --info unused --info veneers
 --list "AutoGuideStick.map" -o AutoGuideStick\AutoGuideStick.axf
--- a/MDK-ARM/AutoGuideStick/AutoGuideStick.map
+++ b/MDK-ARM/AutoGuideStick/AutoGuideStick.map
--- a/MDK-ARM/AutoGuideStick/AutoGuideStick_AutoGuideStick.dep
+++ b/MDK-ARM/AutoGuideStick/AutoGuideStick_AutoGuideStick.dep
--- a/MDK-ARM/AutoGuideStick/ExtDll.iex
+++ b/MDK-ARM/AutoGuideStick/ExtDll.iex
@@ -0,0 +1,2 @@
+[EXTDLL]
+Count=0
--- a/MDK-ARM/AutoGuideStick/app_azure_rtos.d
+++ b/MDK-ARM/AutoGuideStick/app_azure_rtos.d
@@ -3,8 +3,8 @@ autoguidestick/app_azure_rtos.o: ..\AZURE_RTOS\App\app_azure_rtos.c \
  ..\Middlewares\ST\threadx\common\inc\tx_api.h \
  ..\Middlewares\ST\threadx\ports\cortex_m33\ac6\inc\tx_port.h \
  ..\Core\Inc\tx_user.h D:\keil5\ARM\ARMCLANG\include\stdlib.h \
-  D:\keil5\ARM\ARMCLANG\include\string.h \
-  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal.h \
+  D:\keil5\ARM\ARMCLANG\include\string.h ..\fun\headfile.h \
+  ..\Core\Inc\main.h ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal.h \
  ..\Core\Inc\stm32h5xx_hal_conf.h \
  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_rcc.h \
  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_def.h \
@@ -35,4 +35,10 @@ autoguidestick/app_azure_rtos.o: ..\AZURE_RTOS\App\app_azure_rtos.c \
  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_uart.h \
  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_uart_ex.h \
  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_exti.h \
-  ..\AZURE_RTOS\App\app_azure_rtos_config.h
+  ..\Core\Inc\memorymap.h ..\Core\Inc\usart.h ..\Core\Inc\gpio.h \
+  ..\Core\Inc\gpdma.h ..\Core\Inc\tim.h \
+  D:\keil5\ARM\ARMCLANG\include\stdio.h \
+  D:\keil5\ARM\ARMCLANG\include\stdarg.h ..\fun\HCBle.h ..\fun\gps.h \
+  ..\fun\Shake_Motor.h ..\fun\Ultrasound.h ..\fun\Buzzer.h \
+  ..\fun\Motor.h ..\fun\IMU.h ..\fun\imu948.h ..\fun\encoder.h \
+  ..\fun\value.h ..\AZURE_RTOS\App\app_azure_rtos_config.h
--- a/MDK-ARM/AutoGuideStick/app_azure_rtos.o
+++ b/MDK-ARM/AutoGuideStick/app_azure_rtos.o
--- a/MDK-ARM/AutoGuideStick/app_threadx.d
+++ b/MDK-ARM/AutoGuideStick/app_threadx.d
@@ -3,4 +3,42 @@ autoguidestick/app_threadx.o: ..\Core\Src\app_threadx.c \
  ..\Middlewares\ST\threadx\common\inc\tx_api.h \
  ..\Middlewares\ST\threadx\ports\cortex_m33\ac6\inc\tx_port.h \
  ..\Core\Inc\tx_user.h D:\keil5\ARM\ARMCLANG\include\stdlib.h \
-  D:\keil5\ARM\ARMCLANG\include\string.h
+  D:\keil5\ARM\ARMCLANG\include\string.h ..\fun\headfile.h \
+  ..\Core\Inc\main.h ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal.h \
+  ..\Core\Inc\stm32h5xx_hal_conf.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_rcc.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_def.h \
+  ..\Drivers\CMSIS\Device\ST\STM32H5xx\Include\stm32h5xx.h \
+  D:\keil5\ARM\ARMCLANG\include\math.h \
+  ..\Drivers\CMSIS\Device\ST\STM32H5xx\Include\stm32h563xx.h \
+  ..\Drivers\CMSIS\Include\core_cm33.h \
+  D:\keil5\ARM\ARMCLANG\include\stdint.h \
+  D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\cmsis_version.h \
+  D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\cmsis_compiler.h \
+  D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\cmsis_armclang.h \
+  D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\mpu_armv8.h \
+  ..\Drivers\CMSIS\Device\ST\STM32H5xx\Include\system_stm32h5xx.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\Legacy\stm32_hal_legacy.h \
+  D:\keil5\ARM\ARMCLANG\include\stddef.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_rcc_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_gpio.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_gpio_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_dma.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_dma_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_cortex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_flash.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_flash_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_pwr.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_pwr_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_tim.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_tim_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_uart.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_uart_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_exti.h \
+  ..\Core\Inc\memorymap.h ..\Core\Inc\usart.h ..\Core\Inc\gpio.h \
+  ..\Core\Inc\gpdma.h ..\Core\Inc\tim.h \
+  D:\keil5\ARM\ARMCLANG\include\stdio.h \
+  D:\keil5\ARM\ARMCLANG\include\stdarg.h ..\fun\HCBle.h ..\fun\gps.h \
+  ..\fun\Shake_Motor.h ..\fun\Ultrasound.h ..\fun\Buzzer.h \
+  ..\fun\Motor.h ..\fun\IMU.h ..\fun\imu948.h ..\fun\encoder.h \
+  ..\fun\value.h
--- a/MDK-ARM/AutoGuideStick/app_threadx.o
+++ b/MDK-ARM/AutoGuideStick/app_threadx.o
--- a/MDK-ARM/AutoGuideStick/buzzer.d
+++ b/MDK-ARM/AutoGuideStick/buzzer.d
@@ -0,0 +1,43 @@
+autoguidestick/buzzer.o: ..\fun\Buzzer.c ..\fun\Buzzer.h \
+  ..\fun\headfile.h ..\Core\Inc\main.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal.h \
+  ..\Core\Inc\stm32h5xx_hal_conf.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_rcc.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_def.h \
+  ..\Drivers\CMSIS\Device\ST\STM32H5xx\Include\stm32h5xx.h \
+  D:\keil5\ARM\ARMCLANG\include\math.h \
+  ..\Drivers\CMSIS\Device\ST\STM32H5xx\Include\stm32h563xx.h \
+  ..\Drivers\CMSIS\Include\core_cm33.h \
+  D:\keil5\ARM\ARMCLANG\include\stdint.h \
+  D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\cmsis_version.h \
+  D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\cmsis_compiler.h \
+  D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\cmsis_armclang.h \
+  D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\mpu_armv8.h \
+  ..\Drivers\CMSIS\Device\ST\STM32H5xx\Include\system_stm32h5xx.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\Legacy\stm32_hal_legacy.h \
+  D:\keil5\ARM\ARMCLANG\include\stddef.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_rcc_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_gpio.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_gpio_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_dma.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_dma_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_cortex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_flash.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_flash_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_pwr.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_pwr_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_tim.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_tim_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_uart.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_uart_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_exti.h \
+  ..\Core\Inc\memorymap.h ..\Core\Inc\usart.h ..\Core\Inc\gpio.h \
+  ..\Core\Inc\gpdma.h ..\Core\Inc\tim.h ..\Core\Inc\app_threadx.h \
+  ..\Middlewares\ST\threadx\common\inc\tx_api.h \
+  ..\Middlewares\ST\threadx\ports\cortex_m33\ac6\inc\tx_port.h \
+  ..\Core\Inc\tx_user.h D:\keil5\ARM\ARMCLANG\include\stdlib.h \
+  D:\keil5\ARM\ARMCLANG\include\string.h \
+  D:\keil5\ARM\ARMCLANG\include\stdio.h \
+  D:\keil5\ARM\ARMCLANG\include\stdarg.h ..\fun\HCBle.h ..\fun\gps.h \
+  ..\fun\Shake_Motor.h ..\fun\Ultrasound.h ..\fun\Motor.h ..\fun\IMU.h \
+  ..\fun\imu948.h ..\fun\encoder.h ..\fun\value.h
--- a/MDK-ARM/AutoGuideStick/buzzer.o
+++ b/MDK-ARM/AutoGuideStick/buzzer.o
--- a/MDK-ARM/AutoGuideStick/encoder.d
+++ b/MDK-ARM/AutoGuideStick/encoder.d
@@ -0,0 +1,43 @@
+autoguidestick/encoder.o: ..\fun\encoder.c ..\fun\encoder.h \
+  ..\fun\headfile.h ..\Core\Inc\main.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal.h \
+  ..\Core\Inc\stm32h5xx_hal_conf.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_rcc.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_def.h \
+  ..\Drivers\CMSIS\Device\ST\STM32H5xx\Include\stm32h5xx.h \
+  D:\keil5\ARM\ARMCLANG\include\math.h \
+  ..\Drivers\CMSIS\Device\ST\STM32H5xx\Include\stm32h563xx.h \
+  ..\Drivers\CMSIS\Include\core_cm33.h \
+  D:\keil5\ARM\ARMCLANG\include\stdint.h \
+  D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\cmsis_version.h \
+  D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\cmsis_compiler.h \
+  D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\cmsis_armclang.h \
+  D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\mpu_armv8.h \
+  ..\Drivers\CMSIS\Device\ST\STM32H5xx\Include\system_stm32h5xx.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\Legacy\stm32_hal_legacy.h \
+  D:\keil5\ARM\ARMCLANG\include\stddef.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_rcc_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_gpio.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_gpio_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_dma.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_dma_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_cortex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_flash.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_flash_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_pwr.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_pwr_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_tim.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_tim_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_uart.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_uart_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_exti.h \
+  ..\Core\Inc\memorymap.h ..\Core\Inc\usart.h ..\Core\Inc\gpio.h \
+  ..\Core\Inc\gpdma.h ..\Core\Inc\tim.h ..\Core\Inc\app_threadx.h \
+  ..\Middlewares\ST\threadx\common\inc\tx_api.h \
+  ..\Middlewares\ST\threadx\ports\cortex_m33\ac6\inc\tx_port.h \
+  ..\Core\Inc\tx_user.h D:\keil5\ARM\ARMCLANG\include\stdlib.h \
+  D:\keil5\ARM\ARMCLANG\include\string.h \
+  D:\keil5\ARM\ARMCLANG\include\stdio.h \
+  D:\keil5\ARM\ARMCLANG\include\stdarg.h ..\fun\HCBle.h ..\fun\gps.h \
+  ..\fun\Shake_Motor.h ..\fun\Ultrasound.h ..\fun\Buzzer.h \
+  ..\fun\Motor.h ..\fun\IMU.h ..\fun\imu948.h ..\fun\value.h
--- a/MDK-ARM/AutoGuideStick/encoder.o
+++ b/MDK-ARM/AutoGuideStick/encoder.o
--- a/MDK-ARM/AutoGuideStick/gpdma.o
+++ b/MDK-ARM/AutoGuideStick/gpdma.o
--- a/MDK-ARM/AutoGuideStick/gpio.o
+++ b/MDK-ARM/AutoGuideStick/gpio.o
--- a/MDK-ARM/AutoGuideStick/gps.d
+++ b/MDK-ARM/AutoGuideStick/gps.d
@@ -0,0 +1,43 @@
+autoguidestick/gps.o: ..\fun\gps.c ..\fun\gps.h ..\fun\headfile.h \
+  ..\Core\Inc\main.h ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal.h \
+  ..\Core\Inc\stm32h5xx_hal_conf.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_rcc.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_def.h \
+  ..\Drivers\CMSIS\Device\ST\STM32H5xx\Include\stm32h5xx.h \
+  D:\keil5\ARM\ARMCLANG\include\math.h \
+  ..\Drivers\CMSIS\Device\ST\STM32H5xx\Include\stm32h563xx.h \
+  ..\Drivers\CMSIS\Include\core_cm33.h \
+  D:\keil5\ARM\ARMCLANG\include\stdint.h \
+  D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\cmsis_version.h \
+  D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\cmsis_compiler.h \
+  D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\cmsis_armclang.h \
+  D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\mpu_armv8.h \
+  ..\Drivers\CMSIS\Device\ST\STM32H5xx\Include\system_stm32h5xx.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\Legacy\stm32_hal_legacy.h \
+  D:\keil5\ARM\ARMCLANG\include\stddef.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_rcc_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_gpio.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_gpio_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_dma.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_dma_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_cortex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_flash.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_flash_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_pwr.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_pwr_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_tim.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_tim_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_uart.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_uart_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_exti.h \
+  ..\Core\Inc\memorymap.h ..\Core\Inc\usart.h ..\Core\Inc\gpio.h \
+  ..\Core\Inc\gpdma.h ..\Core\Inc\tim.h ..\Core\Inc\app_threadx.h \
+  ..\Middlewares\ST\threadx\common\inc\tx_api.h \
+  ..\Middlewares\ST\threadx\ports\cortex_m33\ac6\inc\tx_port.h \
+  ..\Core\Inc\tx_user.h D:\keil5\ARM\ARMCLANG\include\stdlib.h \
+  D:\keil5\ARM\ARMCLANG\include\string.h \
+  D:\keil5\ARM\ARMCLANG\include\stdio.h \
+  D:\keil5\ARM\ARMCLANG\include\stdarg.h ..\fun\HCBle.h \
+  ..\fun\Shake_Motor.h ..\fun\Ultrasound.h ..\fun\Buzzer.h \
+  ..\fun\Motor.h ..\fun\IMU.h ..\fun\imu948.h ..\fun\encoder.h \
+  ..\fun\value.h
--- a/MDK-ARM/AutoGuideStick/gps.o
+++ b/MDK-ARM/AutoGuideStick/gps.o
--- a/MDK-ARM/AutoGuideStick/hcble.d
+++ b/MDK-ARM/AutoGuideStick/hcble.d
@@ -1,10 +1,5 @@
 autoguidestick/hcble.o: ..\fun\HCBle.c ..\fun\HCBle.h ..\fun\headfile.h \
-  ..\Core\Inc\app_threadx.h \
-  ..\Middlewares\ST\threadx\common\inc\tx_api.h \
-  ..\Middlewares\ST\threadx\ports\cortex_m33\ac6\inc\tx_port.h \
-  ..\Core\Inc\tx_user.h D:\keil5\ARM\ARMCLANG\include\stdlib.h \
-  D:\keil5\ARM\ARMCLANG\include\string.h ..\Core\Inc\main.h \
-  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal.h \
+  ..\Core\Inc\main.h ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal.h \
  ..\Core\Inc\stm32h5xx_hal_conf.h \
  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_rcc.h \
  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_def.h \
@@ -36,5 +31,13 @@ autoguidestick/hcble.o: ..\fun\HCBle.c ..\fun\HCBle.h ..\fun\headfile.h \
  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_uart_ex.h \
  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_exti.h \
  ..\Core\Inc\memorymap.h ..\Core\Inc\usart.h ..\Core\Inc\gpio.h \
-  ..\Core\Inc\gpdma.h D:\keil5\ARM\ARMCLANG\include\stdio.h \
-  D:\keil5\ARM\ARMCLANG\include\stdarg.h
+  ..\Core\Inc\gpdma.h ..\Core\Inc\tim.h ..\Core\Inc\app_threadx.h \
+  ..\Middlewares\ST\threadx\common\inc\tx_api.h \
+  ..\Middlewares\ST\threadx\ports\cortex_m33\ac6\inc\tx_port.h \
+  ..\Core\Inc\tx_user.h D:\keil5\ARM\ARMCLANG\include\stdlib.h \
+  D:\keil5\ARM\ARMCLANG\include\string.h \
+  D:\keil5\ARM\ARMCLANG\include\stdio.h \
+  D:\keil5\ARM\ARMCLANG\include\stdarg.h ..\fun\gps.h \
+  ..\fun\Shake_Motor.h ..\fun\Ultrasound.h ..\fun\Buzzer.h \
+  ..\fun\Motor.h ..\fun\IMU.h ..\fun\imu948.h ..\fun\encoder.h \
+  ..\fun\value.h
--- a/MDK-ARM/AutoGuideStick/hcble.o
+++ b/MDK-ARM/AutoGuideStick/hcble.o
--- a/MDK-ARM/AutoGuideStick/imu.d
+++ b/MDK-ARM/AutoGuideStick/imu.d
@@ -0,0 +1,42 @@
+autoguidestick/imu.o: ..\fun\IMU.c ..\fun\IMU.h ..\fun\headfile.h \
+  ..\Core\Inc\main.h ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal.h \
+  ..\Core\Inc\stm32h5xx_hal_conf.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_rcc.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_def.h \
+  ..\Drivers\CMSIS\Device\ST\STM32H5xx\Include\stm32h5xx.h \
+  D:\keil5\ARM\ARMCLANG\include\math.h \
+  ..\Drivers\CMSIS\Device\ST\STM32H5xx\Include\stm32h563xx.h \
+  ..\Drivers\CMSIS\Include\core_cm33.h \
+  D:\keil5\ARM\ARMCLANG\include\stdint.h \
+  D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\cmsis_version.h \
+  D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\cmsis_compiler.h \
+  D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\cmsis_armclang.h \
+  D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\mpu_armv8.h \
+  ..\Drivers\CMSIS\Device\ST\STM32H5xx\Include\system_stm32h5xx.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\Legacy\stm32_hal_legacy.h \
+  D:\keil5\ARM\ARMCLANG\include\stddef.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_rcc_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_gpio.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_gpio_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_dma.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_dma_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_cortex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_flash.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_flash_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_pwr.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_pwr_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_tim.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_tim_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_uart.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_uart_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_exti.h \
+  ..\Core\Inc\memorymap.h ..\Core\Inc\usart.h ..\Core\Inc\gpio.h \
+  ..\Core\Inc\gpdma.h ..\Core\Inc\tim.h ..\Core\Inc\app_threadx.h \
+  ..\Middlewares\ST\threadx\common\inc\tx_api.h \
+  ..\Middlewares\ST\threadx\ports\cortex_m33\ac6\inc\tx_port.h \
+  ..\Core\Inc\tx_user.h D:\keil5\ARM\ARMCLANG\include\stdlib.h \
+  D:\keil5\ARM\ARMCLANG\include\string.h \
+  D:\keil5\ARM\ARMCLANG\include\stdio.h \
+  D:\keil5\ARM\ARMCLANG\include\stdarg.h ..\fun\HCBle.h ..\fun\gps.h \
+  ..\fun\Shake_Motor.h ..\fun\Ultrasound.h ..\fun\Buzzer.h \
+  ..\fun\Motor.h ..\fun\imu948.h ..\fun\encoder.h ..\fun\value.h
--- a/MDK-ARM/AutoGuideStick/imu.o
+++ b/MDK-ARM/AutoGuideStick/imu.o
--- a/MDK-ARM/AutoGuideStick/imu948.d
+++ b/MDK-ARM/AutoGuideStick/imu948.d
@@ -0,0 +1,43 @@
+autoguidestick/imu948.o: ..\fun\imu948.c ..\fun\imu948.h \
+  ..\fun\headfile.h ..\Core\Inc\main.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal.h \
+  ..\Core\Inc\stm32h5xx_hal_conf.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_rcc.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_def.h \
+  ..\Drivers\CMSIS\Device\ST\STM32H5xx\Include\stm32h5xx.h \
+  D:\keil5\ARM\ARMCLANG\include\math.h \
+  ..\Drivers\CMSIS\Device\ST\STM32H5xx\Include\stm32h563xx.h \
+  ..\Drivers\CMSIS\Include\core_cm33.h \
+  D:\keil5\ARM\ARMCLANG\include\stdint.h \
+  D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\cmsis_version.h \
+  D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\cmsis_compiler.h \
+  D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\cmsis_armclang.h \
+  D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\mpu_armv8.h \
+  ..\Drivers\CMSIS\Device\ST\STM32H5xx\Include\system_stm32h5xx.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\Legacy\stm32_hal_legacy.h \
+  D:\keil5\ARM\ARMCLANG\include\stddef.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_rcc_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_gpio.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_gpio_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_dma.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_dma_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_cortex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_flash.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_flash_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_pwr.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_pwr_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_tim.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_tim_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_uart.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_uart_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_exti.h \
+  ..\Core\Inc\memorymap.h ..\Core\Inc\usart.h ..\Core\Inc\gpio.h \
+  ..\Core\Inc\gpdma.h ..\Core\Inc\tim.h ..\Core\Inc\app_threadx.h \
+  ..\Middlewares\ST\threadx\common\inc\tx_api.h \
+  ..\Middlewares\ST\threadx\ports\cortex_m33\ac6\inc\tx_port.h \
+  ..\Core\Inc\tx_user.h D:\keil5\ARM\ARMCLANG\include\stdlib.h \
+  D:\keil5\ARM\ARMCLANG\include\string.h \
+  D:\keil5\ARM\ARMCLANG\include\stdio.h \
+  D:\keil5\ARM\ARMCLANG\include\stdarg.h ..\fun\HCBle.h ..\fun\gps.h \
+  ..\fun\Shake_Motor.h ..\fun\Ultrasound.h ..\fun\Buzzer.h \
+  ..\fun\Motor.h ..\fun\IMU.h ..\fun\encoder.h ..\fun\value.h
--- a/MDK-ARM/AutoGuideStick/imu948.o
+++ b/MDK-ARM/AutoGuideStick/imu948.o
--- a/MDK-ARM/AutoGuideStick/main.d
+++ b/MDK-ARM/AutoGuideStick/main.d
@@ -2,8 +2,8 @@ autoguidestick/main.o: ..\Core\Src\main.c ..\Core\Inc\app_threadx.h \
  ..\Middlewares\ST\threadx\common\inc\tx_api.h \
  ..\Middlewares\ST\threadx\ports\cortex_m33\ac6\inc\tx_port.h \
  ..\Core\Inc\tx_user.h D:\keil5\ARM\ARMCLANG\include\stdlib.h \
-  D:\keil5\ARM\ARMCLANG\include\string.h ..\Core\Inc\main.h \
-  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal.h \
+  D:\keil5\ARM\ARMCLANG\include\string.h ..\fun\headfile.h \
+  ..\Core\Inc\main.h ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal.h \
  ..\Core\Inc\stm32h5xx_hal_conf.h \
  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_rcc.h \
  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_def.h \
@@ -34,5 +34,10 @@ autoguidestick/main.o: ..\Core\Src\main.c ..\Core\Inc\app_threadx.h \
  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_uart.h \
  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_uart_ex.h \
  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_exti.h \
-  ..\Core\Inc\gpdma.h ..\Core\Inc\memorymap.h ..\Core\Inc\usart.h \
-  ..\Core\Inc\gpio.h
+  ..\Core\Inc\memorymap.h ..\Core\Inc\usart.h ..\Core\Inc\gpio.h \
+  ..\Core\Inc\gpdma.h ..\Core\Inc\tim.h \
+  D:\keil5\ARM\ARMCLANG\include\stdio.h \
+  D:\keil5\ARM\ARMCLANG\include\stdarg.h ..\fun\HCBle.h ..\fun\gps.h \
+  ..\fun\Shake_Motor.h ..\fun\Ultrasound.h ..\fun\Buzzer.h \
+  ..\fun\Motor.h ..\fun\IMU.h ..\fun\imu948.h ..\fun\encoder.h \
+  ..\fun\value.h
--- a/MDK-ARM/AutoGuideStick/main.o
+++ b/MDK-ARM/AutoGuideStick/main.o
--- a/MDK-ARM/AutoGuideStick/motor.d
+++ b/MDK-ARM/AutoGuideStick/motor.d
@@ -0,0 +1,42 @@
+autoguidestick/motor.o: ..\fun\Motor.c ..\fun\Motor.h ..\fun\headfile.h \
+  ..\Core\Inc\main.h ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal.h \
+  ..\Core\Inc\stm32h5xx_hal_conf.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_rcc.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_def.h \
+  ..\Drivers\CMSIS\Device\ST\STM32H5xx\Include\stm32h5xx.h \
+  D:\keil5\ARM\ARMCLANG\include\math.h \
+  ..\Drivers\CMSIS\Device\ST\STM32H5xx\Include\stm32h563xx.h \
+  ..\Drivers\CMSIS\Include\core_cm33.h \
+  D:\keil5\ARM\ARMCLANG\include\stdint.h \
+  D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\cmsis_version.h \
+  D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\cmsis_compiler.h \
+  D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\cmsis_armclang.h \
+  D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\mpu_armv8.h \
+  ..\Drivers\CMSIS\Device\ST\STM32H5xx\Include\system_stm32h5xx.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\Legacy\stm32_hal_legacy.h \
+  D:\keil5\ARM\ARMCLANG\include\stddef.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_rcc_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_gpio.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_gpio_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_dma.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_dma_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_cortex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_flash.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_flash_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_pwr.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_pwr_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_tim.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_tim_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_uart.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_uart_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_exti.h \
+  ..\Core\Inc\memorymap.h ..\Core\Inc\usart.h ..\Core\Inc\gpio.h \
+  ..\Core\Inc\gpdma.h ..\Core\Inc\tim.h ..\Core\Inc\app_threadx.h \
+  ..\Middlewares\ST\threadx\common\inc\tx_api.h \
+  ..\Middlewares\ST\threadx\ports\cortex_m33\ac6\inc\tx_port.h \
+  ..\Core\Inc\tx_user.h D:\keil5\ARM\ARMCLANG\include\stdlib.h \
+  D:\keil5\ARM\ARMCLANG\include\string.h \
+  D:\keil5\ARM\ARMCLANG\include\stdio.h \
+  D:\keil5\ARM\ARMCLANG\include\stdarg.h ..\fun\HCBle.h ..\fun\gps.h \
+  ..\fun\Shake_Motor.h ..\fun\Ultrasound.h ..\fun\Buzzer.h ..\fun\IMU.h \
+  ..\fun\imu948.h ..\fun\encoder.h ..\fun\value.h
--- a/MDK-ARM/AutoGuideStick/motor.o
+++ b/MDK-ARM/AutoGuideStick/motor.o
--- a/MDK-ARM/AutoGuideStick/shake_motor.d
+++ b/MDK-ARM/AutoGuideStick/shake_motor.d
@@ -0,0 +1,43 @@
+autoguidestick/shake_motor.o: ..\fun\Shake_Motor.c ..\fun\Shake_Motor.h \
+  ..\fun\headfile.h ..\Core\Inc\main.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal.h \
+  ..\Core\Inc\stm32h5xx_hal_conf.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_rcc.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_def.h \
+  ..\Drivers\CMSIS\Device\ST\STM32H5xx\Include\stm32h5xx.h \
+  D:\keil5\ARM\ARMCLANG\include\math.h \
+  ..\Drivers\CMSIS\Device\ST\STM32H5xx\Include\stm32h563xx.h \
+  ..\Drivers\CMSIS\Include\core_cm33.h \
+  D:\keil5\ARM\ARMCLANG\include\stdint.h \
+  D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\cmsis_version.h \
+  D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\cmsis_compiler.h \
+  D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\cmsis_armclang.h \
+  D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\mpu_armv8.h \
+  ..\Drivers\CMSIS\Device\ST\STM32H5xx\Include\system_stm32h5xx.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\Legacy\stm32_hal_legacy.h \
+  D:\keil5\ARM\ARMCLANG\include\stddef.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_rcc_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_gpio.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_gpio_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_dma.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_dma_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_cortex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_flash.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_flash_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_pwr.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_pwr_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_tim.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_tim_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_uart.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_uart_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_exti.h \
+  ..\Core\Inc\memorymap.h ..\Core\Inc\usart.h ..\Core\Inc\gpio.h \
+  ..\Core\Inc\gpdma.h ..\Core\Inc\tim.h ..\Core\Inc\app_threadx.h \
+  ..\Middlewares\ST\threadx\common\inc\tx_api.h \
+  ..\Middlewares\ST\threadx\ports\cortex_m33\ac6\inc\tx_port.h \
+  ..\Core\Inc\tx_user.h D:\keil5\ARM\ARMCLANG\include\stdlib.h \
+  D:\keil5\ARM\ARMCLANG\include\string.h \
+  D:\keil5\ARM\ARMCLANG\include\stdio.h \
+  D:\keil5\ARM\ARMCLANG\include\stdarg.h ..\fun\HCBle.h ..\fun\gps.h \
+  ..\fun\Ultrasound.h ..\fun\Buzzer.h ..\fun\Motor.h ..\fun\IMU.h \
+  ..\fun\imu948.h ..\fun\encoder.h ..\fun\value.h
--- a/MDK-ARM/AutoGuideStick/shake_motor.o
+++ b/MDK-ARM/AutoGuideStick/shake_motor.o
--- a/MDK-ARM/AutoGuideStick/startup_stm32h563xx.o
+++ b/MDK-ARM/AutoGuideStick/startup_stm32h563xx.o
--- a/MDK-ARM/AutoGuideStick/stm32h5xx_hal_timebase_tim.o
+++ b/MDK-ARM/AutoGuideStick/stm32h5xx_hal_timebase_tim.o
--- a/MDK-ARM/AutoGuideStick/stm32h5xx_it.d
+++ b/MDK-ARM/AutoGuideStick/stm32h5xx_it.d
@@ -30,11 +30,15 @@ autoguidestick/stm32h5xx_it.o: ..\Core\Src\stm32h5xx_it.c \
  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_uart.h \
  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_uart_ex.h \
  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_exti.h \
-  ..\Core\Inc\stm32h5xx_it.h ..\fun\headfile.h ..\Core\Inc\app_threadx.h \
+  ..\Core\Inc\stm32h5xx_it.h ..\fun\headfile.h ..\Core\Inc\memorymap.h \
+  ..\Core\Inc\usart.h ..\Core\Inc\gpio.h ..\Core\Inc\gpdma.h \
+  ..\Core\Inc\tim.h ..\Core\Inc\app_threadx.h \
  ..\Middlewares\ST\threadx\common\inc\tx_api.h \
  ..\Middlewares\ST\threadx\ports\cortex_m33\ac6\inc\tx_port.h \
  ..\Core\Inc\tx_user.h D:\keil5\ARM\ARMCLANG\include\stdlib.h \
-  D:\keil5\ARM\ARMCLANG\include\string.h ..\Core\Inc\memorymap.h \
-  ..\Core\Inc\usart.h ..\Core\Inc\gpio.h ..\Core\Inc\gpdma.h \
+  D:\keil5\ARM\ARMCLANG\include\string.h \
  D:\keil5\ARM\ARMCLANG\include\stdio.h \
-  D:\keil5\ARM\ARMCLANG\include\stdarg.h ..\fun\HCBle.h
+  D:\keil5\ARM\ARMCLANG\include\stdarg.h ..\fun\HCBle.h ..\fun\gps.h \
+  ..\fun\Shake_Motor.h ..\fun\Ultrasound.h ..\fun\Buzzer.h \
+  ..\fun\Motor.h ..\fun\IMU.h ..\fun\imu948.h ..\fun\encoder.h \
+  ..\fun\value.h
--- a/MDK-ARM/AutoGuideStick/stm32h5xx_it.o
+++ b/MDK-ARM/AutoGuideStick/stm32h5xx_it.o
--- a/MDK-ARM/AutoGuideStick/tim.d
+++ b/MDK-ARM/AutoGuideStick/tim.d
@@ -0,0 +1,32 @@
+autoguidestick/tim.o: ..\Core\Src\tim.c ..\Core\Inc\tim.h \
+  ..\Core\Inc\main.h ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal.h \
+  ..\Core\Inc\stm32h5xx_hal_conf.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_rcc.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_def.h \
+  ..\Drivers\CMSIS\Device\ST\STM32H5xx\Include\stm32h5xx.h \
+  D:\keil5\ARM\ARMCLANG\include\math.h \
+  ..\Drivers\CMSIS\Device\ST\STM32H5xx\Include\stm32h563xx.h \
+  ..\Drivers\CMSIS\Include\core_cm33.h \
+  D:\keil5\ARM\ARMCLANG\include\stdint.h \
+  D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\cmsis_version.h \
+  D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\cmsis_compiler.h \
+  D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\cmsis_armclang.h \
+  D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\mpu_armv8.h \
+  ..\Drivers\CMSIS\Device\ST\STM32H5xx\Include\system_stm32h5xx.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\Legacy\stm32_hal_legacy.h \
+  D:\keil5\ARM\ARMCLANG\include\stddef.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_rcc_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_gpio.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_gpio_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_dma.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_dma_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_cortex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_flash.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_flash_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_pwr.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_pwr_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_tim.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_tim_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_uart.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_uart_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_exti.h
--- a/MDK-ARM/AutoGuideStick/tim.o
+++ b/MDK-ARM/AutoGuideStick/tim.o
--- a/MDK-ARM/AutoGuideStick/txe_thread_create.o
+++ b/MDK-ARM/AutoGuideStick/txe_thread_create.o
--- a/MDK-ARM/AutoGuideStick/ultrasound.d
+++ b/MDK-ARM/AutoGuideStick/ultrasound.d
@@ -0,0 +1,43 @@
+autoguidestick/ultrasound.o: ..\fun\Ultrasound.c ..\fun\Ultrasound.h \
+  ..\fun\headfile.h ..\Core\Inc\main.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal.h \
+  ..\Core\Inc\stm32h5xx_hal_conf.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_rcc.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_def.h \
+  ..\Drivers\CMSIS\Device\ST\STM32H5xx\Include\stm32h5xx.h \
+  D:\keil5\ARM\ARMCLANG\include\math.h \
+  ..\Drivers\CMSIS\Device\ST\STM32H5xx\Include\stm32h563xx.h \
+  ..\Drivers\CMSIS\Include\core_cm33.h \
+  D:\keil5\ARM\ARMCLANG\include\stdint.h \
+  D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\cmsis_version.h \
+  D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\cmsis_compiler.h \
+  D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\cmsis_armclang.h \
+  D:\advance_stick\AutoGuideStick\Drivers\CMSIS\Include\mpu_armv8.h \
+  ..\Drivers\CMSIS\Device\ST\STM32H5xx\Include\system_stm32h5xx.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\Legacy\stm32_hal_legacy.h \
+  D:\keil5\ARM\ARMCLANG\include\stddef.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_rcc_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_gpio.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_gpio_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_dma.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_dma_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_cortex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_flash.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_flash_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_pwr.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_pwr_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_tim.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_tim_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_uart.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_uart_ex.h \
+  ..\Drivers\STM32H5xx_HAL_Driver\Inc\stm32h5xx_hal_exti.h \
+  ..\Core\Inc\memorymap.h ..\Core\Inc\usart.h ..\Core\Inc\gpio.h \
+  ..\Core\Inc\gpdma.h ..\Core\Inc\tim.h ..\Core\Inc\app_threadx.h \
+  ..\Middlewares\ST\threadx\common\inc\tx_api.h \
+  ..\Middlewares\ST\threadx\ports\cortex_m33\ac6\inc\tx_port.h \
+  ..\Core\Inc\tx_user.h D:\keil5\ARM\ARMCLANG\include\stdlib.h \
+  D:\keil5\ARM\ARMCLANG\include\string.h \
+  D:\keil5\ARM\ARMCLANG\include\stdio.h \
+  D:\keil5\ARM\ARMCLANG\include\stdarg.h ..\fun\HCBle.h ..\fun\gps.h \
+  ..\fun\Shake_Motor.h ..\fun\Buzzer.h ..\fun\Motor.h ..\fun\IMU.h \
+  ..\fun\imu948.h ..\fun\encoder.h ..\fun\value.h
--- a/MDK-ARM/AutoGuideStick/ultrasound.o
+++ b/MDK-ARM/AutoGuideStick/ultrasound.o
--- a/MDK-ARM/AutoGuideStick/usart.o
+++ b/MDK-ARM/AutoGuideStick/usart.o
--- a/MDK-ARM/EventRecorderStub.scvd
+++ b/MDK-ARM/EventRecorderStub.scvd
@@ -0,0 +1,9 @@
+<?xml version="1.0" encoding="utf-8"?>
+
+<component_viewer schemaVersion="0.1" xmlns:xs="http://www.w3.org/2001/XMLSchema-instance" xs:noNamespaceSchemaLocation="Component_Viewer.xsd">
+
+<component name="EventRecorderStub" version="1.0.0"/>       <!--name and version of the component-->
+  <events>
+  </events>
+
+</component_viewer>
--- a/README.md
+++ b/README.md
@@ -0,0 +1,107 @@
+# ManGoWalk  --- 智能盲杖导航系统
+<a name="my-desiki-anime-girl"></a>
+![随心一听](https://tse1-mm.cn.bing.net/th/id/OIP-C.lOSBhSbq-aQG17hhtzgXNAHaKz?rs=1&pid=ImgDetMain)
+
+## 日志
+2025.6.4  
+添加了Thread X 操作系统
+
+2025.6.25
+# README: STM32H563 ThreadX Embedded AI System
+
+## ✅ 当前已实现功能
+
+本系统基于 STM32H563ZIT6 和 ThreadX 实时操作系统，模块化地实现了以下核心功能：
+
+### 1. BLE 通信模块（HCBle.c）
+
+* 支持 DMA + 空闲中断模式接收
+* 支持 JSON 指令解析与封装（如：`{"leftSpeed":X,"rightSpeed":Y}`）
+* 支持 BLE 指令转事件 + 数据发送任务队列
+
+### 2. GPS 定位模块（gps.c）
+
+* 支持 DMA + 固定长度接收
+* 提供位置数据 `lat/lon` 解析与格式转换
+* 冷启动支持（首次 GPS fix 时发送提示）
+
+### 3. IMU 模块（imu.c）
+
+* 模拟读取 X 轴角度（后续替换为实际驱动） 暂时还未完成
+* 结合 GPS 信息生成完整的 Location JSON 包发送至 APP
+
+### 4. 超声波避障模块（Ultrasound.c）
+
+* 支持 DWT 微秒延迟与定时捕获
+* 检测障碍 <30cm 自动触发蜂鸣器、震动器
+
+### 5. 控制逻辑线程（Main\_Control）
+
+* 管理 BLE 控制与避障响应行为
+* 控制蜂鸣器与振动提示器
+
+### 6. 多线程调度（ThreadX）
+
+* 所有任务通过 ThreadX 线程调度与事件中转完成解耦
+* BLE/GPS/IMU/Ultrasound 主从线程结构分离清晰
+
+## 🚧 尚未实现或测试模块
+
+| 模块               | 当前状态  | 说明                                  |
+| ---------------- | ----- | ----------------------------------- |
+| IMU 实际驱动         | ❌未实现  | 当前为模拟值 95°，需替换为传感器读取                |
+| OpenMV 视觉模块      | ❌未接入  | 后续可通过 UART/USB 接口处理红绿灯识别            |
+| 电机控制接口           | ❌未实现  | BLE 指令接收后尚未真正调用 `set_motor_speed()` |
+| UI 显示 / APP 交互测试 | 🔄进行中 | JSON 包格式统一，需手机APP实际验证               |
+
+## 🗺️ 系统拓扑图参考
+
+请参考：`ThreadX 系统结构图.png`
+<img src="/TuoPu.png" alt="ThreadX 系统结构图" style="zoom:50%;" />
+
+## 🗺️ 引脚配置参考
+请参考：`Point 引脚配置图.png`
+<img src="/point.png" alt="Point 引脚配置图" style="zoom:50%;" />
+对于图中的有更改过的部分引脚进行说明
+|引脚名称|用途|外设|
+|PC9|HC_Trig|超声波|
+|PC8|HC_Echo|超声波|
+|PB8|蓝牙接收---GPDMA1|BLE|
+|PB9|蓝牙发送---GPDMA1|BLE|
+|PD6|GPS接收---GPDMA1|GPS|
+|PD5|GPS发送|GPS|
+
+
+## 📋 开发环境配置说明
+
+### 软件工具
+
+* **IDE**：STM32CubeIDE / Keil MDK
+* **RTOS**：ThreadX (via STM32CubeMX Middleware)
+* **串口调试**：串口助手、BLE调试助手
+
+### 编译配置
+
+* 使用 `-O1` 优化选项
+* 启用 STM32CubeMX 中的 ThreadX 中间件
+* 打开 UART DMA + IDLE、定时器中断等相关配置
+
+### 推荐硬件连接
+
+* **GPS模块**：连接 USART2 (DMA + TC 中断)
+* **BLE模块**：连接 USART1 (DMA + IDLE)
+* **超声波模块**：使用 TIM2 输入捕获
+* **IMU模块**：I2C / SPI 接口（待扩展）
+* **蜂鸣器与震动马达**：普通 GPIO 控制
+
+## 🛠️ 接下来建议的开发步骤
+
+1. **完成 IMU 驱动集成**：读取 MPU6050 / ICM20948 等传感器
+2. **实现电机控制接口**：将 BLE 指令转换为 PWM 或电机驱动调用
+3. **集成 OpenMV 模块**：红绿灯识别后通过事件机制上报
+4. **完善 APP 端联调**：调试 BLE 接收逻辑，验证 JSON 包格式
+5. **加入 Watchdog 与线程看门狗**：防止任务异常卡死
+6. **优化能耗管理**：使用 ThreadX 的 tickless 模式管理低功耗
+
+---
+
--- a/TuoPu.png
+++ b/TuoPu.png
--- a/fun/Buzzer.c
+++ b/fun/Buzzer.c
@@ -0,0 +1,11 @@
+#include "Buzzer.h"
+
+void Buzzer_Open(void)
+{
+	HAL_GPIO_WritePin(Buzzer_GPIO_Port,Buzzer_Pin,GPIO_PIN_SET); // <20>ߵ<EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD>
+}
+
+void Buzzer_Close(void)
+{
+	HAL_GPIO_WritePin(Buzzer_GPIO_Port,Buzzer_Pin,GPIO_PIN_RESET); // <20>͵<EFBFBD>ƽ<EFBFBD>ر<EFBFBD>
+}
--- a/fun/Buzzer.h
+++ b/fun/Buzzer.h
@@ -0,0 +1,9 @@
+#ifndef __BUZZER_H__
+#define __BUZZER_H__
+
+#include "headfile.h"
+
+void Buzzer_Open(void);
+void Buzzer_Close(void);
+
+#endif
--- a/fun/HCBle.c
+++ b/fun/HCBle.c
@@ -1,7 +1,22 @@
 #include "HCBle.h"

+/**
+<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> δ<><CEB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+
+<EFBFBD><EFBFBD>ʾ: <20><>Ȼ BLE <20>Լ<EFBFBD> GPS<50><53><EFBFBD><EFBFBD>DMA+ UART<52><54><EFBFBD>գ<EFBFBD><D5A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͬ
+BLE --- DMA + IDLE <20>ж<EFBFBD>  ѭ<><D1AD> DMA + <20><><EFBFBD><EFBFBD><EFBFBD>жϴ<D0B6><CFB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>  UART IDLE  
+GPS --- DMA +<2B>̶<EFBFBD><CCB6><EFBFBD><EFBFBD><EFBFBD> + TC<54>ж<EFBFBD>   <20><><EFBFBD>ù̶<C3B9><CCB6><EFBFBD><EFBFBD><EFBFBD> DMA    DMA<4D><41><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
+
+#{
+  "lat": 23.123456,
+  "lon": 113.654321,
+  "angle": 95.0
+}\n
+**/
+
 // <20>ⲿ<EFBFBD><E2B2BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
-extern   UART_HandleTypeDef huart1;
+
+extern   UART_HandleTypeDef huart4;
 extern DMA_HandleTypeDef handle_GPDMA1_Channel5;
 extern DMA_HandleTypeDef handle_GPDMA1_Channel4;

@@ -18,12 +33,27 @@ uint8_t  rx_data;    //
 uint8_t uart_dma_rx_buf[UART_DMA_RX_BUF_SIZE];
 RingBuffer ble_rx_ring = {0}; //<2F><>ʼ<EFBFBD><CABC>
 MotorCommand cmd;
+BleMessage current_location = {0};
+float imu_angle = 0.0f;
+
+TX_EVENT_FLAGS_GROUP ble_event_flags;
+#define BLE_EVENT_DATA_READY  0x01
+
+
+void HCBle_InitEventFlags(void)
+{	tx_event_flags_create(&ble_event_flags, "BLE Events");
+	tx_event_flags_create(&system_events,"gps Events");
+//	tx_event_flags_create(&sensor_events,"Sensor Events");
+}

 //<2F><>ʼ<EFBFBD><CABC>DMA<4D><41><EFBFBD>պ<EFBFBD><D5BA><EFBFBD>
 void HCBle_InitDMAReception(void)
 {
-	HAL_UART_Receive_DMA(&huart1,uart_dma_rx_buf,UART_DMA_RX_BUF_SIZE);
-	__HAL_UART_ENABLE_IT(&huart1,UART_IT_IDLE); //ʹ<>ÿ<EFBFBD><C3BF><EFBFBD><EFBFBD>ж<EFBFBD>
+	HAL_UARTEx_ReceiveToIdle_IT(&huart4, uart_dma_rx_buf, UART_DMA_RX_BUF_SIZE);
+//	HAL_UART_Receive_DMA(&huart4, uart_dma_rx_buf, UART_DMA_RX_BUF_SIZE);
+//	HAL_UARTEx_ReceiveToIdle_DMA(&huart4, uart_dma_rx_buf, UART_DMA_RX_BUF_SIZE);
+	// ֻ<><D6BB>Ҫֱ<D2AA>Ӵ<EFBFBD><D3B4><EFBFBD> <20><><EFBFBD>ڿ<EFBFBD><DABF>н<EFBFBD><D0BD><EFBFBD> 
+	// HAL_UARTEx_RxEventCallback --- <20><>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD>ص<EFBFBD>
 }


@@ -39,94 +69,152 @@ void HCBle_SendData(char *p,...)
 // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
 #ifdef DEBUG_EN
 	//  <20><>Ϣ<EFBFBD><CFA2><EFBFBD><EFBFBD><EFBFBD>ӿ<EFBFBD>
-	HAL_UART_Transmit(&huart1,(uint8_t *)HC_Send_Data,strlen(HC_Send_Data),100);
+	HAL_UART_Transmit(&huart4,(uint8_t *)HC_Send_Data,strlen(HC_Send_Data),100);
 // HAL_UART_Transmi(&huart1,(uint8_t *)formatBuf,strlen(formatBuf),1);
 // <20><><EFBFBD><EFBFBD>û<EFBFBD><C3BB>ʹ<EFBFBD><CAB9><EFBFBD>жϵĴ<CFB5><C4B4>ڷ<EFBFBD><DAB7>ͣ<EFBFBD><CDA3><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 
 #else 
-	HAL_UART_Transmit_DMA(&huart1, (uint8_t *)HC_Send_Data, strlen(HC_Send_Data));
+	HAL_UART_Transmit_DMA(&huart4, (uint8_t *)HC_Send_Data, strlen(HC_Send_Data));
 #endif	
 	
 }
 // <20><><EFBFBD>ڷ<EFBFBD><DAB7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD>Լ<EFBFBD><D4BC><EFBFBD>һ<EFBFBD><D2BB>DMA


-
-
-// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݽ<EFBFBD><DDBD><EFBFBD>
-void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
+// <20>ҷ<EFBFBD><D2B7>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD><EFBFBD> --- \n <20><><EFBFBD>ֿ<EFBFBD><D6BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD>Ծ<EFBFBD>һֱ<D2BB><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+void HAL_UARTEx_RxEventCallback(UART_HandleTypeDef *huart, uint16_t Size)
 {
-	if(huart->Instance == USART1)
-	{
-		if(rx_data == '\n')
-		{
-			if(rx_index > 0)
+    if (huart->Instance == UART4)
+    {
+        for (uint16_t i = 0; i < Size; i++)
+        {
+            uint8_t ch = uart_dma_rx_buf[i];
+            uint16_t next_head = (ble_rx_ring.head + 1) % RING_BUFFER_SIZE;
+
+			if(next_head != ble_rx_ring.tail)
 			{
-				HC_Recevie[rx_index] = '\0';
-				data_received = 1;
-				rx_index = 0;  //<2F><><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ַ<EFBFBD><D6B7><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD>ַ<EFBFBD><D6B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ĵ<EFBFBD><C4B4><EFBFBD><EFBFBD><EFBFBD> <20><>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ַ<EFBFBD><D6B7><EFBFBD> memset
-			}else if(rx_index < RX_DataSize - 1)
-			{
-				
-			}else
-			{
-				rx_index = 0; //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
-				memset(HC_Recevie,'\0',RX_DataSize);
+				 ble_rx_ring.buffer[ble_rx_ring.head] = ch;
+				 ble_rx_ring.head = next_head;
 			}
-		}
+        }
+
+		tx_event_flags_set(&ble_event_flags, BLE_EVENT_DATA_READY, TX_OR);
+        HAL_UARTEx_ReceiveToIdle_IT(&huart4, uart_dma_rx_buf, UART_DMA_RX_BUF_SIZE);
+    }
+	else if(huart->Instance == USART2) // gps<70>Ľ<EFBFBD><C4BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+	{
 		
-		HAL_UART_Receive_IT(huart,&rx_data,1);
+		//		// <20><><EFBFBD><EFBFBD> Size <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>յ<EFBFBD><D5B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+		memcpy(GPS.GPS_Buffer,GPS_DMA_RX_BUF,Size);
+		GPS.GPS_Buffer[Size] = '\0';
+		GPS.isGetData = 1;  //<2F><><EFBFBD>ݽ<EFBFBD><DDBD>ձ<EFBFBD>־λ<D6BE><CEBB>Ϊ1
+		
+		tx_event_flags_set(&system_events, EVENT_GPS_DATA_READY, TX_OR);
+
+		//<2F><><EFBFBD>¿<EFBFBD><C2BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+		HAL_UARTEx_ReceiveToIdle_IT(&huart2,GPS_DMA_RX_BUF,GPS_DMA_RX_BUF_LEN);
+//		HAL_UARTEx_ReceiveToIdle_DMA(&huart2,GPS_DMA_RX_BUF,GPS_DMA_RX_BUF_LEN);
 	}
+	
 }



-// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD><EFBFBD><EFBFBD> Json<6F><6E><EFBFBD>ݴ<EFBFBD><DDB4><EFBFBD> <20>Լ<EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݸ<EFBFBD><DDB8>ֻ<EFBFBD>App
-
-
-// Json<6F><6E><EFBFBD>ݽ<EFBFBD><DDBD><EFBFBD>  <20><><EFBFBD><EFBFBD>ʹ<EFBFBD>õ<EFBFBD>Thread x <20><><EFBFBD>н<EFBFBD><D0BD><EFBFBD>
-void HCBle_ExtractAndParseFrame(void)
+// <20>ѳɹ<EFBFBD>ʵ<EFBFBD><EFBFBD> Ble<6C><65>ֵ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+void HCBle_ParseAndHandleFrame(const char *frame)
 {
-	static char json_buf[128];
-	static int idx = 0;
-	static int in_frame = 0;
+    int left, right;
+    float lat, lon, angle;

-	while (ble_rx_ring.tail != ble_rx_ring.head)
-	{
-		char c = ble_rx_ring.buffer[ble_rx_ring.tail];
-		ble_rx_ring.tail = (ble_rx_ring.tail + 1) % RING_BUFFER_SIZE;
+    if (sscanf(frame, "#{\"leftSpeed\":%d,\"rightSpeed\":%d}$", &left, &right) == 2) {
+        cmd.LeftSpeed = left;
+        cmd.RightSpeed = right;
+		
+		// <20><><EFBFBD><EFBFBD><EFBFBD>ȼ<EFBFBD><C8BC><EFBFBD>
+		target_rpm_L = map_speed_to_rpm(cmd.LeftSpeed);
+		target_rpm_R = map_speed_to_rpm(cmd.RightSpeed);

-		// ֡<><D6A1>ʼ<EFBFBD><CABC>#
-		if (c == '#') {
-			idx = 0;
-			in_frame = 1;
-			json_buf[idx++] = c;
-		}
-		else if (in_frame) {
-			if (idx < sizeof(json_buf) - 1) {
-				json_buf[idx++] = c;
+        HCBle_SendData("left=%d, right=%d\r\n", cmd.LeftSpeed, cmd.RightSpeed);
+//		HCBle_SendData("left=%d, right=%d\r\n", left, right);
+		DriveBOTH(cmd.LeftSpeed,cmd.RightSpeed); //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+//		DriveBOTH(cmd.LeftSpeed,cmd.RightSpeed);
+        return;
+    }

-				// ֡<><D6A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD>\n
-				if (c == '\n') {
-					json_buf[idx] = '\0';

-					// ? JSON<4F><4E><EFBFBD><EFBFBD>
-					if (strstr(json_buf, "leftSpeed") && strstr(json_buf, "rightSpeed")) {
-						sscanf(json_buf, "#{\"leftSpeed\":%d,\"rightSpeed\":%d}", &cmd.LeftSpeed, &cmd.RightSpeed);
-						//SetMotorSpeed(cmd.LeftSpeed, cmd.RightSpeed); <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
-					}

-					in_frame = 0;
-					idx = 0;
-				}
-			} else {
-				// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǿ<EFBFBD><C7BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
-				in_frame = 0;
-				idx = 0;
-			}
-		}
-	}
+    HCBle_SendData("? <20><><EFBFBD><EFBFBD>ʧ<EFBFBD><CAA7>: %s\r\n", frame);
 }




+//Ϊɶһֱʹ<D6B1>ò<EFBFBD><C3B2>ˣ<EFBFBD><CBA3><EFBFBD><EFBFBD><EFBFBD>һֱ<D2BB>ٵȴ<D9B5>TX<54><58><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+// <20>ں<EFBFBD>̨<EFBFBD><CCA8><EFBFBD>е<EFBFBD>һ<EFBFBD><D2BB><EFBFBD>̣߳<DFB3><CCA3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȴ<EFBFBD>UART<52>յ<EFBFBD>BLE<4C><45><EFBFBD>ݺ󴥷<DDBA><F3B4A5B7><EFBFBD><EFBFBD>¼<EFBFBD><C2BC><EFBFBD>Ȼ<EFBFBD><C8BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Щ<EFBFBD><D0A9><EFBFBD><EFBFBD>
+void ble_rx_task_entry(ULONG thread_input)
+{
+    HCBle_InitDMAReception();
+
+    ULONG actual_flags;
+    static char json_buf[128];
+    static int idx = 0;
+    static int parsing = 0;
+
+    while (1)
+    {
+        tx_event_flags_get(&ble_event_flags, BLE_EVENT_DATA_READY, TX_OR_CLEAR, &actual_flags, TX_WAIT_FOREVER);
+
+        while (ble_rx_ring.tail != ble_rx_ring.head)
+        {
+            char c = ble_rx_ring.buffer[ble_rx_ring.tail];
+            ble_rx_ring.tail = (ble_rx_ring.tail + 1) % RING_BUFFER_SIZE;
+
+            if (!parsing) {
+                if (c == '#') {
+                    parsing = 1;
+                    idx = 0;
+                    json_buf[idx++] = c;
+                }
+                continue;
+            }
+
+            if (idx < sizeof(json_buf) - 1) {
+                json_buf[idx++] = c;
+                if (c == '$') {
+                    json_buf[idx] = '\0';
+                    HCBle_ParseAndHandleFrame(json_buf);
+                    parsing = 0;
+                    idx = 0;
+                }
+            } else {
+                parsing = 0;
+                idx = 0;
+            }
+        }
+    }
+}
+
+
+
+// "#{\"lat\":%.6f,\"lon\":%.6f,\"angle\":%.2f}\n",23.123456, 113.654321, 95.0);
+//                 HCBle_SendData("#{\"lat\":%.6f,\"lon\":%.6f,\"angle\":%.2f}\n", 
+//                                  current_location.lat, current_location.lon, current_location.angle);
+void ble_tx_task_entry(ULONG thread_input) {
+	BleMessage msg;
+    while(1) {
+
+//        HCBle_SendData("#{\"lat\":%.6f,\"lon\":%.6f,\"angle\":%.2f}\n",23.123456, 113.654321, 95.0);
+//        tx_thread_sleep(500);
+		if(tx_queue_receive(&ble_tx_queue,&msg,TX_WAIT_FOREVER) == TX_SUCCESS)
+		{
+			HCBle_SendData("#{\"lat\":%.6f,\"lon\":%.6f,\"angle\":%.1f}\n", 
+                              msg.lat, msg.lon,msg.angle);
+			
+		}
+		tx_thread_sleep(100);
+    }
+}
+
+
+
+
+
--- a/fun/HCBle.h
+++ b/fun/HCBle.h
@@ -5,7 +5,8 @@
 //#define DEBUG_EN   		1   //<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1> ʹ<><CAB9>DMA <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʹ<EFBFBD><CAB9> <20><><EFBFBD><EFBFBD>ʽ
 #define RX_DataSize		128
 #define RING_BUFFER_SIZE	256	
-#define UART_DMA_RX_BUF_SIZE  64
+#define UART_DMA_RX_BUF_SIZE  20
+

 // HCBle <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݶ<EFBFBD><DDB6><EFBFBD>
 typedef struct
@@ -29,12 +30,26 @@ typedef struct
 }RingBuffer;


-
+typedef struct
+{
+	float lat;
+	float lon;
+	float angle;	
+}BleMessage;

 extern uint8_t  rx_data;  
 extern RingBuffer ble_rx_ring; //<2F><>ʼ<EFBFBD><CABC>
 extern uint8_t uart_dma_rx_buf[UART_DMA_RX_BUF_SIZE];
+extern BleMessage current_location;

+extern uint8_t flag;
+
+void HCBle_SendData(char *p,...);
 void HCBle_InitDMAReception(void);
 void HCBle_ExtractAndParseFrame(void);
+
+void ble_rx_task_entry(ULONG thread_input);
+void ble_tx_task_entry(ULONG thread_input);
+void HCBle_InitEventFlags(void);
+
 #endif
--- a/fun/IMU.c
+++ b/fun/IMU.c
--- a/fun/IMU.h
+++ b/fun/IMU.h
@@ -0,0 +1,258 @@
+/******************************************************************************
+                        <20>豸<EFBFBD><E8B1B8>IM948ģ<38><C4A3>֮<EFBFBD><D6AE><EFBFBD>Ĵ<EFBFBD><C4B4><EFBFBD>ͨ<EFBFBD>ſ<EFBFBD>
+<EFBFBD>汾: V1.05
+<EFBFBD><EFBFBD>¼: 1<><31><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD>ټƺ<D9BC><C6BA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>̿<EFBFBD><CCBF><EFBFBD><EFBFBD><EFBFBD>
+      2<><32><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20>ų<EFBFBD>У׼<D0A3><D7BC>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD>
+      3<><33><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Զ<EFBFBD>У<EFBFBD><D0A3><EFBFBD><EFBFBD>ʶ<EFBFBD><CAB6><EFBFBD><EFBFBD>
+      4<><34><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD>þ<EFBFBD>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD>ģʽ<C4A3>Ĵ<EFBFBD><C4B4><EFBFBD>ʱ<EFBFBD><CAB1>
+      5<><35><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD>ϴ<EFBFBD>Ȧ<EFBFBD><C8A6><EFBFBD><EFBFBD>֧<EFBFBD><D6A7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>͸<EFBFBD><CDB8>
+      6<><36><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD>z<EFBFBD>Ƕ<EFBFBD>Ϊָ<CEAA><D6B8>ֵ
+*******************************************************************************/
+#ifndef __IMU_H__
+#define __IMU_H__
+
+#include "headfile.h"
+
+typedef signed char            S8;
+typedef unsigned char          U8;
+typedef signed short           S16;
+typedef unsigned short         U16;
+typedef signed long            S32;
+typedef unsigned long          U32;
+typedef float                  F32;
+#define pow2(x) ((x)*(x)) // <20><>ƽ<EFBFBD><C6BD>
+
+// <20><><EFBFBD><EFBFBD>ʱת<CAB1><D7AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD>--------------
+#define scaleAccel       0.00478515625f // <20><><EFBFBD>ٶ<EFBFBD> [-16g~+16g]    9.8*16/32768
+#define scaleQuat        0.000030517578125f // <20><>Ԫ<EFBFBD><D4AA> [-1~+1]         1/32768
+#define scaleAngle       0.0054931640625f // <20>Ƕ<EFBFBD>   [-180~+180]     180/32768
+#define scaleAngleSpeed  0.06103515625f // <20><><EFBFBD>ٶ<EFBFBD> [-2000~+2000]    2000/32768
+#define scaleMag         0.15106201171875f // <20>ų<EFBFBD> [-4950~+4950]   4950/32768
+#define scaleTemperature 0.01f // <20>¶<EFBFBD>
+#define scaleAirPressure 0.0002384185791f // <20><>ѹ [-2000~+2000]    2000/8388608
+#define scaleHeight      0.0010728836f    // <20>߶<EFBFBD> [-9000~+9000]    9000/8388608
+
+#define CmdPacket_Begin  0x49   // <20><>ʼ<EFBFBD><CABC>
+#define CmdPacket_End    0x4D   // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+#define CmdPacketMaxDatSizeRx 73  // ģ<>鷢<EFBFBD><E9B7A2><EFBFBD><EFBFBD>   <20><><EFBFBD>ݰ<EFBFBD><DDB0><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>󳤶<EFBFBD>
+#define CmdPacketMaxDatSizeTx 31  // <20><><EFBFBD>͸<EFBFBD>ģ<EFBFBD><C4A3><EFBFBD><EFBFBD> <20><><EFBFBD>ݰ<EFBFBD><DDB0><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>󳤶<EFBFBD>
+
+#define FifoSize 200   // <20><><EFBFBD><EFBFBD>3<EFBFBD><33>fifo<66><6F>С <20>ֽ<EFBFBD><D6BD><EFBFBD>
+typedef struct // <20><><EFBFBD><EFBFBD> Fifo<66><6F><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+{
+    U8 RxBuf[FifoSize];
+    volatile U8 In;
+    volatile U8 Out;
+    volatile U8 Cnt;
+}struct_UartFifo;
+extern struct_UartFifo UartFifo;
+// <20><><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD><D0B6><EFBFBD><EFBFBD>յ<EFBFBD><D5B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ȼ<EFBFBD><C8BB>浽fifo<66><6F>, Ȼ<><C8BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ѭ<EFBFBD><D1AD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>д<EFBFBD><D0B4><EFBFBD>
+#define Fifo_in(RxByte) if (FifoSize > UartFifo.Cnt)\
+                        {\
+                            UartFifo.RxBuf[UartFifo.In] = (RxByte);\
+                            if(++UartFifo.In >= FifoSize)\
+                            {\
+                                UartFifo.In = 0;\
+                            }\
+                            ++UartFifo.Cnt;\
+                        }
+
+
+// ===============================<3D><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϣ<EFBFBD><CFA2><EFBFBD><EFBFBD>====================================
+//    #define __Debug  // ʹ<>õ<EFBFBD><C3B5>Կ<EFBFBD><D4BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϣ,<2C><>ʹ<EFBFBD>õ<EFBFBD><C3B5><EFBFBD><EFBFBD><EFBFBD>Ϣ<EFBFBD><CFA2><EFBFBD>α<EFBFBD><CEB1>伴<EFBFBD><E4BCB4>
+    #ifdef __Debug
+        #define Dbp(fmt, args...)  printf(fmt, ##args) // <20><><EFBFBD><EFBFBD>Ҫʹ<D2AA>õ<EFBFBD><C3B5><EFBFBD><EFBFBD><EFBFBD>Ϣ, <20>û<EFBFBD><C3BB>Խ<EFBFBD>Dbp<62><70><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+        extern void Dbp_U8_buf(char *sBeginInfo, char *sEndInfo, char *sFormat, const U8 *Buf, U32 Len);
+    #else
+        #define Dbp(fmt, args...)
+        #define Dbp_U8_buf(sBeginInfo, sEndInfo, sFormat, Buf, Len)
+    #endif
+
+
+// =================================<3D><>ֲ<EFBFBD>ӿ<EFBFBD>======================================
+    /**
+     * <20><><EFBFBD>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݰ<EFBFBD>, <20>û<EFBFBD>ֻ<EFBFBD><D6BB><EFBFBD>ѽ<EFBFBD><D1BD>յ<EFBFBD><D5B5><EFBFBD>ÿ<EFBFBD>ֽ<EFBFBD><D6BD><EFBFBD><EFBFBD>ݴ<EFBFBD><DDB4><EFBFBD><EFBFBD>ú<EFBFBD><C3BA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+     * @param byte <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>յ<EFBFBD><D5B5><EFBFBD>ÿ<EFBFBD>ֽ<EFBFBD><D6BD><EFBFBD><EFBFBD><EFBFBD>
+     * @return U8 1=<3D><><EFBFBD>յ<EFBFBD><D5B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݰ<EFBFBD>, 0δ<30><CEB4>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݰ<EFBFBD>
+     */
+    extern U8 Cmd_GetPkt(U8 byte);
+
+    // <20><><EFBFBD><EFBFBD><EFBFBD>յ<EFBFBD><D5B5><EFBFBD>Ч<EFBFBD><D0A7><EFBFBD>ݰ<EFBFBD><DDB0><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ص<EFBFBD><D8B5><EFBFBD><EFBFBD>뵽 Cmd_RxUnpack(U8 *buf, U8 DLen) <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD><C3BB>ڸú<DAB8><C3BA><EFBFBD><EFBFBD>ﴦ<EFBFBD><EFB4A6><EFBFBD><EFBFBD><EFBFBD>ݼ<EFBFBD><DDBC>ɣ<EFBFBD><C9A3><EFBFBD><EFBFBD><EFBFBD>ŷ<EFBFBD><C5B7><EFBFBD>Ǹ<EFBFBD>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>һ<EFBFBD>е<EFBFBD>ȫ<EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD>
+    extern F32 AngleX,AngleY,AngleZ;// <20><>Cmd_RxUnpack<63>л<EFBFBD>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD>ŷ<EFBFBD><C5B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݸ<EFBFBD><DDB8>µ<EFBFBD>ȫ<EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD>Ա<EFBFBD><D4B1>û<EFBFBD><C3BB>Լ<EFBFBD><D4BC><EFBFBD>ҵ<EFBFBD><D2B5><EFBFBD>߼<EFBFBD>ʹ<EFBFBD><CAB9>, <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ݣ<EFBFBD><DDA3>ɲο<C9B2><CEBF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ӽ<EFBFBD><D3BC><EFBFBD>
+    extern U8 isNewData;// 1=<3D><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>µ<EFBFBD><C2B5><EFBFBD><EFBFBD>ݵ<EFBFBD>ȫ<EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+
+    extern void im948_test(void); // <20><><EFBFBD><EFBFBD>ʾ<EFBFBD><CABE> <20><><EFBFBD><EFBFBD><EFBFBD>Ǽ<EFBFBD><C7BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Դ<EFBFBD><D4B4>ڷ<EFBFBD><DAB7><EFBFBD><EFBFBD>Ĳ<EFBFBD><C4B2><EFBFBD>ָ<EFBFBD>Ȼ<EEA3AC><C8BB><EFBFBD><EFBFBD>ģ<EFBFBD><C4A3><EFBFBD><EFBFBD><EFBFBD>в<EFBFBD><D0B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ѭ<EFBFBD><D1AD><EFBFBD>Ｔ<EFBFBD><EFBCB4>
+
+// ================================ģ<><C4A3><EFBFBD>Ĳ<EFBFBD><C4B2><EFBFBD>ָ<EFBFBD><D6B8>=================================
+    extern U8 targetDeviceAddress; // ͨ<>ŵ<EFBFBD>ַ<EFBFBD><D6B7><EFBFBD><EFBFBD>Ϊ0-254ָ<34><D6B8><EFBFBD><EFBFBD><EFBFBD>豸<EFBFBD><E8B1B8>ַ<EFBFBD><D6B7><EFBFBD><EFBFBD>Ϊ255<35><35><EFBFBD><EFBFBD>ָ<EFBFBD><D6B8><EFBFBD>豸(<28><><EFBFBD>㲥), <20><><EFBFBD><EFBFBD>Ҫʹ<D2AA><CAB9>485<38><35><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʽͨ<CABD><CDA8>ʱͨ<CAB1><CDA8><EFBFBD>ò<EFBFBD><C3B2><EFBFBD>ѡ<EFBFBD><D1A1>Ҫ<EFBFBD><D2AA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>豸<EFBFBD><E8B1B8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ǵ<EFBFBD><C7B4><EFBFBD>1<EFBFBD><31>1ͨ<31><CDA8><EFBFBD><EFBFBD>Ϊ<EFBFBD>㲥<EFBFBD><E3B2A5>ַ255<35><35><EFBFBD><EFBFBD>
+    extern void Cmd_02(void);// ˯<>ߴ<EFBFBD><DFB4><EFBFBD><EFBFBD><EFBFBD>
+    extern void Cmd_03(void);// <20><><EFBFBD>Ѵ<EFBFBD><D1B4><EFBFBD><EFBFBD><EFBFBD>
+    extern void Cmd_18(void);// <20>ر<EFBFBD><D8B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϱ<EFBFBD>
+    extern void Cmd_19(void);// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϱ<EFBFBD>
+    extern void Cmd_11(void);// <20><>ȡ1<C8A1>ζ<EFBFBD><CEB6>ĵĹ<C4B5><C4B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+    extern void Cmd_10(void);// <20><>ȡ<EFBFBD>豸<EFBFBD><E8B1B8><EFBFBD>Ժ<EFBFBD>״̬
+    /**
+     * <20><><EFBFBD><EFBFBD><EFBFBD>豸<EFBFBD><E8B1B8><EFBFBD><EFBFBD>
+     * @param accStill    <20>ߵ<EFBFBD>-<2D><>ֹ״̬<D7B4><CCAC><EFBFBD>ٶȷ<D9B6>ֵ <20><>λdm/s?
+     * @param stillToZero <20>ߵ<EFBFBD>-<2D><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>(<28><>λcm/s) 0:<3A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 255:<3A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+     * @param moveToZero  <20>ߵ<EFBFBD>-<2D><>̬<EFBFBD><CCAC><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>(<28><>λcm/s) 0:<3A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+     * @param isCompassOn 1=<3D><><EFBFBD>ںϴų<CFB4> 0=<3D><><EFBFBD>ںϴų<CFB4>
+     * @param barometerFilter <20><>ѹ<EFBFBD>Ƶ<EFBFBD><C6B5>˲<EFBFBD><CBB2>ȼ<EFBFBD>[ȡֵ0-3],<2C><>ֵԽ<D6B5><D4BD>Խƽ<D4BD>ȵ<EFBFBD>ʵʱ<CAB5><CAB1>Խ<EFBFBD><D4BD>
+     * @param reportHz <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϱ<EFBFBD><CFB1>Ĵ<EFBFBD><C4B4><EFBFBD>֡<EFBFBD><D6A1>[ȡֵ0-250HZ], 0<><30>ʾ0.5HZ
+     * @param gyroFilter    <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD>ϵ<EFBFBD><CFB5>[ȡֵ0-2],<2C><>ֵԽ<D6B5><D4BD>Խƽ<D4BD>ȵ<EFBFBD>ʵʱ<CAB5><CAB1>Խ<EFBFBD><D4BD>
+     * @param accFilter     <20><><EFBFBD>ټ<EFBFBD><D9BC>˲<EFBFBD>ϵ<EFBFBD><CFB5>[ȡֵ0-4],<2C><>ֵԽ<D6B5><D4BD>Խƽ<D4BD>ȵ<EFBFBD>ʵʱ<CAB5><CAB1>Խ<EFBFBD><D4BD>
+     * @param compassFilter <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD>ϵ<EFBFBD><CFB5>[ȡֵ0-9],<2C><>ֵԽ<D6B5><D4BD>Խƽ<D4BD>ȵ<EFBFBD>ʵʱ<CAB5><CAB1>Խ<EFBFBD><D4BD>
+     * @param Cmd_ReportTag <20><><EFBFBD>ܶ<EFBFBD><DCB6>ı<EFBFBD>ʶ
+     */
+    extern void Cmd_12(U8 accStill, U8 stillToZero, U8 moveToZero,  U8 isCompassOn, U8 barometerFilter, U8 reportHz, U8 gyroFilter, U8 accFilter, U8 compassFilter, U16 Cmd_ReportTag);
+    extern void Cmd_13(void);// <20>ߵ<EFBFBD><DFB5><EFBFBD>ά<EFBFBD>ռ<EFBFBD>λ<EFBFBD><CEBB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+    extern void Cmd_16(void);// <20>Ʋ<EFBFBD><C6B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+    extern void Cmd_14(void);// <20>ָ<EFBFBD><D6B8><EFBFBD><EFBFBD><EFBFBD>У׼<D0A3><D7BC><EFBFBD><EFBFBD>
+    extern void Cmd_15(void);// <20><><EFBFBD>浱ǰУ׼<D0A3><D7BC><EFBFBD><EFBFBD>Ϊ<EFBFBD><CEAA><EFBFBD><EFBFBD>У׼<D0A3><D7BC><EFBFBD><EFBFBD>
+    extern void Cmd_07(void);// <20><><EFBFBD>ټƼ<D9BC><C6BC><EFBFBD>У׼ ģ<>龲ֹ<E9BEB2><D6B9>ˮƽ<CBAE><C6BD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD>͸<EFBFBD>ָ<EFBFBD><EFBFBD>յ<EFBFBD><D5B5>ظ<EFBFBD><D8B8><EFBFBD><EFBFBD>ȴ<EFBFBD>5<EFBFBD>뼴<EFBFBD><EBBCB4>
+    /**
+     * <20><><EFBFBD>ټƸ߾<C6B8><DFBE><EFBFBD>У׼
+     * @param flag <20><>ģ<EFBFBD><C4A3>δ<EFBFBD><CEB4><EFBFBD><EFBFBD>У׼״̬ʱ<CCAC><CAB1>
+     *                 ֵ0 <20><>ʾ<EFBFBD><CABE><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼһ<CABC><D2BB>У׼<D0A3><D7BC><EFBFBD>ɼ<EFBFBD>1<EFBFBD><31><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+     *                 ֵ255 <20><>ʾѯ<CABE><D1AF><EFBFBD>豸<EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><EFBFBD>У׼
+     *             <20><>ģ<EFBFBD><C4A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У׼<D0A3><D7BC>:
+     *                 ֵ1 <20><>ʾҪ<CABE>ɼ<EFBFBD><C9BC><EFBFBD>1<EFBFBD><31><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+     *                 ֵ255 <20><>ʾҪ<CABE>ɼ<EFBFBD><C9BC><EFBFBD><EFBFBD><EFBFBD>1<EFBFBD><31><EFBFBD><EFBFBD><EFBFBD>ݲ<EFBFBD><DDB2><EFBFBD><EFBFBD><EFBFBD>
+     */
+    extern void Cmd_17(U8 flag);
+
+    extern void Cmd_32(void);// <20><>ʼ<EFBFBD><CABC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У׼
+    extern void Cmd_04(void);// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У׼
+    extern void Cmd_05(void);// z<><7A><EFBFBD>ǹ<EFBFBD><C7B9><EFBFBD>
+    /**
+     * <20><><EFBFBD><EFBFBD> Z<><5A><EFBFBD>Ƕ<EFBFBD>Ϊָ<CEAA><D6B8>ֵ
+     * @param val Ҫ<><D2AA><EFBFBD>õĽǶ<C4BD>ֵ(<28><>λ0.001<EFBFBD><EFBFBD>)<29>з<EFBFBD><D0B7><EFBFBD>32λ<32><CEBB>
+     */
+    extern void Cmd_28(S32 val);
+    extern void Cmd_06(void);// xyz<79><7A><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD>
+    extern void Cmd_08(void);// <20>ָ<EFBFBD>Ĭ<EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵZ<CFB5><5A>ָ<EFBFBD>򼰻ָ<F2BCB0BB>Ĭ<EFBFBD>ϵ<EFBFBD><CFB5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϵ
+    /**
+     * <20><><EFBFBD><EFBFBD>PCB<43><42>װ<EFBFBD><D7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+     * @param accMatrix <20><><EFBFBD>ټƷ<D9BC><C6B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+     * @param comMatrix <20><><EFBFBD><EFBFBD><EFBFBD>Ʒ<EFBFBD><C6B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+     */
+    extern void Cmd_20(S8 *accMatrix, S8 *comMatrix);
+    extern void Cmd_21(void);// <20><>ȡPCB<43><42>װ<EFBFBD><D7B0><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+    /**
+     * <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㲥<EFBFBD><E3B2A5><EFBFBD><EFBFBD>
+     *
+     * @param bleName <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>(<28><><EFBFBD><EFBFBD>֧<EFBFBD><D6A7>15<31><35><EFBFBD>ַ<EFBFBD><D6B7><EFBFBD><EFBFBD><EFBFBD>,<2C><>֧<EFBFBD><D6A7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>)
+     */
+    extern void Cmd_22(U8 *bleName);
+    extern void Cmd_23(void);// <20><>ȡ<EFBFBD><C8A1><EFBFBD><EFBFBD><EFBFBD>㲥<EFBFBD><E3B2A5><EFBFBD><EFBFBD>
+    /**
+     * <20><><EFBFBD>ùػ<C3B9><D8BB><EFBFBD>ѹ<EFBFBD>ͳ<EFBFBD><CDB3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+     * @param PowerDownVoltageFlag <20>ػ<EFBFBD><D8BB><EFBFBD>ѹѡ<D1B9><D1A1> 0=3.4V(﮵<><EFAEB5><EFBFBD><EFBFBD><EFBFBD>) 1=2.7V(<28><><EFBFBD><EFBFBD><EFBFBD>ɵ<EFBFBD><C9B5><EFBFBD><EFBFBD><EFBFBD>)
+     * @param charge_full_mV  <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֹ<EFBFBD><D6B9>ѹ 0:3962mv 1:4002mv 2:4044mv 3:4086mv 4:4130mv 5:4175mv 6:4222mv 7:4270mv 8:4308mv 9:4349mv 10:4391mv
+     * @param charge_full_mA <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD> 0:2ma 1:5ma 2:7ma 3:10ma 4:15ma 5:20ma 6:25ma 7:30ma
+     * @param charge_mA      <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 0:20ma 1:30ma 2:40ma 3:50ma 4:60ma 5:70ma 6:80ma 7:90ma 8:100ma 9:110ma 10:120ma 11:140ma 12:160ma 13:180ma 14:200ma 15:220ma
+     */
+    extern void Cmd_24(U8 PowerDownVoltageFlag, U8 charge_full_mV, U8 charge_full_mA, U8 charge_mA);
+    extern void Cmd_25(void);// <20><>ȡ <20>ػ<EFBFBD><D8BB><EFBFBD>ѹ<EFBFBD>ͳ<EFBFBD><CDB3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+    extern void Cmd_26(void);// <20>Ͽ<EFBFBD><CFBF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+    /**
+     * <20><><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD><C3BB><EFBFBD>GPIO<49><4F><EFBFBD><EFBFBD>
+     *
+     * @param M 0=<3D><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>, 1=<3D><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>, 2=<3D><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>, 3=<3D><><EFBFBD><EFBFBD>0, 4=<3D><><EFBFBD><EFBFBD>1
+     */
+    extern void Cmd_27(U8 M);
+    extern void Cmd_2A(void);// <20>豸<EFBFBD><E8B1B8><EFBFBD><EFBFBD>
+    extern void Cmd_2B(void);// <20>豸<EFBFBD>ػ<EFBFBD>
+    /**
+     * <20><><EFBFBD><EFBFBD> <20><><EFBFBD>йػ<D0B9>ʱ<EFBFBD><CAB1>
+     *
+     * @param idleToPowerOffTime <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>û<EFBFBD><C3BB>ͨ<EFBFBD><CDA8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڹ㲥<DAB9>У<EFBFBD><D0A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʱ<EFBFBD>ﵽ<EFBFBD><EFB5BD>ô<EFBFBD><C3B4><EFBFBD><EFBFBD>10<31><30><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ػ<EFBFBD>  0=<3D><><EFBFBD>ػ<EFBFBD>
+     */
+    extern void Cmd_2C(U8 idleToPowerOffTime);
+    extern void Cmd_2D(void);// <20><>ȡ <20><><EFBFBD>йػ<D0B9>ʱ<EFBFBD><CAB1>
+    /**
+     * <20><><EFBFBD><EFBFBD> <20><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʽ<EFBFBD><CABD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƺͳ<C6BA><CDB3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><>ʶ
+     *
+     * @param DisableBleSetNameAndCahrge 1=<3D><>ֹͨ<D6B9><CDA8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƽ<EFBFBD><C6BC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 0=<3D><><EFBFBD><EFBFBD>(Ĭ<><C4AC>) <20><><EFBFBD>ܿͻ<DCBF><CDBB>Ĳ<EFBFBD>Ʒ<EFBFBD><C6B7><EFBFBD><EFBFBD><EFBFBD>ñ<EFBFBD><C3B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģ<EFBFBD><C4A3><EFBFBD>Ϊ1<CEAA><31><EFBFBD><EFBFBD>
+     */
+    extern void Cmd_2E(U8 DisableBleSetNameAndCahrge);
+    extern void Cmd_2F(void);// <20><>ȡ <20><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʽ<EFBFBD><CABD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƺͳ<C6BA><CDB3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><>ʶ
+    /**
+     * <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD>ͨ<EFBFBD>ŵ<EFBFBD>ַ
+     *
+     * @param address <20>豸<EFBFBD><E8B1B8>ַֻ<D6B7><D6BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ0-254
+     */
+    extern void Cmd_30(U8 address);
+    extern void Cmd_31(void);// <20><>ȡ <20><><EFBFBD><EFBFBD>ͨ<EFBFBD>ŵ<EFBFBD>ַ
+    /**
+     * <20><><EFBFBD><EFBFBD> <20><><EFBFBD>ټƺ<D9BC><C6BA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+     *
+     * @param AccRange  Ŀ<><C4BF><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD><D9B6><EFBFBD><EFBFBD><EFBFBD> 0=2g 1=4g 2=8g 3=16g
+     * @param GyroRange Ŀ<><C4BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 0=256 1=512 2=1024 3=2048
+     */
+    extern void Cmd_33(U8 AccRange, U8 GyroRange);
+    extern void Cmd_34(void);// <20><>ȡ <20><><EFBFBD>ټƺ<D9BC><C6BA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+    /**
+      * <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Զ<EFBFBD>У<EFBFBD><D0A3><EFBFBD><EFBFBD>ʶ
+     *
+     * @param GyroAutoFlag  1=<3D><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Զ<EFBFBD>У<EFBFBD><D0A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȿ<EFBFBD>  0=<3D><>
+     */
+    extern void Cmd_35(U8 GyroAutoFlag);
+    extern void Cmd_36(void);// <20><>ȡ <20><><EFBFBD>ټƺ<D9BC><C6BA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+    /**
+      * <20><><EFBFBD><EFBFBD> <20><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD>ģʽ<C4A3>Ĵ<EFBFBD><C4B4><EFBFBD>ʱ<EFBFBD><CAB1>
+     *
+     * @param EcoTime_10s <20><>ֵ<EFBFBD><D6B5><EFBFBD><EFBFBD>0<EFBFBD><30><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Զ<EFBFBD><D4B6><EFBFBD><EFBFBD><EFBFBD>ģʽ(<28><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˯<EFBFBD>ߺ<EFBFBD><DFBA><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϱ<EFBFBD><CFB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֹEcoTime_10s<30><73>10<31><30><EFBFBD>Զ<EFBFBD><D4B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˶<EFBFBD><CBB6><EFBFBD><EFBFBD><EFBFBD>ģʽ<C4A3><CABD><EFBFBD><EFBFBD>ͣ<EFBFBD><CDA3><EFBFBD><EFBFBD><EFBFBD>ϱ<EFBFBD>)  0=<3D><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Զ<EFBFBD><D4B6><EFBFBD><EFBFBD><EFBFBD>
+     */
+    extern void Cmd_37(U8 EcoTime_10s);
+    extern void Cmd_38(void);// <20><>ȡ <20><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD>ģʽ<C4A3>Ĵ<EFBFBD><C4B4><EFBFBD>ʱ<EFBFBD><CAB1>
+    /**
+     * <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģʽ
+     *
+     * @param Flag ȡֵ2=<3D><><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ò<EFBFBD><C3B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϱ<EFBFBD><CFB1><EFBFBD>1=<3D><><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ò<EFBFBD><C3B2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϱ<EFBFBD>, 0=<3D><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҵ<EFBFBD><D2B4>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϱ<EFBFBD>
+     */
+    extern void Cmd_40(U8 Flag);
+    // <20><>ȡ <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģʽ
+    extern void Cmd_41(void);
+    /**
+       * <20><><EFBFBD><EFBFBD> <20><>ǰ<EFBFBD>߶<EFBFBD>Ϊָ<CEAA><D6B8>ֵ
+     *
+     * @param val Ҫ<><D2AA><EFBFBD>õĸ߶<C4B8>ֵ <20><>λΪmm
+     */
+    extern void Cmd_42(S32 val);
+    /**
+       * <20><><EFBFBD><EFBFBD> <20>Զ<EFBFBD><D4B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>߶ȱ<DFB6>ʶ
+     *
+     * @param OnOff 0=<3D>ر<EFBFBD> 1=<3D><><EFBFBD><EFBFBD>
+     */
+    extern void Cmd_43(U8 OnOff);
+    // <20><>ȡ <20>Զ<EFBFBD><D4B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>߶ȱ<DFB6>ʶ
+    extern void Cmd_44(void);
+    /**
+       * <20><><EFBFBD><EFBFBD> <20><><EFBFBD>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD>
+     *
+     * @param BaudRate Ŀ<>겨<EFBFBD><EAB2A8><EFBFBD><EFBFBD> 0=9600 1=115200 2=230400 3=460800
+     */
+    extern void Cmd_47(U8 BaudRate);
+    // <20><>ȡ <20><><EFBFBD>ڲ<EFBFBD><DAB2><EFBFBD><EFBFBD><EFBFBD>
+    extern void Cmd_48(void);
+    // <20><>˸<EFBFBD><CBB8><EFBFBD><EFBFBD>ledָʾ<D6B8><CABE>
+    extern void Cmd_49(void);
+    /**
+       * <20><><EFBFBD><EFBFBD>͸<EFBFBD><CDB8>
+     *
+     * @param TxBuf Ҫ͸<D2AA><CDB8><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+     * @param TxLen <20>ֽ<EFBFBD><D6BD><EFBFBD> <20><><EFBFBD><EFBFBD>С<EFBFBD><D0A1>CmdPacketMaxDatSizeTx
+     */
+    extern void Cmd_50(U8 *TxBuf, U8 TxLen);
+    /**
+       * <20><><EFBFBD><EFBFBD> <20>Ƿ<EFBFBD><C7B7>ϴ<EFBFBD><CFB4><EFBFBD>Ȧ<EFBFBD><C8A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+     *
+     * @param isReportCycle 0=<3D><><EFBFBD><EFBFBD>ŷ<EFBFBD><C5B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>, 1=<3D><><EFBFBD><EFBFBD>Ȧ<EFBFBD><C8A6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ŷ<EFBFBD><C5B7><EFBFBD>Ǵ<EFBFBD><C7B4><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ȧ<EFBFBD><C8A6>
+     */
+    extern void Cmd_51(U8 isReportCycle);
+
+
+
+#endif
+
--- a/fun/Motor.c
+++ b/fun/Motor.c
@@ -0,0 +1,131 @@
+#include "Motor.h"
+
+extern TIM_HandleTypeDef htim3;
+extern TIM_HandleTypeDef htim4;
+
+/*******
+PWMA --- TIM4_CHANNEL4
+
+PMWB --- TIM3_CHANNEL4
+
+STBY --- 5V Ĭ<><C4AC>5V <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ʼ<EFBFBD><CABC>
+*******/
+void PWM_GPIO_TIM_Init(void)
+{
+	HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_4);
+	HAL_TIM_PWM_Start(&htim4,TIM_CHANNEL_4);
+}
+
+
+void MotorA_Dir(uint8_t dir)
+{
+	if(0 == dir) // ǰ<><C7B0>
+	{
+		HAL_GPIO_WritePin(AIN1_GPIO_Port,AIN1_Pin,GPIO_PIN_SET);
+		HAL_GPIO_WritePin(AIN2_GPIO_Port,AIN2_Pin,GPIO_PIN_RESET);
+	}
+	else  // <20><>ת
+	{
+		HAL_GPIO_WritePin(AIN1_GPIO_Port,AIN1_Pin,GPIO_PIN_RESET);
+		HAL_GPIO_WritePin(AIN2_GPIO_Port,AIN2_Pin,GPIO_PIN_SET);
+	}
+	
+}
+
+
+void MotorB_Dir(uint8_t dir)
+{
+	if(0 == dir) // ǰ<><C7B0>
+	{
+		HAL_GPIO_WritePin(BIN1_GPIO_Port,BIN1_Pin,GPIO_PIN_SET);
+		HAL_GPIO_WritePin(BIN2_GPIO_Port,BIN2_Pin,GPIO_PIN_RESET);
+	}
+	else  // <20><>ת
+	{
+		HAL_GPIO_WritePin(BIN1_GPIO_Port,BIN1_Pin,GPIO_PIN_RESET);
+		HAL_GPIO_WritePin(BIN2_GPIO_Port,BIN2_Pin,GPIO_PIN_SET);
+	}
+	
+}
+
+// <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD>A<EFBFBD><41><EFBFBD>ٶ<EFBFBD>
+void MotorA_Speed(uint8_t speed)
+{
+	__HAL_TIM_SET_COMPARE(&htim4,TIM_CHANNEL_4,speed);
+}
+
+// <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD>B<EFBFBD><42><EFBFBD>ٶ<EFBFBD>
+void MotorB_Speed(uint8_t speed)
+{
+	__HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_4,speed);
+}
+
+
+// ʾ<><CABE> ͬʱ<CDAC><CAB1><EFBFBD>Ƶ<EFBFBD><C6B5><EFBFBD>A/B
+// <20><>ʾ <20><><EFBFBD><EFBFBD> Dir 0  ---- ǰ<><C7B0> go forward
+//      <20><><EFBFBD><EFBFBD> Dir 1  ---- <20><><EFBFBD><EFBFBD> go forward 
+//void DriveBOTH(int16_t speedA,int16_t speedB)
+//{
+//	// <20><><EFBFBD><EFBFBD> A
+//	if (speedA >= 0) MotorA_Dir(0);
+//	else
+//	{
+//		MotorA_Dir(1);
+//		speedA = -speedA;
+//	}
+//	MotorA_Speed(speedA);
+//	
+//	// <20><><EFBFBD><EFBFBD> B
+//	if (speedB >= 0) MotorB_Dir(0);
+//	else
+//	{
+//		MotorB_Dir(1);
+//		speedB = -speedB;
+//	}
+//	MotorB_Speed(speedB);
+//}
+
+
+// Motor.c   <20><><EFBFBD>Ͻ<EFBFBD><CFBD>д<EFBFBD><D0B4><EFBFBD>
+void DriveBOTH(int16_t speedA, int16_t speedB)
+{
+    // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>A
+    if(speedA == 0) {
+        // <20>ƶ<EFBFBD><C6B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD>͵<EFBFBD>ƽ
+        HAL_GPIO_WritePin(AIN1_GPIO_Port, AIN1_Pin, GPIO_PIN_RESET);
+        HAL_GPIO_WritePin(AIN2_GPIO_Port, AIN2_Pin, GPIO_PIN_RESET);
+    } 
+    else if(speedA > 0) {
+        HAL_GPIO_WritePin(AIN1_GPIO_Port, AIN1_Pin, GPIO_PIN_SET);
+        HAL_GPIO_WritePin(AIN2_GPIO_Port, AIN2_Pin, GPIO_PIN_RESET);
+        speedA = abs(speedA); // ȡ<><C8A1><EFBFBD><EFBFBD>ֵ
+    }
+    else {
+        HAL_GPIO_WritePin(AIN1_GPIO_Port, AIN1_Pin, GPIO_PIN_RESET);
+        HAL_GPIO_WritePin(AIN2_GPIO_Port, AIN2_Pin, GPIO_PIN_SET);
+        speedA = abs(speedA); // ȡ<><C8A1><EFBFBD><EFBFBD>ֵ
+    }
+    
+	
+	if(speedB == 0) {
+        // <20>ƶ<EFBFBD><C6B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ϊ<EFBFBD>͵<EFBFBD>ƽ
+        HAL_GPIO_WritePin(BIN1_GPIO_Port, BIN1_Pin, GPIO_PIN_RESET);
+        HAL_GPIO_WritePin(BIN2_GPIO_Port, BIN2_Pin, GPIO_PIN_RESET);
+    } 
+    else if(speedB > 0) {
+        HAL_GPIO_WritePin(BIN1_GPIO_Port, BIN1_Pin, GPIO_PIN_SET);
+        HAL_GPIO_WritePin(BIN2_GPIO_Port, BIN2_Pin, GPIO_PIN_RESET);
+        speedB = abs(speedB); // ȡ<><C8A1><EFBFBD><EFBFBD>ֵ
+    }
+    else {
+        HAL_GPIO_WritePin(BIN1_GPIO_Port, BIN1_Pin, GPIO_PIN_RESET);
+        HAL_GPIO_WritePin(BIN2_GPIO_Port, BIN2_Pin, GPIO_PIN_SET);
+        speedB = abs(speedB); // ȡ<><C8A1><EFBFBD><EFBFBD>ֵ
+    }
+    // ͬ<><CDAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>B...
+    
+	
+    // <20><><EFBFBD><EFBFBD>PWM
+    __HAL_TIM_SET_COMPARE(&htim4, TIM_CHANNEL_4, speedA);
+    __HAL_TIM_SET_COMPARE(&htim3, TIM_CHANNEL_4, speedB);
+}
--- a/fun/Motor.h
+++ b/fun/Motor.h
@@ -0,0 +1,27 @@
+#ifndef __MOTOR_H__
+#define __MOTOR_H__
+
+#include "headfile.h"
+
+
+void PWM_GPIO_TIM_Init(void);
+
+void MotorA_Dir(uint8_t dir);
+
+
+void MotorB_Dir(uint8_t dir);
+
+// <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD>A<EFBFBD><41><EFBFBD>ٶ<EFBFBD>
+void MotorA_Speed(uint8_t speed);
+
+// <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD>B<EFBFBD><42><EFBFBD>ٶ<EFBFBD>
+void MotorB_Speed(uint8_t speed);
+
+// ʾ<><CABE> ͬʱ<CDAC><CAB1><EFBFBD>Ƶ<EFBFBD><C6B5><EFBFBD>A/B
+// <20><>ʾ <20><><EFBFBD><EFBFBD> Dir 0  ---- ǰ<><C7B0> go forward
+//      <20><><EFBFBD><EFBFBD> Dir 1  ---- <20><><EFBFBD><EFBFBD> go forward 
+void DriveBOTH(int16_t speedA,int16_t speedB);
+
+
+#endif
+
--- a/fun/Shake_Motor.c
+++ b/fun/Shake_Motor.c
@@ -0,0 +1,33 @@
+#include "Shake_Motor.h"
+
+void Shake_Motor_Open(void)
+{
+	HAL_GPIO_WritePin(Shake_Motor_GPIO_Port,Shake_Motor_Pin,GPIO_PIN_SET); // <20>ߵ<EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD>
+}
+
+void Shake_Motor_Close(void)
+{
+	HAL_GPIO_WritePin(Shake_Motor_GPIO_Port,Shake_Motor_Pin,GPIO_PIN_RESET); // <20>͵<EFBFBD>ƽ<EFBFBD>ر<EFBFBD>
+}
+
+// <20><><EFBFBD>ӷ<EFBFBD><D3B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƹ<EFBFBD><C6B9><EFBFBD>
+void Shake_Motor_Left(void)
+{
+    // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģʽ: <20><><EFBFBD><EFBFBD>3<EFBFBD><33>
+    for(int i=0; i<3; i++)
+    {
+        HAL_GPIO_WritePin(Shake_Motor_GPIO_Port, Shake_Motor_Pin, GPIO_PIN_SET);
+        tx_thread_sleep(50);
+        HAL_GPIO_WritePin(Shake_Motor_GPIO_Port, Shake_Motor_Pin, GPIO_PIN_RESET);
+        tx_thread_sleep(100);
+    }
+}
+
+void Shake_Motor_Right(void)
+{
+    // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģʽ: <20><><EFBFBD><EFBFBD>1<EFBFBD><31>
+    HAL_GPIO_WritePin(Shake_Motor_GPIO_Port, Shake_Motor_Pin, GPIO_PIN_SET);
+    tx_thread_sleep(300);
+    HAL_GPIO_WritePin(Shake_Motor_GPIO_Port, Shake_Motor_Pin, GPIO_PIN_RESET);
+}
+
--- a/fun/Shake_Motor.h
+++ b/fun/Shake_Motor.h
@@ -0,0 +1,11 @@
+#ifndef __SHAKE_MOTOR_H
+#define __SHAKE_MOTOR_H	
+
+#include "headfile.h"
+
+void Shake_Motor_Open(void);
+void Shake_Motor_Close(void);
+// <20><><EFBFBD>ӷ<EFBFBD><D3B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ƹ<EFBFBD><C6B9><EFBFBD>
+void Shake_Motor_Left(void);
+void Shake_Motor_Right(void);
+#endif
--- a/fun/Ultrasound.c
+++ b/fun/Ultrasound.c
@@ -0,0 +1,129 @@
+#include "Ultrasound.h"
+TX_THREAD ultrasonic_task_handle;
+TX_EVENT_FLAGS_GROUP ultrasonic_event;
+
+
+extern TIM_HandleTypeDef htim2;
+
+volatile uint32_t ic_val1 = 0; // <20><><EFBFBD><EFBFBD>ֵ1
+volatile uint32_t ic_val2 = 0; // <20><><EFBFBD><EFBFBD>ֵ2
+volatile uint8_t  is_first_capture = 0; // <20>Ƿ<EFBFBD>Ϊ<EFBFBD><CEAA>һ<EFBFBD>β<EFBFBD><CEB2><EFBFBD>
+volatile uint32_t distance_cm = 0;  // <20><><EFBFBD>밴<EFBFBD><EBB0B4> cm<63><6D><EFBFBD><EFBFBD>
+volatile uint8_t obstacle_level = 0; // 0 - <20><><EFBFBD>ϰ<EFBFBD> , 1 = Զ ,2 = <20><> , 3 = <20><>
+/*******
+<EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+Psc: 250 - 1  <20>Դﵽ   1tick = 1us<75><73>Ч<EFBFBD><D0A7>
+*******/
+
+/****
+
+DWT<EFBFBD>ĳ<EFBFBD>ʼ<EFBFBD><EFBFBD>  <20><>ȷ<EFBFBD><C8B7>
+
+<EFBFBD>ŵ<EFBFBD>main.c<>н<EFBFBD><D0BD>г<EFBFBD>ʼ<EFBFBD><CABC>
+****/
+void DWT_Init(void)
+{
+	CoreDebug->DEMCR |= CoreDebug_DEMCR_TRCENA_Msk;
+	DWT->CYCCNT = (uint32_t)0u; //<2F>¼<EFBFBD><C2BC><EFBFBD><EFBFBD><EFBFBD>
+	DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;
+}
+
+
+
+
+/********
+delay_us() <20><><EFBFBD><EFBFBD>ʵ<EFBFBD><CAB5> (ʹ<><CAB9>DWT)  <20><>ȷ<EFBFBD><C8B7>
+*********/
+void delay_us(uint32_t us)
+{
+	uint32_t start = DWT->CYCCNT;
+	uint32_t ticks = us * (SystemCoreClock / 1000000U); // us * 64
+	while((DWT->CYCCNT - start) < ticks);
+}
+
+
+
+
+/******
+Trig <20><><EFBFBD><EFBFBD>
+
+ You only need to supply a short 10uS 
+pulse to the trigger input to start the ranging
+
+<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Բ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>У<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǰһ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڲ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> Trig Ϊ<>ߵ<EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD>ܵ<EFBFBD><DCB5>´<EFBFBD><C2B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģ<EFBFBD><C4A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˼<EFBFBD><CBBC><EFBFBD> HAL_GPIO_WritePin(..., RESET); HAL_Delay(); <20><>һ<EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD>д<EFBFBD><D0B4><EFBFBD><EFBFBD>
+******/
+void HCSR04_Trigger(void)
+{
+	HAL_GPIO_WritePin(HC_Trig_GPIO_Port,HC_Trig_Pin,GPIO_PIN_RESET);
+	delay_us(2);  //<2F><><EFBFBD><EFBFBD>2~5us
+	HAL_GPIO_WritePin(HC_Trig_GPIO_Port,HC_Trig_Pin,GPIO_PIN_SET);
+	delay_us(10); // <20><><EFBFBD>ָߵ<D6B8>ƽ10us 
+	HAL_GPIO_WritePin(HC_Trig_GPIO_Port,HC_Trig_Pin,GPIO_PIN_RESET);
+}
+
+
+#ifdef TEST
+void ultrasonic_task_entry(ULONG thread_input) {
+    HAL_TIM_IC_Start_IT(&htim5, TIM_CHANNEL_1);
+	DWT_Init();
+	
+    while (1) {
+        HCSR04_Trigger();
+
+        ULONG events;
+        if (tx_event_flags_get(&ultrasonic_event, EVENT_ECHO_DONE, TX_OR_CLEAR,
+                               &events, TX_WAIT_FOREVER) == TX_SUCCESS) {
+            if (distance_cm < 30) {
+                obstacle_level = 3; // <20>ܽ<EFBFBD>
+            }else if(distance_cm > 30 && distance_cm < 50)
+			{
+				obstacle_level = 2; // <20><>
+			}else if(distance_cm > 50 && distance_cm < 80)
+			{
+				obstacle_level = 1;
+			}else 
+			{
+				obstacle_level = 0; // <20><><EFBFBD>ϰ<EFBFBD>
+			}
+			
+			if(obstacle_level > 0)
+			{
+				tx_event_flags_set(&response_events,EVENT_OBSTACLE_DETECTED,TX_OR); // <20><><EFBFBD><EFBFBD><EFBFBD>¼<EFBFBD>
+			}
+        }
+
+        tx_thread_sleep(50);  // ÿ<>β<EFBFBD><CEB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 50 ticks  20Hz <20><><EFBFBD><EFBFBD>Ƶ<EFBFBD><C6B5>
+    }
+}
+#endif
+
+
+
+
+
+
+/*******
+Echo <20><><EFBFBD>벶<EFBFBD><EBB2B6><EFBFBD>ص<EFBFBD><D8B5><EFBFBD><EFBFBD><EFBFBD>
+
+******/
+void HAL_TIM_IC_CaptureCallback(TIM_HandleTypeDef *htim) {
+    if (htim->Channel == HAL_TIM_ACTIVE_CHANNEL_1) {
+        if (is_first_capture == 0) {
+            ic_val1 = HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_1);
+            __HAL_TIM_SET_CAPTUREPOLARITY(htim, TIM_CHANNEL_1, TIM_INPUTCHANNELPOLARITY_FALLING);
+            is_first_capture = 1;
+        } else {
+            ic_val2 = HAL_TIM_ReadCapturedValue(htim, TIM_CHANNEL_1);
+            __HAL_TIM_SET_CAPTUREPOLARITY(htim, TIM_CHANNEL_1, TIM_INPUTCHANNELPOLARITY_RISING);
+            is_first_capture = 0;
+
+            uint32_t delta = (ic_val2 > ic_val1) ? (ic_val2 - ic_val1) : (0xFFFF - ic_val1 + ic_val2);
+            distance_cm = delta / 58;
+
+            // ֪ͨ<CDA8><D6AA><EFBFBD><EFBFBD> ȡ<><C8A1>ע<EFBFBD><D7A2><EFBFBD>Ի<EFBFBD><D4BB>Ѳ<EFBFBD><D1B2><EFBFBD><EFBFBD>߳<EFBFBD>
+            tx_event_flags_set(&ultrasonic_event, EVENT_ECHO_DONE, TX_OR);
+        }
+		   HAL_TIM_IC_Start_IT(&htim5,TIM_CHANNEL_1);
+    }
+}
+
--- a/fun/Ultrasound.h
+++ b/fun/Ultrasound.h
@@ -0,0 +1,27 @@
+#ifndef __ULTRASOUND_H
+#define __ULTRASOUND_H	
+
+#include "headfile.h"
+
+#define HCSR_TEST 1
+#define EVENT_ECHO_DONE		0x01
+
+extern volatile uint8_t obstacle_level;
+extern volatile uint32_t distance_cm;
+void DWT_Init(void);
+/********
+delay_us() <20><><EFBFBD><EFBFBD>ʵ<EFBFBD><CAB5> (ʹ<><CAB9>DWT)  <20><>ȷ<EFBFBD><C8B7>
+*********/
+void delay_us(uint32_t us);
+
+/******
+Trig <20><><EFBFBD><EFBFBD>
+
+ You only need to supply a short 10uS 
+pulse to the trigger input to start the ranging
+
+<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Բ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>У<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ǰһ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ڲ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> Trig Ϊ<>ߵ<EFBFBD>ƽ<EFBFBD><C6BD><EFBFBD><EFBFBD><EFBFBD>ܵ<EFBFBD><DCB5>´<EFBFBD><C2B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ģ<EFBFBD><C4A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˼<EFBFBD><CBBC><EFBFBD> HAL_GPIO_WritePin(..., RESET); HAL_Delay(); <20><>һ<EFBFBD>ֱ<EFBFBD><D6B1><EFBFBD>д<EFBFBD><D0B4><EFBFBD><EFBFBD>
+******/
+void HCSR04_Trigger(void);
+void ultrasonic_task_entry(ULONG thread_input);
+#endif
--- a/fun/encoder.c
+++ b/fun/encoder.c
@@ -0,0 +1,135 @@
+#include "encoder.h"
+
+volatile int32_t tick_L = 0,tick_R = 0;
+volatile int16_t rpm_L = 0,rpm_R = 0;
+
+extern TIM_HandleTypeDef htim1;
+extern TIM_HandleTypeDef htim8;
+
+static TX_THREAD enc_thread;
+static UCHAR enc_stack[512]; 
+
+static void enc_thread_entry(ULONG);
+
+UINT Encoder_ThreadCreate(void)
+{
+	UINT status;
+	
+	HAL_TIM_Encoder_Start(&htim1,TIM_CHANNEL_ALL);
+	HAL_TIM_Encoder_Start(&htim8,TIM_CHANNEL_ALL);
+	
+	__HAL_TIM_SET_COUNTER(&htim1,0);
+	__HAL_TIM_SET_COUNTER(&htim8,0);
+	
+    status = tx_thread_create(&enc_thread, "Encoder", enc_thread_entry, 0,
+                     enc_stack, sizeof(enc_stack),
+                     6, 6, TX_NO_TIME_SLICE, TX_AUTO_START);
+					 
+	return status;
+}
+
+
+static void enc_thread_entry(ULONG arg)
+{
+	const float msPerMin = 6000.0f;
+	const float ticksPerRev = 1024.0f; // <20><><EFBFBD><EFBFBD>  * 4
+	uint16_t lastL = 0, lastR = 0;
+	
+    while (1)
+    {
+        uint16_t cntL = __HAL_TIM_GET_COUNTER(&htim1);
+        uint16_t cntR = __HAL_TIM_GET_COUNTER(&htim8);
+
+        int16_t dL = (int16_t)(cntL - lastL);
+        int16_t dR = (int16_t)(cntR - lastR);
+
+        lastL = cntL;
+        lastR = cntR;
+
+        tick_L += dL;
+        tick_R += dR;
+
+        rpm_L = (int16_t)(dL * msPerMin / (ticksPerRev * 10));
+        rpm_R = (int16_t)(dR * msPerMin / (ticksPerRev * 10));
+
+        tx_thread_sleep(10);
+    }
+	
+}
+
+
+//
+
+//ControlThread
+//
+
+static TX_THREAD control_thread;
+static UCHAR control_stack[512];
+
+volatile int16_t target_rpm_L = 0;
+volatile int16_t target_rpm_R = 0;
+
+static void control_thread_entry(ULONG arg);
+
+
+UINT ControlThreadCreate(void)
+{
+	UINT status;
+    status = tx_thread_create(&control_thread, "Control", control_thread_entry, 0,
+                     control_stack, sizeof(control_stack),
+                     7, 7, TX_NO_TIME_SLICE, TX_AUTO_START);
+					 
+	return status;
+}
+
+
+static void control_thread_entry(ULONG arg)
+{
+    static int16_t pwmL = 0, pwmR = 0;
+    static float integralL = 0, integralR = 0;
+    float Kp = 1.0f, Ki = 0.1f;
+
+    while (1)
+    {
+        int16_t errorL = target_rpm_L - rpm_L;
+        int16_t errorR = target_rpm_R - rpm_R;
+
+        integralL += errorL;
+        integralR += errorR;
+		
+		// <20>ڻ<EFBFBD><DABB><EFBFBD><EFBFBD>ۼӺ<DBBC><D3BA><EFBFBD><EFBFBD>ӿ<EFBFBD><D3BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD>߼<EFBFBD>
+		if (errorL == 0) integralL = 0;
+		if (errorR == 0) integralR = 0;
+
+		const float max_integral = 1000.0f;
+        if(integralL > max_integral) integralL = max_integral;
+        if(integralL < -max_integral) integralL = -max_integral;
+		
+		if(integralR > max_integral) integralR = max_integral;
+        if(integralR < -max_integral) integralR = -max_integral;
+        // ͬ<><CDAC><EFBFBD><EFBFBD><EFBFBD><EFBFBD>integralR...
+		
+        pwmL += (int16_t)(Kp * errorL + Ki * integralL);
+        pwmR += (int16_t)(Kp * errorR + Ki * integralR);
+
+        if (pwmL < 0) pwmL = 0;
+        if (pwmL > 255) pwmL = 255;
+        if (pwmR < 0) pwmR = 0;
+        if (pwmR > 255) pwmR = 255;
+
+        DriveBOTH(pwmL, pwmR);
+
+        tx_thread_sleep(10);
+    }
+}
+
+
+
+
+
+int16_t map_speed_to_rpm(int speed)
+{
+    if (abs(speed) < 30) return 0;
+    return (speed - 60) * 1.5;  // <20><><EFBFBD><EFBFBD>
+}
+
--- a/fun/encoder.h
+++ b/fun/encoder.h
@@ -0,0 +1,12 @@
+#ifndef __ENCODER_H__
+#define __ENCODER_H__
+
+#include "headfile.h"
+
+extern volatile int16_t target_rpm_L;
+extern volatile int16_t target_rpm_R;
+int16_t map_speed_to_rpm(int speed);
+UINT ControlThreadCreate(void);
+UINT Encoder_ThreadCreate(void);
+
+#endif 
--- a/fun/gps.c
+++ b/fun/gps.c
@@ -0,0 +1,368 @@
+#include "gps.h"
+
+
+//#define TEST 					1  //<2F><><EFBFBD><EFBFBD>ʹ<EFBFBD><CAB9>,ȥ<><C8A5><EFBFBD><EFBFBD>ʱ<EFBFBD><CAB1><EFBFBD><EFBFBD><EFBFBD>ԽӴ<D4BD><D3B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ĵ<EFBFBD><C4B5><EFBFBD>
+#define parse					1 // <20><><EFBFBD><EFBFBD>ʹ<EFBFBD><CAB9>
+
+
+
+/**
+<EFBFBD><EFBFBD>ʾ: <20><>Ȼ BLE <20>Լ<EFBFBD> GPS<50><53><EFBFBD><EFBFBD>DMA+ UART<52><54><EFBFBD>գ<EFBFBD><D5A3><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ͬ
+BLE --- DMA + IDLE <20>ж<EFBFBD>  ѭ<><D1AD> DMA + <20><><EFBFBD><EFBFBD><EFBFBD>жϴ<D0B6><CFB4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>  UART IDLE  
+GPS --- DMA +<2B>̶<EFBFBD><CCB6><EFBFBD><EFBFBD><EFBFBD> + TC<54>ж<EFBFBD>   <20><><EFBFBD>ù̶<C3B9><CCB6><EFBFBD><EFBFBD><EFBFBD> DMA    DMA<4D><41><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ж<EFBFBD>
+
+**/
+
+
+//<2F>ⲿ<EFBFBD><E2B2BF><EFBFBD><EFBFBD>
+extern UART_HandleTypeDef huart2;
+extern DMA_HandleTypeDef handle_GPDMA1_Channel3;
+
+
+//ȫ<><C8AB><EFBFBD><EFBFBD><EFBFBD>ݱ<EFBFBD><DDB1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+uint8_t GPS_DMA_RX_BUF[GPS_DMA_RX_BUF_LEN];  //<2F><><EFBFBD><EFBFBD>DMA<4D><41><EFBFBD>ջ<EFBFBD><D5BB><EFBFBD>
+_GPSData GPS;
+
+void GPS_Init(void)
+{
+    HAL_UARTEx_ReceiveToIdle_IT(&huart2, GPS_DMA_RX_BUF, GPS_DMA_RX_BUF_LEN);
+//		HAL_UARTEx_ReceiveToIdle_DMA(&huart2,GPS_DMA_RX_BUF,GPS_DMA_RX_BUF_LEN);
+}
+
+#ifdef parse
+// GPS<50><53><EFBFBD>ݽ<EFBFBD><DDBD><EFBFBD>
+//void parseGpsBuffer()
+//{
+//	char *subString;
+//	char *subStringNext;
+//	char i = 0;
+//	
+//	if(GPS.isGetData)
+//	{
+//		GPS.isGetData = 0; // <20>ѱ<EFBFBD>־λ<D6BE><CEBB>Ϊ1
+//		
+//		char usefullBuffer[2] = {0};
+//		for(i = 0; i <= 6; i++)
+//		{
+//			if(i == 0)
+//			{
+//				
+//				subString = strstr(GPS.GPS_Buffer,",");
+//				if(!subString)return;
+//			}
+//			else
+//			{
+//				subString++;
+//				subStringNext = strstr(subString,",");
+//				if(!subStringNext)return;
+//				
+//				switch(i)
+//				{
+//					case 1:
+//						memcpy(GPS.UTCTime,subString,subStringNext - subString);
+//						break;
+//					case 2:
+//						memcpy(usefullBuffer,subString,subStringNext - subString);
+//						break;
+//					case 3:
+//						memcpy(GPS.latitude,subString,subStringNext - subString);
+//						break;
+//					case 4:
+//						memcpy(GPS.N_S,subString,subStringNext - subString);
+//						break;
+//					case 5:
+//						memcpy(GPS.longitude,subString,subStringNext - subString);
+//						break;
+//					case 6:
+//						memcpy(GPS.E_W,subString,subStringNext - subString);
+//						break;
+//					default:break;
+//				}
+//				
+//				subString = subStringNext;
+//			}
+//			
+//		}
+//		GPS.isParseData = 1;
+//		GPS.isUsefull = (usefullBuffer[0] == 'A') ? 1 : 0;
+//	}
+//	
+//}
+
+
+//void parseGpsBuffer(void)
+//{
+//    if (!GPS.isGetData) return;
+//    GPS.isGetData = 0;
+
+//    /* ----------- <20><><EFBFBD><EFBFBD><EFBFBD>б<EFBFBD> ------------ */
+//    const char *p = GPS.GPS_Buffer;
+
+//    /* <20><> RMC ----------------------------------------------------- */
+//    if (!strncmp(p, "$GNRMC", 6) || !strncmp(p, "$GPRMC", 6))
+//    {
+//        char status, lat[16], ns[3], lon[16], ew[3];
+//        if (sscanf(p,
+//                   "$%*[^,],%*[^,],%c,%15[^,],%2[^,],%15[^,],%2[^,],",
+//                   &status, lat, ns, lon, ew) == 5)
+//        {
+//            if (status == 'A')                      /* <20><>λ<EFBFBD><CEBB>Ч */
+//            {
+//                strcpy(GPS.latitude,  lat);
+//                strcpy(GPS.N_S,       ns);
+//                strcpy(GPS.longitude, lon);
+//                strcpy(GPS.E_W,       ew);
+//                GPS.isParseData = 1;
+//                GPS.isUsefull   = 1;
+//            }
+//        }
+//        return;
+//    }
+
+//    /* <20><> GGA ----------------------------------------------------- */
+//    if (!strncmp(p, "$GNGGA", 6) || !strncmp(p, "$GPGGA", 6))
+//    {
+//        int q;  char lat[16], ns[3], lon[16], ew[3];
+//        if (sscanf(p,
+//                   "$%*[^,],%*[^,],%15[^,],%2[^,],%15[^,],%2[^,],%d,",
+//                   lat, ns, lon, ew, &q) == 5)
+//        {
+//            if (q > 0)                              /* q=0 <20>޶<EFBFBD>λ */
+//            {
+//                strcpy(GPS.latitude,  lat);
+//                strcpy(GPS.N_S,       ns);
+//                strcpy(GPS.longitude, lon);
+//                strcpy(GPS.E_W,       ew);
+//                GPS.isParseData = 1;
+//                GPS.isUsefull   = 1;
+//            }
+//        }
+//        return;
+//    }
+
+//    /* <20><> GLL ----------------------------------------------------- */
+//    if (!strncmp(p, "$GNGLL", 6) || !strncmp(p, "$GPGLL", 6))
+//    {
+//        char status, lat[16], ns[3], lon[16], ew[3];
+//        if (sscanf(p,
+//                   "$%*[^,],%15[^,],%2[^,],%15[^,],%2[^,],%*[^,],%c",
+//                   lat, ns, lon, ew, &status) == 5)
+//        {
+//            if (status == 'A')
+//            {
+//                strcpy(GPS.latitude,  lat);
+//                strcpy(GPS.N_S,       ns);
+//                strcpy(GPS.longitude, lon);
+//                strcpy(GPS.E_W,       ew);
+//                GPS.isParseData = 1;
+//                GPS.isUsefull   = 1;
+//            }
+//        }
+//    }
+//}
+
+/* === <20><> <20>л<EFBFBD><D0BB><EFBFBD><EFBFBD><EFBFBD> & <20><><EFBFBD>ͺ<EFBFBD><CDBA><EFBFBD> === */
+static char line_buf[GPS_Buffer_Length];
+static uint16_t line_w = 0;
+
+void GPS_LinePush(uint8_t ch)
+{
+    if (ch == '\n') {                     /* <20><><EFBFBD><EFBFBD><EFBFBD>н<EFBFBD><D0BD><EFBFBD> */
+        line_buf[line_w] = '\0';
+
+
+        /* A. <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> '$' <20><>ͷ<EFBFBD><CDB7>˵<EFBFBD><CBB5><EFBFBD><EFBFBD>ֻ<EFBFBD><D6BB><EFBFBD><EFBFBD>һ<EFBFBD><D2BB><EFBFBD>ĺ<EFBFBD><C4BA><EFBFBD><EFBFBD><EFBFBD> <20><> ֱ<><D6B1>ƴ<EFBFBD><C6B4> */
+        if (line_buf[0] != '$' && GPS.isGetData == 0 && GPS.GPS_Buffer[0] == '$')
+        {
+            strncat(GPS.GPS_Buffer, line_buf, GPS_Buffer_Length - strlen(GPS.GPS_Buffer) - 1);
+        }
+        else        /* B. <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>¾<EFBFBD> */
+        {
+            strncpy(GPS.GPS_Buffer, line_buf, GPS_Buffer_Length);
+        }
+
+        line_w = 0;
+        GPS.isGetData = 1;
+        tx_event_flags_set(&system_events, EVENT_GPS_DATA_READY, TX_OR);
+        return;
+    }
+}
+
+/* === <20><> <20>½<EFBFBD><C2BD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ֻҪ<D6BB><D2AA>γ<EFBFBD><CEB3> === */
+void parseGpsBuffer(void)
+{
+    if (!GPS.isGetData) return;
+    GPS.isGetData = 0;
+    GPS.isParseData = 0;
+    GPS.isUsefull  = 0;
+
+    const char *p = GPS.GPS_Buffer;
+    /* ---- $GxRMC ---- */
+    if (!strncmp(p, "$GNRMC", 6) || !strncmp(p, "$GPRMC", 6)) {
+        char s, lat[16], ns[3], lon[16], ew[3];
+        if (sscanf(p,
+                   "$%*[^,],%*[^,],%c,%15[^,],%2[^,],%15[^,],%2[^,],",
+                   &s, lat, ns, lon, ew) == 5 && s == 'A') {
+            strcpy(GPS.latitude, lat); strcpy(GPS.N_S, ns);
+            strcpy(GPS.longitude, lon); strcpy(GPS.E_W, ew);
+            GPS.isParseData = GPS.isUsefull = 1;
+        }
+        return;
+    }
+    /* ---- $GxGGA ---- */
+    if (!strncmp(p, "$GNGGA", 6) || !strncmp(p, "$GPGGA", 6)) {
+        int q; char lat[16], ns[3], lon[16], ew[3];
+        if (sscanf(p,
+            "$%*[^,],%*[^,],%15[^,],%2[^,],%15[^,],%2[^,],%d,",
+            lat, ns, lon, ew, &q) == 5 && q > 0) {
+            strcpy(GPS.latitude, lat); strcpy(GPS.N_S, ns);
+            strcpy(GPS.longitude, lon); strcpy(GPS.E_W, ew);
+            GPS.isParseData = GPS.isUsefull = 1;
+        }
+        return;
+    }
+    /* ---- $GxGLL ---- */
+    if (!strncmp(p, "$GNGLL", 6) || !strncmp(p, "$GPGLL", 6)) {
+        char s, lat[16], ns[3], lon[16], ew[3];
+        if (sscanf(p,
+            "$%*[^,],%15[^,],%2[^,],%15[^,],%2[^,],%*[^,],%c",
+            lat, ns, lon, ew, &s) == 5 && s == 'A') {
+            strcpy(GPS.latitude, lat); strcpy(GPS.N_S, ns);
+            strcpy(GPS.longitude, lon); strcpy(GPS.E_W, ew);
+            GPS.isParseData = GPS.isUsefull = 1;
+        }
+    }
+}
+
+
+#else // <20><><EFBFBD><EFBFBD><EFBFBD>Ƕ<EFBFBD><C7B6>ڸĽ<DAB8><C4BD><EFBFBD><EFBFBD><EFBFBD>parse <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫ<EFBFBD><D2AA><EFBFBD>в<EFBFBD><D0B2><EFBFBD>
+void parseGpsBuffer()
+{
+    char *fields[7];
+    char *token;
+    int fieldIndex = 0;
+
+    if (GPS.isGetData)
+    {
+        GPS.isGetData = 0; // <20>ѱ<EFBFBD>־λ<D6BE><CEBB>Ϊ0
+
+        token = strtok(GPS.GPS_Buffer, ",");
+        while (token != NULL && fieldIndex < 7)
+        {
+            fields[fieldIndex++] = token;
+            token = strtok(NULL, ",");
+        }
+
+        if (fieldIndex == 7)
+        {
+            memcpy(GPS.UTCTime, fields[1], sizeof(GPS.UTCTime) - 1);
+            GPS.UTCTime[sizeof(GPS.UTCTime) - 1] = '\0';
+
+            memcpy(GPS.latitude, fields[3], sizeof(GPS.latitude) - 1);
+            GPS.latitude[sizeof(GPS.latitude) - 1] = '\0';
+
+            memcpy(GPS.N_S, fields[4], sizeof(GPS.N_S) - 1);
+            GPS.N_S[sizeof(GPS.N_S) - 1] = '\0';
+
+            memcpy(GPS.longitude, fields[5], sizeof(GPS.longitude) - 1);
+            GPS.longitude[sizeof(GPS.longitude) - 1] = '\0';
+
+            memcpy(GPS.E_W, fields[6], sizeof(GPS.E_W) - 1);
+            GPS.E_W[sizeof(GPS.E_W) - 1] = '\0';
+
+            GPS.isParseData = 1;
+            GPS.isUsefull = (fields[2][0] == 'A') ? 1 : 0;
+        }
+    }
+}
+
+
+/*
+<EFBFBD>Ľ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ܽ<EFBFBD>
+ʹ<EFBFBD><EFBFBD> strtok <20><><EFBFBD>зָ<D0B7><D6B8><EFBFBD>
+<EFBFBD><EFBFBD><EFBFBD>Ӽ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>طָ<EFBFBD><EFBFBD>ַ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>洢<EFBFBD>ֶΣ<EFBFBD>
+ʹ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>洢ÿ<EFBFBD><EFBFBD><EFBFBD>ֶε<EFBFBD>ָ<EFBFBD>룬<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ظ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>롣
+<EFBFBD>߽<EFBFBD><EFBFBD><EFBFBD><EFBFBD>飺
+ȷ<EFBFBD><EFBFBD><EFBFBD>ֶ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>㹻<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Խ<EFBFBD>硣
+<EFBFBD>ַ<EFBFBD><EFBFBD><EFBFBD><EFBFBD>ضϣ<EFBFBD>
+ȷ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ƶ<EFBFBD><EFBFBD>ַ<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> null <20><>β<EFBFBD><CEB2><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ǳ<EFBFBD>ڵ<EFBFBD><DAB5>ַ<EFBFBD><D6B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>⡣
+*/
+#endif
+
+// ת<><D7AA><EFBFBD>Ƕ<EFBFBD>
+double Convert_to_degrees(char *data)
+{
+	double temp = atof(data);
+	int deg = (int)(temp / 100);
+	double min = temp - deg * 100;
+	return deg + (min / 60.0);
+}
+
+
+// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+void GPS_Data_CLR(void)
+{
+	memset(GPS_DMA_RX_BUF,0,GPS_DMA_RX_BUF_LEN);
+	
+}
+
+
+//void HAL_UARTEx_RxEventCallback(UART_HandleTypeDef *huart, uint16_t Size)
+//{
+//	if(huart->Instance == USART2)
+//	{
+//		// <20><><EFBFBD><EFBFBD> Size <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>յ<EFBFBD><D5B5><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+//		memcpy(GPS.GPS_Buffer,GPS_DMA_RX_BUF,Size);
+//		GPS.GPS_Buffer[Size] = '\0';
+//		GPS.isGetData = 1;  //<2F><><EFBFBD>ݽ<EFBFBD><DDBD>ձ<EFBFBD>־λ<D6BE><CEBB>Ϊ1
+//		
+//		tx_event_flags_set(&system_events, EVENT_GPS_DATA_READY, TX_OR);
+//		//<2F><><EFBFBD>¿<EFBFBD><C2BF><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+//		HAL_UARTEx_ReceiveToIdle_IT(&huart2,GPS_DMA_RX_BUF,GPS_DMA_RX_BUF_LEN);
+//	}
+//	
+//}
+
+
+// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ҫʹ<D2AA>õ<EFBFBD>ThreadX <20>Ͱ<EFBFBD>gps.h<>е<EFBFBD> TEST <20>궨<EFBFBD><EAB6A8><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+#ifdef  TEST
+
+
+void gps_thread_entry(ULONG thread_input)
+{
+    GPS_Init();
+    static int gps_first_fix_sent = 0;
+	BleMessage msg;
+    while (1)
+    {
+        ULONG events;
+        tx_event_flags_get(&system_events, EVENT_GPS_DATA_READY, TX_OR_CLEAR, &events, TX_WAIT_FOREVER);
+
+        parseGpsBuffer();
+//		HCBle_SendData("[GPS] isGetData=%d, isParseData=%d, isUsefull=%d, Buffer=%s\r\n",
+//		GPS.isGetData, GPS.isParseData, GPS.isUsefull, GPS.GPS_Buffer);
+        if (GPS.isParseData && GPS.isUsefull)
+        {
+            current_location.lat = Convert_to_degrees(GPS.latitude);
+            current_location.lon = Convert_to_degrees(GPS.longitude);
+            tx_event_flags_set(&system_events, EVENT_LOCATION_UPDATED, TX_OR);
+
+			
+//			msg.lat = current_location.lat;
+//			msg.lon = current_location.lon;
+//            snprintf(msg, sizeof(msg), "#{\"lat\":%.6f,\"lon\":%.6f,\"angle\":%.2f}\n",
+//                     current_location.lat,
+//                     current_location.lon,
+//                     current_location.angle);
+			BleMessage msg = current_location;
+			tx_queue_send(&ble_tx_queue, &msg, TX_WAIT_FOREVER);
+
+        }
+    }
+}
+
+
+#endif
+
+
--- a/fun/gps.h
+++ b/fun/gps.h
@@ -0,0 +1,41 @@
+#ifndef __GPS_H
+#define __GPS_H	
+
+#include "headfile.h"
+
+#define USART_REC_LEN			200	
+#define EN_USART2_RX			1  //ʹ<>ܽ<EFBFBD><DCBD><EFBFBD> --- 1 <20><>֮ 0
+
+#define GPS_DMA_RX_BUF_LEN		220 //<2F>ɸ<EFBFBD><C9B8><EFBFBD>NMEA<45><41><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+
+
+// <20><><EFBFBD><EFBFBD>gps<70>ṹ<EFBFBD><E1B9B9>
+#define GPS_Buffer_Length 256
+#define UTCTime_Length 11
+#define latitude_Length 11
+#define N_S_Length 2
+#define longitude_Length 12
+#define E_W_Length 2 
+
+typedef struct GPSData 
+{
+	char GPS_Buffer[GPS_Buffer_Length];
+	char isGetData;		//<2F>Ƿ<EFBFBD><C7B7><EFBFBD>ȡ<EFBFBD><C8A1>GPS<50><53><EFBFBD><EFBFBD>
+	char isParseData;	//<2F>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+	char UTCTime[UTCTime_Length];		//UTCʱ<43><CAB1>
+	char latitude[latitude_Length];		//γ<><CEB3>
+	char N_S[N_S_Length];		//N/S
+	char longitude[longitude_Length];		//<2F><><EFBFBD><EFBFBD>
+	char E_W[E_W_Length];		//E/W
+	char isUsefull;		//<2F><>λ<EFBFBD><CEBB>Ϣ<EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD>Ч
+} _GPSData;
+
+extern _GPSData GPS;
+extern uint8_t GPS_DMA_RX_BUF[GPS_DMA_RX_BUF_LEN];
+double Convert_to_degrees(char *data);
+void parseGpsBuffer();
+void gps_thread_entry(ULONG thread_input);
+void GPS_Init(void);
+void GPS_LinePush(uint8_t ch);
+
+#endif
--- a/fun/headfile.h
+++ b/fun/headfile.h
@@ -1,17 +1,31 @@
 #ifndef __HEADFILE_H
 #define __HEADFILE_H

-#include "app_threadx.h"
 #include "main.h"
 #include "memorymap.h"
 #include "usart.h"
 #include "gpio.h"
 #include "gpdma.h"
+#include "tim.h"
+#include "app_threadx.h"

 #include "stdio.h"
 #include "stdlib.h"
 #include "stdarg.h"
+#include "string.h"
+#include "math.h"

 #include "HCBle.h"
+#include "gps.h"
+#include "Shake_Motor.h"
+#include "Ultrasound.h"
+#include "Buzzer.h"
+#include "Motor.h"
+#include "IMU.h"
+#include "imu948.h"
+#include "encoder.h"
+
+
+#include "value.h" // ȫ<>ֱ<EFBFBD><D6B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>

 #endif
--- a/fun/imu948.c
+++ b/fun/imu948.c
@@ -0,0 +1,130 @@
+#include "imu948.h"
+
+#define DECLINATION_DEG  -3.0
+extern _GPSData gps_data;
+extern TX_QUEUE ble_tx_queue;
+extern uint8_t rx_byte;
+extern TX_EVENT_FLAGS_GROUP sensor_events;
+
+void imu600_init(void)
+{
+	
+	HAL_UART_Receive_IT(&huart3,&rx_byte,1);
+	HAL_Delay(6000);
+	
+	//------- <20><><EFBFBD>Ѵ<EFBFBD><D1B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>úô<C3BA><C3B4><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>Ȼ<EFBFBD><C8BB><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϱ<EFBFBD>-----
+	Cmd_03(); //<2F><><EFBFBD>Ѵ<EFBFBD><D1B4><EFBFBD><EFBFBD><EFBFBD>
+	HAL_Delay(500);
+	    /**
+       * <20><><EFBFBD><EFBFBD><EFBFBD>豸<EFBFBD><E8B1B8><EFBFBD><EFBFBD>
+     * @param accStill    <20>ߵ<EFBFBD>-<2D><>ֹ״̬<D7B4><CCAC><EFBFBD>ٶȷ<D9B6>ֵ <20><>λdm/s?
+     * @param stillToZero <20>ߵ<EFBFBD>-<2D><>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>(<28><>λcm/s) 0:<3A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> 255:<3A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+     * @param moveToZero  <20>ߵ<EFBFBD>-<2D><>̬<EFBFBD><CCAC><EFBFBD><EFBFBD><EFBFBD>ٶ<EFBFBD>(<28><>λcm/s) 0:<3A><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+     * @param isCompassOn 1=<3D>迪<EFBFBD><E8BFAA><EFBFBD>ų<EFBFBD> 0=<3D><><EFBFBD>رմų<D5B4>
+     * @param barometerFilter <20><>ѹ<EFBFBD>Ƶ<EFBFBD><C6B5>˲<EFBFBD><CBB2>ȼ<EFBFBD>[ȡֵ0-3],<2C><>ֵԽ<D6B5><D4BD>Խƽ<D4BD>ȵ<EFBFBD>ʵʱ<CAB5><CAB1>Խ<EFBFBD><D4BD>
+     * @param reportHz <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϱ<EFBFBD><CFB1>Ĵ<EFBFBD><C4B4><EFBFBD>֡<EFBFBD><D6A1>[ȡֵ0-250HZ], 0<><30>ʾ0.5HZ
+     * @param gyroFilter    <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD>ϵ<EFBFBD><CFB5>[ȡֵ0-2],<2C><>ֵԽ<D6B5><D4BD>Խƽ<D4BD>ȵ<EFBFBD>ʵʱ<CAB5><CAB1>Խ<EFBFBD><D4BD>
+     * @param accFilter     <20><><EFBFBD>ټ<EFBFBD><D9BC>˲<EFBFBD>ϵ<EFBFBD><CFB5>[ȡֵ0-4],<2C><>ֵԽ<D6B5><D4BD>Խƽ<D4BD>ȵ<EFBFBD>ʵʱ<CAB5><CAB1>Խ<EFBFBD><D4BD>
+     * @param compassFilter <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>˲<EFBFBD>ϵ<EFBFBD><CFB5>[ȡֵ0-9],<2C><>ֵԽ<D6B5><D4BD>Խƽ<D4BD>ȵ<EFBFBD>ʵʱ<CAB5><CAB1>Խ<EFBFBD><D4BD>
+     * @param Cmd_ReportTag <20><><EFBFBD>ܶ<EFBFBD><DCB6>ı<EFBFBD>ʶ
+     */
+    Cmd_12(5, 255, 0,  0, 3, 2, 2, 4, 9, 0xFFF);// 2 <20><><EFBFBD><EFBFBD><EFBFBD>豸<EFBFBD><E8B1B8><EFBFBD><EFBFBD>(<28><><EFBFBD><EFBFBD>1) ֻ<><D6BB><EFBFBD><EFBFBD>ŷ<EFBFBD><C5B7><EFBFBD><EFBFBD>
+	HAL_Delay(500);
+    Cmd_19();// 3 <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϱ<EFBFBD>   <20><><EFBFBD><EFBFBD><EFBFBD>ҾͲ<D2BE><CDB2><EFBFBD><EFBFBD>и<EFBFBD><D0B8><EFBFBD><EFBFBD><EFBFBD>
+}
+
+//void IM948_Init(void)
+//{
+//    HAL_UART_Receive_IT(&huart3, &rx_byte, 1);
+
+//    // <20><>ģ<EFBFBD><C4A3><EFBFBD>ϵ<EFBFBD>
+//    tx_thread_sleep( TX_TIMER_TICKS_PER_SECOND * 5 );
+
+
+//    Cmd_03();  // <20><><EFBFBD><EFBFBD>
+//	tx_thread_sleep( TX_TIMER_TICKS_PER_SECOND / 10 );
+
+//    // ֻ<><D6BB><EFBFBD><EFBFBD>ŷ<EFBFBD><C5B7><EFBFBD><EFBFBD>
+//    Cmd_12(
+//        5, 255, 0,
+//        0, 3, 5,
+//        2, 2, 4,
+//        0x0040 // ֻҪŷ<D2AA><C5B7><EFBFBD><EFBFBD>
+//    );
+//	tx_thread_sleep( TX_TIMER_TICKS_PER_SECOND / 10 );
+
+//    Cmd_19(); // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ϱ<EFBFBD>
+//}
+
+
+extern TX_QUEUE im948_uart_rx_queue;
+/***
+<EFBFBD><EFBFBD><EFBFBD>ļ<EFBFBD> <20><>IMU.c<><63>Ϊ<EFBFBD>ײ<EFBFBD><D7B2><EFBFBD><EFBFBD><EFBFBD> Base
+<EFBFBD><EFBFBD><EFBFBD>ڱ<EFBFBD>дӦ<EFBFBD>ò<EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+***/
+
+void imu_angle_ble_task_entry(ULONG thread_input)
+{
+    ULONG rx_data;
+	static uint8_t filtInit = 0;
+	static float   heading_filt = 0;
+	const  float   alpha = 0.20f;               // 20% һ<>׵<EFBFBD>ͨ
+    // <20><>ʼ<EFBFBD><CABC>ģ<EFBFBD><C4A3>
+//    IM948_Init();
+//			HCBle_SendData("halo");
+    while(1)
+    {
+        // <20><><EFBFBD><EFBFBD><EFBFBD>ȶ<EFBFBD><C8B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>д<EFBFBD><D0B4>ڽ<EFBFBD><DABD><EFBFBD>
+        if (tx_queue_receive(&im948_uart_rx_queue, &rx_data, TX_WAIT_FOREVER) == TX_SUCCESS)
+        {
+            Cmd_GetPkt( (uint8_t)rx_data );
+        }
+
+        // <20><><EFBFBD>Ƿ<EFBFBD><C7B7><EFBFBD><EFBFBD>°<EFBFBD>
+        if (isNewData)
+        {
+            isNewData = 0;
+
+
+            // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>angle<6C><65>ä<EFBFBD>ȳ<EFBFBD><C8B3><EFBFBD><EFBFBD>ж<EFBFBD>
+			        /* <20><><EFBFBD><EFBFBD> <20><> IMU <20>Ĵ<EFBFBD><C4B4><EFBFBD>/<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ȡ<EFBFBD>ں<EFBFBD>ŷ<EFBFBD><C5B7><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD> */
+			float angleRaw = AngleZ;                   /* <20><><EFBFBD>Ƕ<EFBFBD><C7B6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>ű<EFBFBD><C5B1><EFBFBD>׼ */
+
+        /* ת<>汱    <20><>ݸ<EFBFBD><DDB8><EFBFBD><EFBFBD><EFBFBD>ڡ<EFBFBD><DAA1><EFBFBD><EFBFBD>ݴ<EFBFBD>ƫ<EFBFBD><C6AB>Ϊ-3.0<EFBFBD><EFBFBD>*/
+			float heading = angleRaw + DECLINATION_DEG;
+			if (heading > 180)  heading -= 360;
+			if (heading < -180) heading += 360;
+
+        /* <20><><EFBFBD><EFBFBD>һ<EFBFBD><D2BB> IIR */
+			const float alpha = 0.2f;
+			heading_filt += alpha * (heading - heading_filt);
+
+			current_location.angle = heading_filt;
+//			current_location.angle = AngleZ * 0.0054931640625f;
+			if(current_location.lat != 0 && current_location.lon != 0)
+			{
+				BleMessage msg = current_location; // <20>ṹ<EFBFBD><E1B9B9>ֱ<EFBFBD>ӿ<EFBFBD><D3BF><EFBFBD>
+				tx_queue_send(&ble_tx_queue,&msg,TX_NO_WAIT);
+			}
+			tx_thread_sleep(10);
+        }
+    }
+}
+
+
+
+// <20><><EFBFBD><EFBFBD><EFBFBD>úý<C3BA><C3BD>պ<EFBFBD><D5BA><EFBFBD>
+void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart)
+{
+	if(huart->Instance == USART3)
+	{
+//		HCBle_SendData("RX USART3: 0x%02X (%c)\r\n", rx_byte, rx_byte);
+
+//		Fifo_in(rx_byte);
+		ULONG rx_data = rx_byte;
+		tx_queue_send(&im948_uart_rx_queue, &rx_data, TX_NO_WAIT);
+//		tx_event_flags_set(&sensor_events, EVENT_IMU_DATA_READY, TX_OR);
+		//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>жϣ<D0B6><CFA3>Ա<EFBFBD><D4B1><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
+		HAL_UART_Receive_IT(huart,&rx_byte,1);
+	}
+}
--- a/fun/imu948.h
+++ b/fun/imu948.h
@@ -0,0 +1,9 @@
+#ifndef __IMU948_H__
+#define __IMU948_H__
+
+#include "headfile.h"
+
+void imu_angle_ble_task_entry(ULONG thread_input);
+void imu600_init(void);
+
+#endif
--- a/fun/value.h
+++ b/fun/value.h
@@ -0,0 +1,15 @@
+#ifndef __VALUE_H__
+#define __VALUE_H__
+
+//typedef struct
+//{
+//	float lat;
+//	float lon;
+//	float angle;	
+//}BleMessage;
+
+#define EVENT_GPS_DATA_READY			(1U << 0)
+#define EVENT_IMU_DATA_READY			(1U << 1)
+
+
+#endif
--- a/point.jpg
+++ b/point.jpg
Author	SHA1	Message	Date
MADAO	75157b2462	Merge branch 'madao'	2025-07-08 23:32:52 +08:00
MADAO	a9884051f8	Final	2025-07-08 23:30:16 +08:00
MADAO	a635b6d5eb	add Encoder and PID control and Abstacle control	2025-07-03 00:29:57 +08:00
MADAO	eb643e686f	Merge branch 'madao'	2025-07-02 00:08:05 +08:00
MADAO	0361cd17af	imu and gps allright	2025-07-02 00:06:16 +08:00
MADAO	a6ebf4b8a5	gps parse successful	2025-07-01 22:12:49 +08:00
MADAO	69d790d5e2	gps could receive but not parese	2025-07-01 21:40:02 +08:00
MADAO	4d36d07b2c	IMU Angle Z get Successful	2025-07-01 13:16:48 +08:00
MADAO	1dd27dce37	Test GPS;it can achieve function what i wannna	2025-06-29 19:29:35 +08:00
MADAO	32d4821e6f	add IMU,slove promblem about Ble_tx	2025-06-29 14:06:17 +08:00
MADAO	88103d9eee	add Motor function	2025-06-28 22:33:16 +08:00
MADAO	8b51db6587	Merge branch 'madao'	2025-06-28 20:35:14 +08:00
MADAO	9ca28fd516	Ble receive Successful	2025-06-28 20:33:31 +08:00
MADAO	03dba7bcc3	替换\n结尾,replace with $	2025-06-28 16:09:05 +08:00
MADAO	10be34ec09	加入了消息队列，线程是可以正常运行的,rx接收任务一直运行不成功，暂时放弃	2025-06-26 22:54:24 +08:00
MADAO	36ddb0a136	tx task success,rx dont	2025-06-26 15:09:26 +08:00
MADAO	59f65128b0	BLE transmit and receive success	2025-06-26 01:41:18 +08:00
MADAO	331d7dedb7	Ble Transmit Success	2025-06-25 21:34:21 +08:00
MADAO	47d3432695	Merge branch 'madao'	2025-06-25 16:59:31 +08:00
MADAO	18c43dbb18	add point.jpg	2025-06-25 16:58:26 +08:00
MADAO	7092525208	change	2025-06-25 16:56:18 +08:00
MADAO	3be581a78b	引脚说明	2025-06-25 16:51:26 +08:00
MADAO	9b471b16da	add new jpg	2025-06-25 16:40:01 +08:00
MADAO	ab6a22962b	add new things in Readme	2025-06-25 14:38:48 +08:00
MADAO	957d1df05f	Add ble,gps,imu and others in threadx,need test	2025-06-25 14:25:28 +08:00
MADAO	64f668fc5f	Merge branch 'madao' of https://gitea.245353.xyz/MADAO/ManGoWalk_STM32 into madao # Please enter a commit message to explain why this merge is necessary,	2025-06-24 21:08:18 +08:00
MADAO	985882d85e	small change	2025-06-24 21:06:53 +08:00
MADAO	74c16a8bd9	Merge pull request 'WIP 加入Buzzer,Shake_Motor,Ultrasound' (#1 ) from develop into madao Reviewed-on: #1	2025-06-24 20:56:30 +08:00
MADAO	20d90d8933	加入Buzzer,Shake_Motor,Ultrasound	2025-06-24 20:46:30 +08:00
MADAO	3d456a334c	更新readme	2025-06-11 10:48:42 +08:00
MADAO	5cc35cbad3	Merge branch 'madao-temp' into madao	2025-06-10 15:21:50 +08:00
MADAO	346aa5c324	新加入gps数据解析,使用GDMA1---CHANNEL3	2025-06-10 15:04:02 +08:00
MADAO	f48b55fe1d	build(AutoGuideStick): 重新编译项目并更新日志文件 - 在 2025 年 6 月 9 日进行了重新编译 - 更新了 keil-assistant.log、uv4.log、uv4.log.lock 和 AutoGuideStick.build_log.htm 文件 - 新增了多个源文件的编译记录，包括 app_azure_rtos.c、gpdma.c、tx_initialize_high_level.c 等 - 程序大小增加：Code=54718 RO-data=726 RW-data=16 ZI-data=5296 - 构建时间延长至 00:00:24	2025-06-09 21:52:49 +08:00
MADAO	ac737fcd69	Merge branch 'main' of https://gitea.245353.xyz/MADAO/ManGoWalk_STM32 into madao	2025-06-09 21:30:14 +08:00
MADAO	5fc481ed9b	Merge branch 'main' of https://gitea.245353.xyz/MADAO/ManGoWalk_STM32 into madao	2025-06-09 11:16:51 +08:00
MADAO	3fd92a9b8a	Merge branch 'main' of https://gitea.245353.xyz/MADAO/ManGoWalk_STM32	2025-06-08 22:48:14 +08:00
MADAO	6095c835b8	更新 README.md	2025-06-04 21:51:33 +08:00
MADAO	1615fff7a4	更新日志	2025-06-04 21:39:53 +08:00