添加ET16s遥控器

Core/Inc/main.h

@@ -62,6 +62,8 @@ void Error_Handler(void);
 #define CMPS_INT_Pin GPIO_PIN_3
 #define CMPS_INT_GPIO_Port GPIOG
 #define CMPS_INT_EXTI_IRQn EXTI3_IRQn
+#define step_Pin GPIO_PIN_1
+#define step_GPIO_Port GPIOC
 #define USER_KEY_Pin GPIO_PIN_0
 #define USER_KEY_GPIO_Port GPIOA
 #define USER_KEY_EXTI_IRQn EXTI0_IRQn
@@ -73,6 +75,8 @@ void Error_Handler(void);
 #define GYRO_INT_Pin GPIO_PIN_5
 #define GYRO_INT_GPIO_Port GPIOC
 #define GYRO_INT_EXTI_IRQn EXTI9_5_IRQn
+#define Step_direction_Pin GPIO_PIN_1
+#define Step_direction_GPIO_Port GPIOB
 #define GYRO_CS_Pin GPIO_PIN_0
 #define GYRO_CS_GPIO_Port GPIOB
 

Core/Src/freertos.c

@@ -124,7 +124,7 @@ void MX_FREERTOS_Init(void) {
 
   /* USER CODE BEGIN RTOS_THREADS */
   /* add threads, ... */
-   osThreadNew(Task_Init, NULL, &attr_init); // 创建åˆ<EFBFBD>始化任åŠ?
+   osThreadNew(Task_Init, NULL, &attr_init); // 创建初始化任<EFBFBD>?
   /* USER CODE END RTOS_THREADS */
 
   /* USER CODE BEGIN RTOS_EVENTS */

Core/Src/gpio.c

@@ -56,9 +56,15 @@ void MX_GPIO_Init(void)
   /*Configure GPIO pin Output Level */
   HAL_GPIO_WritePin(CMPS_RST_GPIO_Port, CMPS_RST_Pin, GPIO_PIN_RESET);
 
+  /*Configure GPIO pin Output Level */
+  HAL_GPIO_WritePin(step_GPIO_Port, step_Pin, GPIO_PIN_RESET);
+
   /*Configure GPIO pin Output Level */
   HAL_GPIO_WritePin(ACCL_CS_GPIO_Port, ACCL_CS_Pin, GPIO_PIN_SET);
 
+  /*Configure GPIO pin Output Level */
+  HAL_GPIO_WritePin(Step_direction_GPIO_Port, Step_direction_Pin, GPIO_PIN_RESET);
+
   /*Configure GPIO pin Output Level */
   HAL_GPIO_WritePin(GYRO_CS_GPIO_Port, GYRO_CS_Pin, GPIO_PIN_SET);
 
@@ -75,6 +81,13 @@ void MX_GPIO_Init(void)
   GPIO_InitStruct.Pull = GPIO_PULLUP;
   HAL_GPIO_Init(CMPS_INT_GPIO_Port, &GPIO_InitStruct);
 
+  /*Configure GPIO pin : PtPin */
+  GPIO_InitStruct.Pin = step_Pin;
+  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
+  GPIO_InitStruct.Pull = GPIO_PULLUP;
+  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+  HAL_GPIO_Init(step_GPIO_Port, &GPIO_InitStruct);
+
   /*Configure GPIO pin : PtPin */
   GPIO_InitStruct.Pin = USER_KEY_Pin;
   GPIO_InitStruct.Mode = GPIO_MODE_IT_FALLING;
@@ -94,6 +107,13 @@ void MX_GPIO_Init(void)
   GPIO_InitStruct.Pull = GPIO_PULLUP;
   HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
 
+  /*Configure GPIO pin : PtPin */
+  GPIO_InitStruct.Pin = Step_direction_Pin;
+  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
+  GPIO_InitStruct.Pull = GPIO_PULLUP;
+  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+  HAL_GPIO_Init(Step_direction_GPIO_Port, &GPIO_InitStruct);
+
   /*Configure GPIO pin : PtPin */
   GPIO_InitStruct.Pin = GYRO_CS_Pin;
   GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;

Core/Src/usart.c

@@ -104,9 +104,9 @@ void MX_USART3_UART_Init(void)
   /* USER CODE END USART3_Init 1 */
   huart3.Instance = USART3;
   huart3.Init.BaudRate = 100000;
-  huart3.Init.WordLength = UART_WORDLENGTH_8B;
+  huart3.Init.WordLength = UART_WORDLENGTH_9B;
   huart3.Init.StopBits = UART_STOPBITS_1;
-  huart3.Init.Parity = UART_PARITY_NONE;
+  huart3.Init.Parity = UART_PARITY_EVEN;
   huart3.Init.Mode = UART_MODE_RX;
   huart3.Init.HwFlowCtl = UART_HWCONTROL_NONE;
   huart3.Init.OverSampling = UART_OVERSAMPLING_16;

MDK-ARM/JLinkSettings.ini

@@ -0,0 +1,45 @@
+[BREAKPOINTS]
+ForceImpTypeAny = 0
+ShowInfoWin = 1
+EnableFlashBP = 2
+BPDuringExecution = 0
+[CFI]
+CFISize = 0x00
+CFIAddr = 0x00
+[CPU]
+MaxNumAPs = 0
+LowPowerHandlingMode = 0
+OverrideMemMap = 0
+AllowSimulation = 1
+ScriptFile=""
+[FLASH]
+RMWThreshold = 0x400
+Loaders=""
+EraseType = 0x00
+CacheExcludeSize = 0x00
+CacheExcludeAddr = 0x00
+MinNumBytesFlashDL = 0
+SkipProgOnCRCMatch = 1
+VerifyDownload = 1
+AllowCaching = 1
+EnableFlashDL = 2
+Override = 0
+Device="ARM7"
+[GENERAL]
+MaxNumTransfers = 0x00
+WorkRAMSize = 0x00
+WorkRAMAddr = 0x00
+RAMUsageLimit = 0x00
+[SWO]
+SWOLogFile=""
+[MEM]
+RdOverrideOrMask = 0x00
+RdOverrideAndMask = 0xFFFFFFFF
+RdOverrideAddr = 0xFFFFFFFF
+WrOverrideOrMask = 0x00
+WrOverrideAndMask = 0xFFFFFFFF
+WrOverrideAddr = 0xFFFFFFFF
+[RAM]
+VerifyDownload = 0x00
+[DYN_MEM_MAP]
+NumUserRegion = 0x00

MDK-ARM/Steering Wheel_Infatry.uvoptx

@@ -117,6 +117,11 @@
         <pMon>BIN\CMSIS_AGDI.dll</pMon>
       </DebugOpt>
       <TargetDriverDllRegistry>
+        <SetRegEntry>
+          <Number>0</Number>
+          <Key>JL2CM3</Key>
+          <Name>-U20760100 -O14 -S2 -ZTIFSpeedSel5000 -A0 -C0 -JU1 -JI127.0.0.1 -JP0 -RST0 -TO18 -TC10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -TB1 -TFE0 -FO7 -FD20000000 -FC1000 -FN1 -FF0STM32F4xx_1024.FLM -FS08000000 -FL0100000 -FP0($$Device:STM32F407IGHx$CMSIS\Flash\STM32F4xx_1024.FLM)</Name>
+        </SetRegEntry>
         <SetRegEntry>
           <Number>0</Number>
           <Key>CMSIS_AGDI</Key>
@@ -158,22 +163,12 @@
         <Ww>
           <count>0</count>
           <WinNumber>1</WinNumber>
-          <ItemText>telescoping_cmd</ItemText>
+          <ItemText>et16s,0x0A</ItemText>
         </Ww>
         <Ww>
           <count>1</count>
           <WinNumber>1</WinNumber>
-          <ItemText>telescoping</ItemText>
+          <ItemText>cmd</ItemText>
-        </Ww>
-        <Ww>
-          <count>2</count>
-          <WinNumber>1</WinNumber>
-          <ItemText>encoder</ItemText>
-        </Ww>
-        <Ww>
-          <count>3</count>
-          <WinNumber>1</WinNumber>
-          <ItemText>rx_data_oid,0x10</ItemText>
         </Ww>
       </WatchWindow1>
       <Tracepoint>
@@ -786,7 +781,7 @@
 
   <Group>
     <GroupName>bsp</GroupName>
-    <tvExp>0</tvExp>
+    <tvExp>1</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
@@ -1078,7 +1073,7 @@
 
   <Group>
     <GroupName>device</GroupName>
-    <tvExp>0</tvExp>
+    <tvExp>1</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
@@ -1161,8 +1156,8 @@
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\User\device\remote_control.c</PathWithFileName>
+      <PathWithFileName>..\User\device\ai.c</PathWithFileName>
-      <FilenameWithoutPath>remote_control.c</FilenameWithoutPath>
+      <FilenameWithoutPath>ai.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
@@ -1173,8 +1168,8 @@
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\User\device\ai.c</PathWithFileName>
+      <PathWithFileName>..\User\device\ET16s.c</PathWithFileName>
-      <FilenameWithoutPath>ai.c</FilenameWithoutPath>
+      <FilenameWithoutPath>ET16s.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
@@ -1202,6 +1197,18 @@
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
+    <File>
+      <GroupNumber>8</GroupNumber>
+      <FileNumber>77</FileNumber>
+      <FileType>1</FileType>
+      <tvExp>0</tvExp>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
+      <PathWithFileName>..\User\device\motor_lk.c</PathWithFileName>
+      <FilenameWithoutPath>motor_lk.c</FilenameWithoutPath>
+      <RteFlg>0</RteFlg>
+      <bShared>0</bShared>
+    </File>
   </Group>
 
   <Group>
@@ -1212,7 +1219,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>77</FileNumber>
+      <FileNumber>78</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1224,7 +1231,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>78</FileNumber>
+      <FileNumber>79</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1236,7 +1243,7 @@
     </File>
     <File>
       <GroupNumber>9</GroupNumber>
-      <FileNumber>79</FileNumber>
+      <FileNumber>80</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1246,18 +1253,6 @@
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
-    <File>
-      <GroupNumber>9</GroupNumber>
-      <FileNumber>80</FileNumber>
-      <FileType>1</FileType>
-      <tvExp>0</tvExp>
-      <tvExpOptDlg>0</tvExpOptDlg>
-      <bDave2>0</bDave2>
-      <PathWithFileName>..\User\module\telecoping_gimbal.c</PathWithFileName>
-      <FilenameWithoutPath>telecoping_gimbal.c</FilenameWithoutPath>
-      <RteFlg>0</RteFlg>
-      <bShared>0</bShared>
-    </File>
     <File>
       <GroupNumber>9</GroupNumber>
       <FileNumber>81</FileNumber>
@@ -1270,6 +1265,18 @@
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
+    <File>
+      <GroupNumber>9</GroupNumber>
+      <FileNumber>82</FileNumber>
+      <FileType>1</FileType>
+      <tvExp>0</tvExp>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
+      <PathWithFileName>..\User\module\step_motor.c</PathWithFileName>
+      <FilenameWithoutPath>step_motor.c</FilenameWithoutPath>
+      <RteFlg>0</RteFlg>
+      <bShared>0</bShared>
+    </File>
   </Group>
 
   <Group>
@@ -1280,7 +1287,7 @@
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>82</FileNumber>
+      <FileNumber>83</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1292,7 +1299,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>83</FileNumber>
+      <FileNumber>84</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1304,7 +1311,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>84</FileNumber>
+      <FileNumber>85</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1316,7 +1323,19 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>85</FileNumber>
+      <FileNumber>86</FileNumber>
+      <FileType>1</FileType>
+      <tvExp>0</tvExp>
+      <tvExpOptDlg>0</tvExpOptDlg>
+      <bDave2>0</bDave2>
+      <PathWithFileName>..\User\task\ET16s.c</PathWithFileName>
+      <FilenameWithoutPath>ET16s.c</FilenameWithoutPath>
+      <RteFlg>0</RteFlg>
+      <bShared>0</bShared>
+    </File>
+    <File>
+      <GroupNumber>10</GroupNumber>
+      <FileNumber>87</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1328,7 +1347,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>86</FileNumber>
+      <FileNumber>88</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1340,7 +1359,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>87</FileNumber>
+      <FileNumber>89</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1352,7 +1371,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>88</FileNumber>
+      <FileNumber>90</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1364,19 +1383,19 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>89</FileNumber>
+      <FileNumber>91</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
       <bDave2>0</bDave2>
-      <PathWithFileName>..\User\task\telecoping.c</PathWithFileName>
+      <PathWithFileName>..\User\task\step_motor.c</PathWithFileName>
-      <FilenameWithoutPath>telecoping.c</FilenameWithoutPath>
+      <FilenameWithoutPath>step_motor.c</FilenameWithoutPath>
       <RteFlg>0</RteFlg>
       <bShared>0</bShared>
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>90</FileNumber>
+      <FileNumber>92</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1388,7 +1407,7 @@
     </File>
     <File>
       <GroupNumber>10</GroupNumber>
-      <FileNumber>91</FileNumber>
+      <FileNumber>93</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1402,13 +1421,13 @@
 
   <Group>
     <GroupName>cmd</GroupName>
-    <tvExp>1</tvExp>
+    <tvExp>0</tvExp>
     <tvExpOptDlg>0</tvExpOptDlg>
     <cbSel>0</cbSel>
     <RteFlg>0</RteFlg>
     <File>
       <GroupNumber>11</GroupNumber>
-      <FileNumber>92</FileNumber>
+      <FileNumber>94</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1420,7 +1439,7 @@
     </File>
     <File>
       <GroupNumber>11</GroupNumber>
-      <FileNumber>93</FileNumber>
+      <FileNumber>95</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1432,7 +1451,7 @@
     </File>
     <File>
       <GroupNumber>11</GroupNumber>
-      <FileNumber>94</FileNumber>
+      <FileNumber>96</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>
@@ -1444,7 +1463,7 @@
     </File>
     <File>
       <GroupNumber>11</GroupNumber>
-      <FileNumber>95</FileNumber>
+      <FileNumber>97</FileNumber>
       <FileType>1</FileType>
       <tvExp>0</tvExp>
       <tvExpOptDlg>0</tvExpOptDlg>

MDK-ARM/Steering Wheel_Infatry.uvprojx

@@ -358,7 +358,7 @@
               <MiscControls></MiscControls>
               <Define></Define>
               <Undefine></Undefine>
-              <IncludePath>../Core/Inc</IncludePath>
+              <IncludePath>../Core/Inc;../Drivers/CMSIS/Include</IncludePath>
             </VariousControls>
           </Aads>
           <LDads>
@@ -1868,16 +1868,16 @@
               <FileType>1</FileType>
               <FilePath>..\User\device\motor_lz.c</FilePath>
             </File>
-            <File>
-              <FileName>remote_control.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\User\device\remote_control.c</FilePath>
-            </File>
             <File>
               <FileName>ai.c</FileName>
               <FileType>1</FileType>
               <FilePath>..\User\device\ai.c</FilePath>
             </File>
+            <File>
+              <FileName>ET16s.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\device\ET16s.c</FilePath>
+            </File>
             <File>
               <FileName>dr16.c</FileName>
               <FileType>1</FileType>
@@ -1888,6 +1888,11 @@
               <FileType>1</FileType>
               <FilePath>..\User\device\Oid.c</FilePath>
             </File>
+            <File>
+              <FileName>motor_lk.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\device\motor_lk.c</FilePath>
+            </File>
           </Files>
         </Group>
         <Group>
@@ -1908,16 +1913,16 @@
               <FileType>1</FileType>
               <FilePath>..\User\module\shoot.c</FilePath>
             </File>
-            <File>
-              <FileName>telecoping_gimbal.c</FileName>
-              <FileType>1</FileType>
-              <FilePath>..\User\module\telecoping_gimbal.c</FilePath>
-            </File>
             <File>
               <FileName>chassis.c</FileName>
               <FileType>1</FileType>
               <FilePath>..\User\module\chassis.c</FilePath>
             </File>
+            <File>
+              <FileName>step_motor.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\module\step_motor.c</FilePath>
+            </File>
           </Files>
         </Group>
         <Group>
@@ -1938,6 +1943,11 @@
               <FileType>1</FileType>
               <FilePath>..\User\task\cmd.c</FilePath>
             </File>
+            <File>
+              <FileName>ET16s.c</FileName>
+              <FileType>1</FileType>
+              <FilePath>..\User\task\ET16s.c</FilePath>
+            </File>
             <File>
               <FileName>dr16.c</FileName>
               <FileType>1</FileType>
@@ -1959,9 +1969,9 @@
               <FilePath>..\User\task\shoot_ctrl.c</FilePath>
             </File>
             <File>
-              <FileName>telecoping.c</FileName>
+              <FileName>step_motor.c</FileName>
               <FileType>1</FileType>
-              <FilePath>..\User\task\telecoping.c</FilePath>
+              <FilePath>..\User\task\step_motor.c</FilePath>
             </File>
             <File>
               <FileName>init.c</FileName>

MDK-ARM/Steering Wheel_Infatry/Steering Wheel_Infatry.build_log.htm

@@ -22,7 +22,7 @@ Dialog DLL:      TCM.DLL V1.48.0.0
  
 <h2>Project:</h2>
 D:\yunha\git_gimbal\RM\Steering Wheel_Infatry\MDK-ARM\Steering Wheel_Infatry.uvprojx
-Project File Date:  01/17/2026
+Project File Date:  01/21/2026
 
 <h2>Output:</h2>
 *** Using Compiler 'V6.16', folder: 'D:\Keil_v5\ARM\ARMCLANG\Bin'
@@ -32,11 +32,13 @@ Note: source file '..\User\bsp\gpio.c' - object file renamed from 'Steering Whee
 Note: source file '..\User\bsp\i2c.c' - object file renamed from 'Steering Wheel_Infatry\i2c.o' to 'Steering Wheel_Infatry\i2c_1.o'.
 Note: source file '..\User\bsp\spi.c' - object file renamed from 'Steering Wheel_Infatry\spi.o' to 'Steering Wheel_Infatry\spi_1.o'.
 Note: source file '..\User\task\ai.c' - object file renamed from 'Steering Wheel_Infatry\ai.o' to 'Steering Wheel_Infatry\ai_1.o'.
+Note: source file '..\User\task\ET16s.c' - object file renamed from 'Steering Wheel_Infatry\ET16s.o' to 'Steering Wheel_Infatry\et16s_1.o'.
 Note: source file '..\User\task\dr16.c' - object file renamed from 'Steering Wheel_Infatry\dr16.o' to 'Steering Wheel_Infatry\dr16_1.o'.
+Note: source file '..\User\task\step_motor.c' - object file renamed from 'Steering Wheel_Infatry\step_motor.o' to 'Steering Wheel_Infatry\step_motor_1.o'.
 Note: source file '..\User\module\cmd\cmd.c' - object file renamed from 'Steering Wheel_Infatry\cmd.o' to 'Steering Wheel_Infatry\cmd_1.o'.
-compiling gimbal_ctrl.c...
+compiling ET16s.c...
 linking...
-Program Size: Code=59816 RO-data=1420 RW-data=1164 ZI-data=118908  
+Program Size: Code=61288 RO-data=1480 RW-data=1084 ZI-data=118980  
 FromELF: creating hex file...
 "Steering Wheel_Infatry\Steering Wheel_Infatry.axf" - 0 Error(s), 0 Warning(s).
 
@@ -62,7 +64,7 @@ Package Vendor: Keil
 
    * Component: ARM::CMSIS:CORE:5.4.0
       Include file:  CMSIS\Core\Include\tz_context.h
-Build Time Elapsed:  00:00:03
+Build Time Elapsed:  00:00:02
 </pre>
 </body>
 </html>

MDK-ARM/Steering Wheel_Infatry/Steering Wheel_Infatry.lnp

@@ -71,23 +71,25 @@
 "steering wheel_infatry\motor.o"
 "steering wheel_infatry\motor_dm.o"
 "steering wheel_infatry\motor_lz.o"
-"steering wheel_infatry\remote_control.o"
 "steering wheel_infatry\ai.o"
+"steering wheel_infatry\et16s.o"
 "steering wheel_infatry\dr16.o"
 "steering wheel_infatry\oid.o"
+"steering wheel_infatry\motor_lk.o"
 "steering wheel_infatry\config.o"
 "steering wheel_infatry\gimbal.o"
 "steering wheel_infatry\shoot.o"
-"steering wheel_infatry\telecoping_gimbal.o"
 "steering wheel_infatry\chassis.o"
+"steering wheel_infatry\step_motor.o"
 "steering wheel_infatry\chassis_ctrl.o"
 "steering wheel_infatry\ai_1.o"
 "steering wheel_infatry\cmd.o"
+"steering wheel_infatry\et16s_1.o"
 "steering wheel_infatry\dr16_1.o"
 "steering wheel_infatry\atti_esti.o"
 "steering wheel_infatry\gimbal_ctrl.o"
 "steering wheel_infatry\shoot_ctrl.o"
-"steering wheel_infatry\telecoping.o"
+"steering wheel_infatry\step_motor_1.o"
 "steering wheel_infatry\init.o"
 "steering wheel_infatry\user_task.o"
 "steering wheel_infatry\cmd_1.o"

MDK-ARM/Steering Wheel_Infatry/chassis_ctrl.d

@@ -62,5 +62,4 @@ steering\ wheel_infatry/chassis_ctrl.o: ..\User\task\chassis_ctrl.c \
   ..\User\module\cmd\cmd.h ..\User\module\cmd\cmd_types.h \
   ..\User\module\cmd\cmd_adapter.h ..\User\device\dr16.h \
   ..\User\device\device.h ..\User\module\cmd\cmd_behavior.h \
-  ..\User\module\gimbal.h ..\User\module\shoot.h \
+  ..\User\module\gimbal.h ..\User\module\shoot.h
-  ..\User\module\telescoping_gimal.h

MDK-ARM/Steering Wheel_Infatry/cmd.d

@@ -12,17 +12,18 @@ steering\ wheel_infatry/cmd.o: ..\User\task\cmd.c \
   ..\Middlewares\Third_Party\FreeRTOS\Source\include\mpu_wrappers.h \
   ..\Middlewares\Third_Party\FreeRTOS\Source\include\task.h \
   ..\Middlewares\Third_Party\FreeRTOS\Source\include\list.h \
-  ..\User\device\dr16.h ..\User\component\user_math.h \
+  ..\User\device\ET16s.h \
+  D:\Keil_v5\ARM\ARMCLANG\Bin\..\include\stdbool.h ..\User\device\dr16.h \
+  ..\User\component\user_math.h \
   D:\Keil_v5\ARM\ARMCLANG\Bin\..\include\float.h \
-  D:\Keil_v5\ARM\ARMCLANG\Bin\..\include\math.h \
+  D:\Keil_v5\ARM\ARMCLANG\Bin\..\include\math.h ..\User\device\device.h \
-  D:\Keil_v5\ARM\ARMCLANG\Bin\..\include\stdbool.h \
+  ..\User\module\config.h ..\User\module\gimbal.h \
-  ..\User\device\device.h ..\User\module\config.h \
+  ..\User\component\ahrs.h ..\User\component\user_math.h \
-  ..\User\module\gimbal.h ..\User\component\ahrs.h \
+  ..\User\component\filter.h ..\User\component\pid.h \
-  ..\User\component\user_math.h ..\User\component\filter.h \
+  ..\User\component\filter.h ..\User\device\motor.h \
-  ..\User\component\pid.h ..\User\component\filter.h \
+  ..\User\device\device.h ..\User\device\motor_dm.h ..\User\bsp\can.h \
-  ..\User\device\motor.h ..\User\device\device.h \
+  ..\Core\Inc\can.h ..\Core\Inc\main.h \
-  ..\User\device\motor_dm.h ..\User\bsp\can.h ..\Core\Inc\can.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal.h \
-  ..\Core\Inc\main.h ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal.h \
   ..\Core\Inc\stm32f4xx_hal_conf.h \
   ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_rcc.h \
   ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_def.h \
@@ -63,5 +64,5 @@ steering\ wheel_infatry/cmd.o: ..\User\task\cmd.c \
   ..\User\module\cmd\cmd.h ..\User\module\cmd\cmd_types.h \
   ..\User\module\cmd\cmd_adapter.h ..\User\module\cmd\cmd_behavior.h \
   ..\User\module\gimbal.h ..\User\module\shoot.h \
-  ..\User\module\telescoping_gimal.h ..\User\module\cmd\cmd_adapter.h \
+  ..\User\module\cmd\cmd_adapter.h ..\User\module\cmd\cmd_behavior.h \
-  ..\User\module\cmd\cmd_behavior.h ..\User\module\cmd\cmd_types.h
+  ..\User\module\cmd\cmd_types.h

MDK-ARM/Steering Wheel_Infatry/config.d

@@ -64,4 +64,4 @@ steering\ wheel_infatry/config.o: ..\User\module\config.c \
   ..\Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.h \
   ..\User\device\device.h ..\User\module\cmd\cmd_behavior.h \
   ..\User\module\gimbal.h ..\User\module\shoot.h \
-  ..\User\module\telescoping_gimal.h ..\User\module\chassis.h
+  ..\User\module\chassis.h

MDK-ARM/Steering Wheel_Infatry/dr16_1.d

@@ -11,4 +11,9 @@ steering\ wheel_infatry/dr16_1.o: ..\User\task\dr16.c \
   ..\Middlewares\Third_Party\FreeRTOS\Source\portable\RVDS\ARM_CM4F\portmacro.h \
   ..\Middlewares\Third_Party\FreeRTOS\Source\include\mpu_wrappers.h \
   ..\Middlewares\Third_Party\FreeRTOS\Source\include\task.h \
-  ..\Middlewares\Third_Party\FreeRTOS\Source\include\list.h
+  ..\Middlewares\Third_Party\FreeRTOS\Source\include\list.h \
+  ..\User\device\dr16.h ..\User\component\user_math.h \
+  D:\Keil_v5\ARM\ARMCLANG\Bin\..\include\float.h \
+  D:\Keil_v5\ARM\ARMCLANG\Bin\..\include\math.h \
+  D:\Keil_v5\ARM\ARMCLANG\Bin\..\include\stdbool.h \
+  ..\User\device\device.h

MDK-ARM/Steering Wheel_Infatry/et16s.d

@@ -0,0 +1,43 @@
+steering\ wheel_infatry/et16s.o: ..\User\device\ET16s.c \
+  ..\User\device\ET16s.h \
+  ..\Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.h \
+  D:\Keil_v5\ARM\ARMCLANG\Bin\..\include\stdint.h \
+  D:\Keil_v5\ARM\ARMCLANG\Bin\..\include\stddef.h \
+  D:\Keil_v5\ARM\ARMCLANG\Bin\..\include\stdbool.h \
+  ..\User\device\device.h \
+  D:\Keil_v5\ARM\ARMCLANG\Bin\..\include\string.h ..\User\bsp\uart.h \
+  ..\Core\Inc\usart.h ..\Core\Inc\main.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal.h \
+  ..\Core\Inc\stm32f4xx_hal_conf.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_rcc.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_def.h \
+  ..\Drivers\CMSIS\Device\ST\STM32F4xx\Include\stm32f4xx.h \
+  ..\Drivers\CMSIS\Device\ST\STM32F4xx\Include\stm32f407xx.h \
+  ..\Drivers\CMSIS\Include\core_cm4.h \
+  ..\Drivers\CMSIS\Include\cmsis_version.h \
+  ..\Drivers\CMSIS\Include\cmsis_compiler.h \
+  ..\Drivers\CMSIS\Include\cmsis_armclang.h \
+  ..\Drivers\CMSIS\Include\mpu_armv7.h \
+  ..\Drivers\CMSIS\Device\ST\STM32F4xx\Include\system_stm32f4xx.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\Legacy\stm32_hal_legacy.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_rcc_ex.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_gpio.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_gpio_ex.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_exti.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_dma.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_dma_ex.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_cortex.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_can.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_flash.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_flash_ex.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_flash_ramfunc.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_i2c.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_i2c_ex.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_pwr.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_pwr_ex.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_spi.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_tim.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_tim_ex.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_uart.h \
+  ..\User\bsp\bsp.h ..\User\component\calc_lib.h \
+  ..\User\module\struct_typedef.h

MDK-ARM/Steering Wheel_Infatry/et16s_1.d

@@ -0,0 +1,16 @@
+steering\ wheel_infatry/et16s_1.o: ..\User\task\ET16s.c \
+  ..\User\task\user_task.h \
+  ..\Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.h \
+  D:\Keil_v5\ARM\ARMCLANG\Bin\..\include\stdint.h \
+  D:\Keil_v5\ARM\ARMCLANG\Bin\..\include\stddef.h \
+  ..\Middlewares\Third_Party\FreeRTOS\Source\include\FreeRTOS.h \
+  ..\Core\Inc\FreeRTOSConfig.h \
+  ..\Middlewares\Third_Party\FreeRTOS\Source\include\projdefs.h \
+  ..\Middlewares\Third_Party\FreeRTOS\Source\include\portable.h \
+  ..\Middlewares\Third_Party\FreeRTOS\Source\include\deprecated_definitions.h \
+  ..\Middlewares\Third_Party\FreeRTOS\Source\portable\RVDS\ARM_CM4F\portmacro.h \
+  ..\Middlewares\Third_Party\FreeRTOS\Source\include\mpu_wrappers.h \
+  ..\Middlewares\Third_Party\FreeRTOS\Source\include\task.h \
+  ..\Middlewares\Third_Party\FreeRTOS\Source\include\list.h \
+  ..\User\device\ET16s.h \
+  D:\Keil_v5\ARM\ARMCLANG\Bin\..\include\stdbool.h

MDK-ARM/Steering Wheel_Infatry/fixation.d

@@ -0,0 +1,2 @@
+steering\ wheel_infatry/fixation.o: ..\User\module\fixation.c \
+  ..\User\module\fixation.h

MDK-ARM/Steering Wheel_Infatry/gimbal_ctrl.d

@@ -64,4 +64,4 @@ steering\ wheel_infatry/gimbal_ctrl.o: ..\User\task\gimbal_ctrl.c \
   ..\User\module\cmd\cmd_types.h ..\User\module\cmd\cmd_adapter.h \
   ..\User\device\dr16.h ..\User\device\device.h \
   ..\User\module\cmd\cmd_behavior.h ..\User\module\gimbal.h \
-  ..\User\module\shoot.h ..\User\module\telescoping_gimal.h
+  ..\User\module\shoot.h

MDK-ARM/Steering Wheel_Infatry/init.d

@@ -59,5 +59,4 @@ steering\ wheel_infatry/init.o: ..\User\task\init.c \
   ..\User\device\motor_rm.h ..\User\device\motor.h \
   ..\User\module\chassis.h ..\User\module\struct_typedef.h \
   ..\User\device\bmi088.h ..\User\component\user_math.h \
-  ..\User\device\dr16.h ..\User\device\device.h \
+  ..\User\device\dr16.h ..\User\device\device.h ..\User\device\et16s.h
-  ..\User\module\telescoping_gimal.h

MDK-ARM/Steering Wheel_Infatry/motor_lk.d

@@ -0,0 +1,54 @@
+steering\ wheel_infatry/motor_lk.o: ..\User\device\motor_lk.c \
+  ..\User\device\motor_lk.h ..\User\device\device.h \
+  D:\Keil_v5\ARM\ARMCLANG\Bin\..\include\stdbool.h \
+  D:\Keil_v5\ARM\ARMCLANG\Bin\..\include\stdint.h ..\User\device\motor.h \
+  ..\User\bsp\can.h ..\Core\Inc\can.h ..\Core\Inc\main.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal.h \
+  ..\Core\Inc\stm32f4xx_hal_conf.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_rcc.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_def.h \
+  ..\Drivers\CMSIS\Device\ST\STM32F4xx\Include\stm32f4xx.h \
+  ..\Drivers\CMSIS\Device\ST\STM32F4xx\Include\stm32f407xx.h \
+  ..\Drivers\CMSIS\Include\core_cm4.h \
+  ..\Drivers\CMSIS\Include\cmsis_version.h \
+  ..\Drivers\CMSIS\Include\cmsis_compiler.h \
+  ..\Drivers\CMSIS\Include\cmsis_armclang.h \
+  ..\Drivers\CMSIS\Include\mpu_armv7.h \
+  ..\Drivers\CMSIS\Device\ST\STM32F4xx\Include\system_stm32f4xx.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\Legacy\stm32_hal_legacy.h \
+  D:\Keil_v5\ARM\ARMCLANG\Bin\..\include\stddef.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_rcc_ex.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_gpio.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_gpio_ex.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_exti.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_dma.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_dma_ex.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_cortex.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_can.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_flash.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_flash_ex.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_flash_ramfunc.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_i2c.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_i2c_ex.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_pwr.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_pwr_ex.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_spi.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_tim.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_tim_ex.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_uart.h \
+  ..\User\bsp\bsp.h ..\User\bsp\mm.h \
+  ..\Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os.h \
+  ..\Middlewares\Third_Party\FreeRTOS\Source\include\FreeRTOS.h \
+  ..\Core\Inc\FreeRTOSConfig.h \
+  ..\Middlewares\Third_Party\FreeRTOS\Source\include\projdefs.h \
+  ..\Middlewares\Third_Party\FreeRTOS\Source\include\portable.h \
+  ..\Middlewares\Third_Party\FreeRTOS\Source\include\deprecated_definitions.h \
+  ..\Middlewares\Third_Party\FreeRTOS\Source\portable\RVDS\ARM_CM4F\portmacro.h \
+  ..\Middlewares\Third_Party\FreeRTOS\Source\include\mpu_wrappers.h \
+  ..\Middlewares\Third_Party\FreeRTOS\Source\include\task.h \
+  ..\Middlewares\Third_Party\FreeRTOS\Source\include\list.h \
+  ..\Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.h \
+  D:\Keil_v5\ARM\ARMCLANG\Bin\..\include\string.h ..\User\bsp\time.h \
+  ..\User\component\user_math.h \
+  D:\Keil_v5\ARM\ARMCLANG\Bin\..\include\float.h \
+  D:\Keil_v5\ARM\ARMCLANG\Bin\..\include\math.h

MDK-ARM/Steering Wheel_Infatry/shoot_ctrl.d

@@ -65,4 +65,4 @@ steering\ wheel_infatry/shoot_ctrl.o: ..\User\task\shoot_ctrl.c \
   ..\User\module\cmd\cmd_types.h ..\User\module\cmd\cmd_adapter.h \
   ..\User\device\dr16.h ..\User\device\device.h \
   ..\User\module\cmd\cmd_behavior.h ..\User\module\gimbal.h \
-  ..\User\module\shoot.h ..\User\module\telescoping_gimal.h
+  ..\User\module\shoot.h

MDK-ARM/Steering Wheel_Infatry/step_motor.d

@@ -0,0 +1,38 @@
+steering\ wheel_infatry/step_motor.o: ..\User\module\step_motor.c \
+  ..\Core\Inc\main.h ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal.h \
+  ..\Core\Inc\stm32f4xx_hal_conf.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_rcc.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_def.h \
+  ..\Drivers\CMSIS\Device\ST\STM32F4xx\Include\stm32f4xx.h \
+  ..\Drivers\CMSIS\Device\ST\STM32F4xx\Include\stm32f407xx.h \
+  ..\Drivers\CMSIS\Include\core_cm4.h \
+  D:\Keil_v5\ARM\ARMCLANG\Bin\..\include\stdint.h \
+  ..\Drivers\CMSIS\Include\cmsis_version.h \
+  ..\Drivers\CMSIS\Include\cmsis_compiler.h \
+  ..\Drivers\CMSIS\Include\cmsis_armclang.h \
+  ..\Drivers\CMSIS\Include\mpu_armv7.h \
+  ..\Drivers\CMSIS\Device\ST\STM32F4xx\Include\system_stm32f4xx.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\Legacy\stm32_hal_legacy.h \
+  D:\Keil_v5\ARM\ARMCLANG\Bin\..\include\stddef.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_rcc_ex.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_gpio.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_gpio_ex.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_exti.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_dma.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_dma_ex.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_cortex.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_can.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_flash.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_flash_ex.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_flash_ramfunc.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_i2c.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_i2c_ex.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_pwr.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_pwr_ex.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_spi.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_tim.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_tim_ex.h \
+  ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_uart.h \
+  ..\User\module\step_motor.h ..\User\bsp\gpio.h \
+  D:\Keil_v5\ARM\ARMCLANG\Bin\..\include\stdbool.h ..\User\bsp\bsp.h \
+  ..\Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.h

MDK-ARM/Steering Wheel_Infatry/step_motor_1.d

@@ -0,0 +1,14 @@
+steering\ wheel_infatry/step_motor_1.o: ..\User\task\step_motor.c \
+  ..\User\task\user_task.h \
+  ..\Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.h \
+  D:\Keil_v5\ARM\ARMCLANG\Bin\..\include\stdint.h \
+  D:\Keil_v5\ARM\ARMCLANG\Bin\..\include\stddef.h \
+  ..\Middlewares\Third_Party\FreeRTOS\Source\include\FreeRTOS.h \
+  ..\Core\Inc\FreeRTOSConfig.h \
+  ..\Middlewares\Third_Party\FreeRTOS\Source\include\projdefs.h \
+  ..\Middlewares\Third_Party\FreeRTOS\Source\include\portable.h \
+  ..\Middlewares\Third_Party\FreeRTOS\Source\include\deprecated_definitions.h \
+  ..\Middlewares\Third_Party\FreeRTOS\Source\portable\RVDS\ARM_CM4F\portmacro.h \
+  ..\Middlewares\Third_Party\FreeRTOS\Source\include\mpu_wrappers.h \
+  ..\Middlewares\Third_Party\FreeRTOS\Source\include\task.h \
+  ..\Middlewares\Third_Party\FreeRTOS\Source\include\list.h

Steering Wheel_Infatry.ioc

@@ -123,24 +123,26 @@ Mcu.Pin22=PF1
 Mcu.Pin23=PG6
 Mcu.Pin24=PF6
 Mcu.Pin25=PG3
-Mcu.Pin26=PA0-WKUP
+Mcu.Pin26=PC1
-Mcu.Pin27=PA4
+Mcu.Pin27=PA0-WKUP
-Mcu.Pin28=PC4
+Mcu.Pin28=PA4
-Mcu.Pin29=PC5
+Mcu.Pin29=PC4
 Mcu.Pin3=PB3
-Mcu.Pin30=PB12
+Mcu.Pin30=PC5
-Mcu.Pin31=PA7
+Mcu.Pin31=PB12
-Mcu.Pin32=PB0
+Mcu.Pin32=PA7
-Mcu.Pin33=VP_FREERTOS_VS_CMSIS_V2
+Mcu.Pin33=PB1
-Mcu.Pin34=VP_SYS_VS_Systick
+Mcu.Pin34=PB0
-Mcu.Pin35=VP_TIM10_VS_ClockSourceINT
+Mcu.Pin35=VP_FREERTOS_VS_CMSIS_V2
+Mcu.Pin36=VP_SYS_VS_Systick
+Mcu.Pin37=VP_TIM10_VS_ClockSourceINT
 Mcu.Pin4=PA14
 Mcu.Pin5=PA13
 Mcu.Pin6=PB9
 Mcu.Pin7=PB7
 Mcu.Pin8=PB6
 Mcu.Pin9=PD6
-Mcu.PinsNb=36
+Mcu.PinsNb=38
 Mcu.ThirdPartyNb=0
 Mcu.UserConstants=
 Mcu.UserName=STM32F407IGHx
@@ -207,6 +209,11 @@ PB0.GPIO_Speed=GPIO_SPEED_FREQ_MEDIUM
 PB0.Locked=true
 PB0.PinState=GPIO_PIN_SET
 PB0.Signal=GPIO_Output
+PB1.GPIOParameters=GPIO_PuPd,GPIO_Label
+PB1.GPIO_Label=Step_direction
+PB1.GPIO_PuPd=GPIO_PULLUP
+PB1.Locked=true
+PB1.Signal=GPIO_Output
 PB12.Locked=true
 PB12.Mode=CAN_Activate
 PB12.Signal=CAN2_RX
@@ -222,6 +229,11 @@ PB8.Mode=I2C
 PB8.Signal=I2C1_SCL
 PB9.Mode=I2C
 PB9.Signal=I2C1_SDA
+PC1.GPIOParameters=GPIO_PuPd,GPIO_Label
+PC1.GPIO_Label=step
+PC1.GPIO_PuPd=GPIO_PULLUP
+PC1.Locked=true
+PC1.Signal=GPIO_Output
 PC10.Mode=Asynchronous
 PC10.Signal=USART3_TX
 PC11.Mode=Asynchronous
@@ -242,6 +254,13 @@ PC5.GPIO_ModeDefaultEXTI=GPIO_MODE_IT_FALLING
 PC5.GPIO_PuPd=GPIO_PULLUP
 PC5.Locked=true
 PC5.Signal=GPXTI5
+PCC.Checker=false
+PCC.Line=STM32F407/417
+PCC.MCU=STM32F407I(E-G)Hx
+PCC.PartNumber=STM32F407IGHx
+PCC.Series=STM32F4
+PCC.Temperature=25
+PCC.Vdd=3.3
 PD0.Locked=true
 PD0.Mode=CAN_Activate
 PD0.Signal=CAN1_RX
@@ -281,6 +300,7 @@ PinOutPanel.CurrentBGAView=Top
 PinOutPanel.RotationAngle=0
 ProjectManager.AskForMigrate=true
 ProjectManager.BackupPrevious=false
+ProjectManager.CompilerLinker=GCC
 ProjectManager.CompilerOptimize=6
 ProjectManager.ComputerToolchain=false
 ProjectManager.CoupleFile=true
@@ -290,6 +310,7 @@ ProjectManager.DeletePrevious=true
 ProjectManager.DeviceId=STM32F407IGHx
 ProjectManager.FirmwarePackage=STM32Cube FW_F4 V1.28.3
 ProjectManager.FreePins=false
+ProjectManager.FreePinsContext=
 ProjectManager.HalAssertFull=false
 ProjectManager.HeapSize=0x200
 ProjectManager.KeepUserCode=true
@@ -368,9 +389,11 @@ USART1.VirtualMode=VM_ASYNC
 USART2.IPParameters=VirtualMode
 USART2.VirtualMode=VM_ASYNC
 USART3.BaudRate=100000
-USART3.IPParameters=VirtualMode,BaudRate,Mode
+USART3.IPParameters=VirtualMode,BaudRate,Mode,Parity,WordLength
 USART3.Mode=MODE_RX
+USART3.Parity=PARITY_EVEN
 USART3.VirtualMode=VM_ASYNC
+USART3.WordLength=WORDLENGTH_9B
 USART6.IPParameters=VirtualMode
 USART6.VirtualMode=VM_ASYNC
 VP_FREERTOS_VS_CMSIS_V2.Mode=CMSIS_V2

User/bsp/bsp_config.yaml

@@ -24,6 +24,16 @@ gpio:
     ioc_label: GYRO_CS
     pin: PB0
     type: OUTPUT
+  - custom_name: STEP__DIRECTION
+    has_exti: false
+    ioc_label: Step_direction
+    pin: PB1
+    type: OUTPUT
+  - custom_name: STEP_MOTOR
+    has_exti: false
+    ioc_label: step
+    pin: PC1
+    type: OUTPUT
   - custom_name: ACCL_INT
     has_exti: true
     ioc_label: ACCL_INT

User/bsp/gpio.c

@@ -29,6 +29,8 @@ static const BSP_GPIO_MAP_t GPIO_Map[BSP_GPIO_NUM] = {
     {USER_KEY_Pin, USER_KEY_GPIO_Port},
     {ACCL_CS_Pin, ACCL_CS_GPIO_Port},
     {GYRO_CS_Pin, GYRO_CS_GPIO_Port},
+    {Step_direction_Pin, Step_direction_GPIO_Port},
+    {step_Pin, step_GPIO_Port},
     {ACCL_INT_Pin, ACCL_INT_GPIO_Port},
     {GYRO_INT_Pin, GYRO_INT_GPIO_Port},
     {CMPS_INT_Pin, CMPS_INT_GPIO_Port},

User/bsp/gpio.h

@@ -25,6 +25,8 @@ typedef enum {
   BSP_GPIO_USER_KEY,
   BSP_GPIO_ACCL_CS,
   BSP_GPIO_GYRO_CS,
+  BSP_GPIO_STEP__DIRECTION,
+  BSP_GPIO_STEP_MOTOR,
   BSP_GPIO_ACCL_INT,
   BSP_GPIO_GYRO_INT,
   BSP_GPIO_CMPS_INT,

User/component/ahrs/ahrs.c

@@ -0,0 +1,417 @@
+/*
+  开源的AHRS算法。
+  MadgwickAHRS
+*/
+
+#include "ahrs.h"
+
+#include <string.h>
+
+#include "user_math.h"
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+#define BETA_IMU (0.033f)
+#define BETA_AHRS (0.041f)
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+/* 2 * proportional gain (Kp) */
+static float beta = BETA_IMU;
+
+/**
+ * @brief 不使用磁力计计算姿态
+ *
+ * @param ahrs 姿态解算主结构体
+ * @param accl 加速度计数据
+ * @param gyro 陀螺仪数据
+ * @return int8_t 0对应没有错误
+ */
+static int8_t AHRS_UpdateIMU(AHRS_t *ahrs, const AHRS_Accl_t *accl,
+                             const AHRS_Gyro_t *gyro) {
+  if (ahrs == NULL) return -1;
+  if (accl == NULL) return -1;
+  if (gyro == NULL) return -1;
+
+  beta = BETA_IMU;
+
+  float ax = accl->x;
+  float ay = accl->y;
+  float az = accl->z;
+
+  float gx = gyro->x;
+  float gy = gyro->y;
+  float gz = gyro->z;
+
+  float recip_norm;
+  float s0, s1, s2, s3;
+  float q_dot1, q_dot2, q_dot3, q_dot4;
+  float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2, _8q1, _8q2, q0q0, q1q1, q2q2,
+      q3q3;
+
+  /* Rate of change of quaternion from gyroscope */
+  q_dot1 = 0.5f * (-ahrs->quat.q1 * gx - ahrs->quat.q2 * gy -
+                   ahrs->quat.q3 * gz);
+  q_dot2 = 0.5f * (ahrs->quat.q0 * gx + ahrs->quat.q2 * gz -
+                   ahrs->quat.q3 * gy);
+  q_dot3 = 0.5f * (ahrs->quat.q0 * gy - ahrs->quat.q1 * gz +
+                   ahrs->quat.q3 * gx);
+  q_dot4 = 0.5f * (ahrs->quat.q0 * gz + ahrs->quat.q1 * gy -
+                   ahrs->quat.q2 * gx);
+
+  /* Compute feedback only if accelerometer measurement valid (avoids NaN in
+   * accelerometer normalisation) */
+  if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
+    /* Normalise accelerometer measurement */
+    recip_norm = InvSqrt(ax * ax + ay * ay + az * az);
+    ax *= recip_norm;
+    ay *= recip_norm;
+    az *= recip_norm;
+
+    /* Auxiliary variables to avoid repeated arithmetic */
+    _2q0 = 2.0f * ahrs->quat.q0;
+    _2q1 = 2.0f * ahrs->quat.q1;
+    _2q2 = 2.0f * ahrs->quat.q2;
+    _2q3 = 2.0f * ahrs->quat.q3;
+    _4q0 = 4.0f * ahrs->quat.q0;
+    _4q1 = 4.0f * ahrs->quat.q1;
+    _4q2 = 4.0f * ahrs->quat.q2;
+    _8q1 = 8.0f * ahrs->quat.q1;
+    _8q2 = 8.0f * ahrs->quat.q2;
+    q0q0 = ahrs->quat.q0 * ahrs->quat.q0;
+    q1q1 = ahrs->quat.q1 * ahrs->quat.q1;
+    q2q2 = ahrs->quat.q2 * ahrs->quat.q2;
+    q3q3 = ahrs->quat.q3 * ahrs->quat.q3;
+
+    /* Gradient decent algorithm corrective step */
+    s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
+    s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * ahrs->quat.q1 -
+         _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
+    s2 = 4.0f * q0q0 * ahrs->quat.q2 + _2q0 * ax + _4q2 * q3q3 -
+         _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
+    s3 = 4.0f * q1q1 * ahrs->quat.q3 - _2q1 * ax +
+         4.0f * q2q2 * ahrs->quat.q3 - _2q2 * ay;
+
+    /* normalise step magnitude */
+    recip_norm = InvSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3);
+
+    s0 *= recip_norm;
+    s1 *= recip_norm;
+    s2 *= recip_norm;
+    s3 *= recip_norm;
+
+    /* Apply feedback step */
+    q_dot1 -= beta * s0;
+    q_dot2 -= beta * s1;
+    q_dot3 -= beta * s2;
+    q_dot4 -= beta * s3;
+  }
+
+  /* Integrate rate of change of quaternion to yield quaternion */
+  ahrs->quat.q0 += q_dot1 * ahrs->inv_sample_freq;
+  ahrs->quat.q1 += q_dot2 * ahrs->inv_sample_freq;
+  ahrs->quat.q2 += q_dot3 * ahrs->inv_sample_freq;
+  ahrs->quat.q3 += q_dot4 * ahrs->inv_sample_freq;
+
+  /* Normalise quaternion */
+  recip_norm = InvSqrt(ahrs->quat.q0 * ahrs->quat.q0 +
+                       ahrs->quat.q1 * ahrs->quat.q1 +
+                       ahrs->quat.q2 * ahrs->quat.q2 +
+                       ahrs->quat.q3 * ahrs->quat.q3);
+  ahrs->quat.q0 *= recip_norm;
+  ahrs->quat.q1 *= recip_norm;
+  ahrs->quat.q2 *= recip_norm;
+  ahrs->quat.q3 *= recip_norm;
+
+  return 0;
+}
+
+/**
+ * @brief 初始化姿态解算
+ *
+ * @param ahrs 姿态解算主结构体
+ * @param magn 磁力计数据
+ * @param sample_freq 采样频率
+ * @return int8_t 0对应没有错误
+ */
+int8_t AHRS_Init(AHRS_t *ahrs, const AHRS_Magn_t *magn, float sample_freq) {
+  if (ahrs == NULL) return -1;
+
+  ahrs->inv_sample_freq = 1.0f / sample_freq;
+
+  ahrs->quat.q0 = 1.0f;
+  ahrs->quat.q1 = 0.0f;
+  ahrs->quat.q2 = 0.0f;
+  ahrs->quat.q3 = 0.0f;
+
+  if (magn) {
+    float yaw = -atan2(magn->y, magn->x);
+
+    if ((magn->x == 0.0f) && (magn->y == 0.0f) && (magn->z == 0.0f)) {
+      ahrs->quat.q0 = 0.800884545f;
+      ahrs->quat.q1 = 0.00862364192f;
+      ahrs->quat.q2 = -0.00283267116f;
+      ahrs->quat.q3 = 0.598749936f;
+
+    } else if ((yaw < (M_PI / 2.0f)) || (yaw > 0.0f)) {
+      ahrs->quat.q0 = 0.997458339f;
+      ahrs->quat.q1 = 0.000336312107f;
+      ahrs->quat.q2 = -0.0057230792f;
+      ahrs->quat.q3 = 0.0740156546;
+
+    } else if ((yaw < M_PI) || (yaw > (M_PI / 2.0f))) {
+      ahrs->quat.q0 = 0.800884545f;
+      ahrs->quat.q1 = 0.00862364192f;
+      ahrs->quat.q2 = -0.00283267116f;
+      ahrs->quat.q3 = 0.598749936f;
+
+    } else if ((yaw < 90.0f) || (yaw > M_PI)) {
+      ahrs->quat.q0 = 0.800884545f;
+      ahrs->quat.q1 = 0.00862364192f;
+      ahrs->quat.q2 = -0.00283267116f;
+      ahrs->quat.q3 = 0.598749936f;
+
+    } else if ((yaw < 90.0f) || (yaw > 0.0f)) {
+      ahrs->quat.q0 = 0.800884545f;
+      ahrs->quat.q1 = 0.00862364192f;
+      ahrs->quat.q2 = -0.00283267116f;
+      ahrs->quat.q3 = 0.598749936f;
+    }
+  }
+  return 0;
+}
+
+/**
+ * @brief 姿态运算更新一次
+ * @note 输入数据必须是NED(North East Down) 参考坐标系
+ *
+ * @param ahrs 姿态解算主结构体
+ * @param accl 加速度计数据
+ * @param gyro 陀螺仪数据
+ * @param magn 磁力计数据
+ * @return int8_t 0对应没有错误
+ */
+int8_t AHRS_Update(AHRS_t *ahrs, const AHRS_Accl_t *accl,
+                   const AHRS_Gyro_t *gyro, const AHRS_Magn_t *magn) {
+  if (ahrs == NULL) return -1;
+  if (accl == NULL) return -1;
+  if (gyro == NULL) return -1;
+
+  beta = BETA_AHRS;
+
+  float recip_norm;
+  float s0, s1, s2, s3;
+  float q_dot1, q_dot2, q_dot3, q_dot4;
+  float hx, hy;
+  float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1,
+      _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3,
+      q2q2, q2q3, q3q3;
+
+  if (magn == NULL) return AHRS_UpdateIMU(ahrs, accl, gyro);
+
+  float mx = magn->x;
+  float my = magn->y;
+  float mz = magn->z;
+
+  /* Use IMU algorithm if magnetometer measurement invalid (avoids NaN in */
+  /* magnetometer normalisation) */
+  if ((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
+    return AHRS_UpdateIMU(ahrs, accl, gyro);
+  }
+
+  float ax = accl->x;
+  float ay = accl->y;
+  float az = accl->z;
+
+  float gx = gyro->x;
+  float gy = gyro->y;
+  float gz = gyro->z;
+
+  /* Rate of change of quaternion from gyroscope */
+  q_dot1 = 0.5f * (-ahrs->quat.q1 * gx - ahrs->quat.q2 * gy -
+                   ahrs->quat.q3 * gz);
+  q_dot2 = 0.5f * (ahrs->quat.q0 * gx + ahrs->quat.q2 * gz -
+                   ahrs->quat.q3 * gy);
+  q_dot3 = 0.5f * (ahrs->quat.q0 * gy - ahrs->quat.q1 * gz +
+                   ahrs->quat.q3 * gx);
+  q_dot4 = 0.5f * (ahrs->quat.q0 * gz + ahrs->quat.q1 * gy -
+                   ahrs->quat.q2 * gx);
+
+  /* Compute feedback only if accelerometer measurement valid (avoids NaN in
+   * accelerometer normalisation) */
+  if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
+    /* Normalise accelerometer measurement */
+    recip_norm = InvSqrt(ax * ax + ay * ay + az * az);
+    ax *= recip_norm;
+    ay *= recip_norm;
+    az *= recip_norm;
+
+    /* Normalise magnetometer measurement */
+    recip_norm = InvSqrt(mx * mx + my * my + mz * mz);
+    mx *= recip_norm;
+    my *= recip_norm;
+    mz *= recip_norm;
+
+    /* Auxiliary variables to avoid repeated arithmetic */
+    _2q0mx = 2.0f * ahrs->quat.q0 * mx;
+    _2q0my = 2.0f * ahrs->quat.q0 * my;
+    _2q0mz = 2.0f * ahrs->quat.q0 * mz;
+    _2q1mx = 2.0f * ahrs->quat.q1 * mx;
+    _2q0 = 2.0f * ahrs->quat.q0;
+    _2q1 = 2.0f * ahrs->quat.q1;
+    _2q2 = 2.0f * ahrs->quat.q2;
+    _2q3 = 2.0f * ahrs->quat.q3;
+    _2q0q2 = 2.0f * ahrs->quat.q0 * ahrs->quat.q2;
+    _2q2q3 = 2.0f * ahrs->quat.q2 * ahrs->quat.q3;
+    q0q0 = ahrs->quat.q0 * ahrs->quat.q0;
+    q0q1 = ahrs->quat.q0 * ahrs->quat.q1;
+    q0q2 = ahrs->quat.q0 * ahrs->quat.q2;
+    q0q3 = ahrs->quat.q0 * ahrs->quat.q3;
+    q1q1 = ahrs->quat.q1 * ahrs->quat.q1;
+    q1q2 = ahrs->quat.q1 * ahrs->quat.q2;
+    q1q3 = ahrs->quat.q1 * ahrs->quat.q3;
+    q2q2 = ahrs->quat.q2 * ahrs->quat.q2;
+    q2q3 = ahrs->quat.q2 * ahrs->quat.q3;
+    q3q3 = ahrs->quat.q3 * ahrs->quat.q3;
+
+    /* Reference direction of Earth's magnetic field */
+    hx = mx * q0q0 - _2q0my * ahrs->quat.q3 +
+         _2q0mz * ahrs->quat.q2 + mx * q1q1 +
+         _2q1 * my * ahrs->quat.q2 + _2q1 * mz * ahrs->quat.q3 -
+         mx * q2q2 - mx * q3q3;
+    hy = _2q0mx * ahrs->quat.q3 + my * q0q0 -
+         _2q0mz * ahrs->quat.q1 + _2q1mx * ahrs->quat.q2 -
+         my * q1q1 + my * q2q2 + _2q2 * mz * ahrs->quat.q3 - my * q3q3;
+    // _2bx = sqrtf(hx * hx + hy * hy);
+    // 改为invsqrt
+    _2bx = 1.f / InvSqrt(hx * hx + hy * hy);
+    _2bz = -_2q0mx * ahrs->quat.q2 + _2q0my * ahrs->quat.q1 +
+           mz * q0q0 + _2q1mx * ahrs->quat.q3 - mz * q1q1 +
+           _2q2 * my * ahrs->quat.q3 - mz * q2q2 + mz * q3q3;
+    _4bx = 2.0f * _2bx;
+    _4bz = 2.0f * _2bz;
+
+    /* Gradient decent algorithm corrective step */
+    s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) +
+         _2q1 * (2.0f * q0q1 + _2q2q3 - ay) -
+         _2bz * ahrs->quat.q2 *
+             (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) +
+         (-_2bx * ahrs->quat.q3 + _2bz * ahrs->quat.q1) *
+             (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) +
+         _2bx * ahrs->quat.q2 *
+             (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
+    s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) +
+         _2q0 * (2.0f * q0q1 + _2q2q3 - ay) -
+         4.0f * ahrs->quat.q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) +
+         _2bz * ahrs->quat.q3 *
+             (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) +
+         (_2bx * ahrs->quat.q2 + _2bz * ahrs->quat.q0) *
+             (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) +
+         (_2bx * ahrs->quat.q3 - _4bz * ahrs->quat.q1) *
+             (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
+    s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) +
+         _2q3 * (2.0f * q0q1 + _2q2q3 - ay) -
+         4.0f * ahrs->quat.q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) +
+         (-_4bx * ahrs->quat.q2 - _2bz * ahrs->quat.q0) *
+             (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) +
+         (_2bx * ahrs->quat.q1 + _2bz * ahrs->quat.q3) *
+             (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) +
+         (_2bx * ahrs->quat.q0 - _4bz * ahrs->quat.q2) *
+             (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
+    s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) +
+         _2q2 * (2.0f * q0q1 + _2q2q3 - ay) +
+         (-_4bx * ahrs->quat.q3 + _2bz * ahrs->quat.q1) *
+             (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) +
+         (-_2bx * ahrs->quat.q0 + _2bz * ahrs->quat.q2) *
+             (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) +
+         _2bx * ahrs->quat.q1 *
+             (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
+    /* normalise step magnitude */
+    recip_norm = InvSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3);
+    s0 *= recip_norm;
+    s1 *= recip_norm;
+    s2 *= recip_norm;
+    s3 *= recip_norm;
+
+    /* Apply feedback step */
+    q_dot1 -= beta * s0;
+    q_dot2 -= beta * s1;
+    q_dot3 -= beta * s2;
+    q_dot4 -= beta * s3;
+  }
+
+  /* Integrate rate of change of quaternion to yield quaternion */
+  ahrs->quat.q0 += q_dot1 * ahrs->inv_sample_freq;
+  ahrs->quat.q1 += q_dot2 * ahrs->inv_sample_freq;
+  ahrs->quat.q2 += q_dot3 * ahrs->inv_sample_freq;
+  ahrs->quat.q3 += q_dot4 * ahrs->inv_sample_freq;
+
+  /* Normalise quaternion */
+  recip_norm = InvSqrt(ahrs->quat.q0 * ahrs->quat.q0 +
+                       ahrs->quat.q1 * ahrs->quat.q1 +
+                       ahrs->quat.q2 * ahrs->quat.q2 +
+                       ahrs->quat.q3 * ahrs->quat.q3);
+  ahrs->quat.q0 *= recip_norm;
+  ahrs->quat.q1 *= recip_norm;
+  ahrs->quat.q2 *= recip_norm;
+  ahrs->quat.q3 *= recip_norm;
+
+  return 0;
+}
+
+/**
+ * @brief 通过姿态解算主结构体中的四元数计算欧拉角
+ *
+ * @param eulr 欧拉角
+ * @param ahrs 姿态解算主结构体
+ * @return int8_t 0对应没有错误
+ */
+int8_t AHRS_GetEulr(AHRS_Eulr_t *eulr, const AHRS_t *ahrs) {
+  if (eulr == NULL) return -1;
+  if (ahrs == NULL) return -1;
+
+  const float sinr_cosp = 2.0f * (ahrs->quat.q0 * ahrs->quat.q1 +
+                                  ahrs->quat.q2 * ahrs->quat.q3);
+  const float cosr_cosp =
+      1.0f - 2.0f * (ahrs->quat.q1 * ahrs->quat.q1 +
+                     ahrs->quat.q2 * ahrs->quat.q2);
+  eulr->pit = atan2f(sinr_cosp, cosr_cosp);
+
+  const float sinp = 2.0f * (ahrs->quat.q0 * ahrs->quat.q2 -
+                             ahrs->quat.q3 * ahrs->quat.q1);
+
+  if (fabsf(sinp) >= 1.0f)
+    eulr->rol = copysignf(M_PI / 2.0f, sinp);
+  else
+    eulr->rol = asinf(sinp);
+
+  const float siny_cosp = 2.0f * (ahrs->quat.q0 * ahrs->quat.q3 +
+                                  ahrs->quat.q1 * ahrs->quat.q2);
+  const float cosy_cosp =
+      1.0f - 2.0f * (ahrs->quat.q2 * ahrs->quat.q2 +
+                     ahrs->quat.q3 * ahrs->quat.q3);
+  eulr->yaw = atan2f(siny_cosp, cosy_cosp);
+
+#if 0
+  eulr->yaw *= M_RAD2DEG_MULT;
+  eulr->rol *= M_RAD2DEG_MULT;
+  eulr->pit *= M_RAD2DEG_MULT;
+#endif
+
+  return 0;
+}
+
+/**
+ * \brief 将对应数据置零
+ *
+ * \param eulr 被操作的数据
+ */
+void AHRS_ResetEulr(AHRS_Eulr_t *eulr) { memset(eulr, 0, sizeof(*eulr)); }
+
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */

User/component/ahrs/ahrs.h

@@ -0,0 +1,114 @@
+/*
+  开源的AHRS算法。
+  MadgwickAHRS
+*/
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "user_math.h"
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+/* 欧拉角（Euler angle） */
+typedef struct {
+  float yaw; /* 偏航角（Yaw angle） */
+  float pit; /* 俯仰角（Pitch angle） */
+  float rol; /* 翻滚角（Roll angle） */
+} AHRS_Eulr_t;
+
+/* 加速度计 Accelerometer */
+typedef struct {
+  float x;
+  float y;
+  float z;
+} AHRS_Accl_t;
+
+/* 陀螺仪 Gyroscope */
+typedef struct {
+  float x;
+  float y;
+  float z;
+} AHRS_Gyro_t;
+
+/* 磁力计 Magnetometer */
+typedef struct {
+  float x;
+  float y;
+  float z;
+} AHRS_Magn_t;
+
+/* 四元数 */
+typedef struct {
+  float q0;
+  float q1;
+  float q2;
+  float q3;
+} AHRS_Quaternion_t;
+
+/* 姿态解算算法主结构体 */
+typedef struct {
+  /* 四元数 */
+  AHRS_Quaternion_t quat;
+
+  float inv_sample_freq; /* 采样频率的的倒数 */
+} AHRS_t;
+
+/* USER STRUCT BEGIN */
+
+/* USER STRUCT END */
+
+/**
+ * @brief 初始化姿态解算
+ *
+ * @param ahrs 姿态解算主结构体
+ * @param magn 磁力计数据
+ * @param sample_freq 采样频率
+ * @return int8_t 0对应没有错误
+ */
+int8_t AHRS_Init(AHRS_t *ahrs, const AHRS_Magn_t *magn, float sample_freq);
+
+/**
+ * @brief 姿态运算更新一次
+ *
+ * @param ahrs 姿态解算主结构体
+ * @param accl 加速度计数据
+ * @param gyro 陀螺仪数据
+ * @param magn 磁力计数据
+ * @return int8_t 0对应没有错误
+ */
+int8_t AHRS_Update(AHRS_t *ahrs, const AHRS_Accl_t *accl,
+                   const AHRS_Gyro_t *gyro, const AHRS_Magn_t *magn);
+
+/**
+ * @brief 通过姿态解算主结构体中的四元数计算欧拉角
+ *
+ * @param eulr 欧拉角
+ * @param ahrs 姿态解算主结构体
+ * @return int8_t 0对应没有错误
+ */
+int8_t AHRS_GetEulr(AHRS_Eulr_t *eulr, const AHRS_t *ahrs);
+
+/**
+ * \brief 将对应数据置零
+ *
+ * \param eulr 被操作的数据
+ */
+void AHRS_ResetEulr(AHRS_Eulr_t *eulr);
+
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */
+
+#ifdef __cplusplus
+}
+#endif

User/component/filter/filter.c

@@ -0,0 +1,185 @@
+/*
+  各类滤波器。
+*/
+
+#include "filter.h"
+
+#include "user_math.h"
+
+/**
+ * @brief 初始化滤波器
+ *
+ * @param f 滤波器
+ * @param sample_freq 采样频率
+ * @param cutoff_freq 截止频率
+ */
+void LowPassFilter2p_Init(LowPassFilter2p_t *f, float sample_freq,
+                          float cutoff_freq) {
+  if (f == NULL) return;
+
+  f->cutoff_freq = cutoff_freq;
+
+  f->delay_element_1 = 0.0f;
+  f->delay_element_2 = 0.0f;
+
+  if (f->cutoff_freq <= 0.0f) {
+    /* no filtering */
+    f->b0 = 1.0f;
+    f->b1 = 0.0f;
+    f->b2 = 0.0f;
+
+    f->a1 = 0.0f;
+    f->a2 = 0.0f;
+
+    return;
+  }
+  const float fr = sample_freq / f->cutoff_freq;
+  const float ohm = tanf(M_PI / fr);
+  const float c = 1.0f + 2.0f * cosf(M_PI / 4.0f) * ohm + ohm * ohm;
+
+  f->b0 = ohm * ohm / c;
+  f->b1 = 2.0f * f->b0;
+  f->b2 = f->b0;
+
+  f->a1 = 2.0f * (ohm * ohm - 1.0f) / c;
+  f->a2 = (1.0f - 2.0f * cosf(M_PI / 4.0f) * ohm + ohm * ohm) / c;
+}
+
+/**
+ * @brief 施加一次滤波计算
+ *
+ * @param f 滤波器
+ * @param sample 采样的值
+ * @return float 滤波后的值
+ */
+float LowPassFilter2p_Apply(LowPassFilter2p_t *f, float sample) {
+  if (f == NULL) return 0.0f;
+
+  /* do the filtering */
+  float delay_element_0 =
+      sample - f->delay_element_1 * f->a1 - f->delay_element_2 * f->a2;
+
+  if (isinf(delay_element_0)) {
+    /* don't allow bad values to propagate via the filter */
+    delay_element_0 = sample;
+  }
+
+  const float output = delay_element_0 * f->b0 + f->delay_element_1 * f->b1 +
+                       f->delay_element_2 * f->b2;
+
+  f->delay_element_2 = f->delay_element_1;
+  f->delay_element_1 = delay_element_0;
+
+  /* return the value. Should be no need to check limits */
+  return output;
+}
+
+/**
+ * @brief 重置滤波器
+ *
+ * @param f 滤波器
+ * @param sample 采样的值
+ * @return float 滤波后的值
+ */
+float LowPassFilter2p_Reset(LowPassFilter2p_t *f, float sample) {
+  if (f == NULL) return 0.0f;
+
+  const float dval = sample / (f->b0 + f->b1 + f->b2);
+
+  if (isfinite(dval)) {
+    f->delay_element_1 = dval;
+    f->delay_element_2 = dval;
+
+  } else {
+    f->delay_element_1 = sample;
+    f->delay_element_2 = sample;
+  }
+
+  return LowPassFilter2p_Apply(f, sample);
+}
+
+/**
+ * @brief 初始化滤波器
+ *
+ * @param f 滤波器
+ * @param sample_freq 采样频率
+ * @param notch_freq 中心频率
+ * @param bandwidth 带宽
+ */
+void NotchFilter_Init(NotchFilter_t *f, float sample_freq, float notch_freq,
+                      float bandwidth) {
+  if (f == NULL) return;
+
+  f->notch_freq = notch_freq;
+  f->bandwidth = bandwidth;
+
+  f->delay_element_1 = 0.0f;
+  f->delay_element_2 = 0.0f;
+
+  if (notch_freq <= 0.0f) {
+    /* no filtering */
+    f->b0 = 1.0f;
+    f->b1 = 0.0f;
+    f->b2 = 0.0f;
+
+    f->a1 = 0.0f;
+    f->a2 = 0.0f;
+
+    return;
+  }
+
+  const float alpha = tanf(M_PI * bandwidth / sample_freq);
+  const float beta = -cosf(M_2PI * notch_freq / sample_freq);
+  const float a0_inv = 1.0f / (alpha + 1.0f);
+
+  f->b0 = a0_inv;
+  f->b1 = 2.0f * beta * a0_inv;
+  f->b2 = a0_inv;
+
+  f->a1 = f->b1;
+  f->a2 = (1.0f - alpha) * a0_inv;
+}
+
+/**
+ * @brief 施加一次滤波计算
+ *
+ * @param f 滤波器
+ * @param sample 采样的值
+ * @return float 滤波后的值
+ */
+inline float NotchFilter_Apply(NotchFilter_t *f, float sample) {
+  if (f == NULL) return 0.0f;
+
+  /* Direct Form II implementation */
+  const float delay_element_0 =
+      sample - f->delay_element_1 * f->a1 - f->delay_element_2 * f->a2;
+  const float output = delay_element_0 * f->b0 + f->delay_element_1 * f->b1 +
+                       f->delay_element_2 * f->b2;
+
+  f->delay_element_2 = f->delay_element_1;
+  f->delay_element_1 = delay_element_0;
+
+  return output;
+}
+
+/**
+ * @brief 重置滤波器
+ *
+ * @param f 滤波器
+ * @param sample 采样的值
+ * @return float 滤波后的值
+ */
+float NotchFilter_Reset(NotchFilter_t *f, float sample) {
+  if (f == NULL) return 0.0f;
+
+  float dval = sample;
+
+  if (fabsf(f->b0 + f->b1 + f->b2) > FLT_EPSILON) {
+    dval = dval / (f->b0 + f->b1 + f->b2);
+  }
+
+  f->delay_element_1 = dval;
+  f->delay_element_2 = dval;
+
+  return NotchFilter_Apply(f, sample);
+}

User/component/filter/filter.h

@@ -0,0 +1,120 @@
+/*
+  各类滤波器。
+*/
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "user_math.h"
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+/* 二阶低通滤波器 */
+typedef struct {
+  float cutoff_freq; /* 截止频率 */
+
+  float a1;
+  float a2;
+
+  float b0;
+  float b1;
+  float b2;
+
+  float delay_element_1;
+  float delay_element_2;
+
+} LowPassFilter2p_t;
+
+/* 带阻滤波器 */
+typedef struct {
+  float notch_freq; /* 阻止频率 */
+  float bandwidth;  /* 带宽 */
+
+  float a1;
+  float a2;
+
+  float b0;
+  float b1;
+  float b2;
+  float delay_element_1;
+  float delay_element_2;
+
+} NotchFilter_t;
+
+/* USER STRUCT BEGIN */
+
+/* USER STRUCT END */
+
+/**
+ * @brief 初始化滤波器
+ *
+ * @param f 滤波器
+ * @param sample_freq 采样频率
+ * @param cutoff_freq 截止频率
+ */
+void LowPassFilter2p_Init(LowPassFilter2p_t *f, float sample_freq,
+                          float cutoff_freq);
+
+/**
+ * @brief 施加一次滤波计算
+ *
+ * @param f 滤波器
+ * @param sample 采样的值
+ * @return float 滤波后的值
+ */
+float LowPassFilter2p_Apply(LowPassFilter2p_t *f, float sample);
+
+/**
+ * @brief 重置滤波器
+ *
+ * @param f 滤波器
+ * @param sample 采样的值
+ * @return float 滤波后的值
+ */
+float LowPassFilter2p_Reset(LowPassFilter2p_t *f, float sample);
+
+/**
+ * @brief 初始化滤波器
+ *
+ * @param f 滤波器
+ * @param sample_freq 采样频率
+ * @param notch_freq 中心频率
+ * @param bandwidth 带宽
+ */
+void NotchFilter_Init(NotchFilter_t *f, float sample_freq, float notch_freq,
+                      float bandwidth);
+
+/**
+ * @brief 施加一次滤波计算
+ *
+ * @param f 滤波器
+ * @param sample 采样的值
+ * @return float 滤波后的值
+ */
+float NotchFilter_Apply(NotchFilter_t *f, float sample);
+
+/**
+ * @brief 重置滤波器
+ *
+ * @param f 滤波器
+ * @param sample 采样的值
+ * @return float 滤波后的值
+ */
+float NotchFilter_Reset(NotchFilter_t *f, float sample);
+
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */
+
+#ifdef __cplusplus
+}
+#endif

User/device/ET16s.c

@@ -0,0 +1,181 @@
+/* Includes ----------------------------------------------------------------- */
+#include "ET16s.h"
+#include "device/device.h"
+#include <string.h>
+//#include "error_detect.h"
+#include  "bsp/uart.h"
+#include "component/calc_lib.h"
+
+#define R12DS
+//#define DR16
+/*
+两个遥控器都是
+             x                
+             |                      
+             |                     
+              ———y 
+*/
+#ifdef DR16
+
+#define FRAME_LEN 36
+
+#endif
+
+#ifdef R12DS
+
+#define FRAME_LEN 25u
+
+#endif
+/* Private define ----------------------------------------------------------- */
+#define REMOTE_CH_VALUE_MIN (364u)
+#define REMOTE_CH_VALUE_MID (1024u)
+#define REMOTE_CH_VALUE_MAX (1684u)
+
+/* Private macro ------------------------------------------------------------ */
+/* Private typedef ---------------------------------------------------------- */
+/* Private variables -------------------------------------------------------- */
+
+
+static osThreadId_t thread_alert;
+//uint8_t cbuf[FRAME_LEN]; //此处设置为两帧字节的长度 若为一帧会出现乱码的情况
+//uint8_t cbuf[sizeof(LD_raw_t)];
+uint8_t cbuf[25u];
+/* Private function  -------------------------------------------------------- */
+int8_t REMOTE_Restart(void) {
+  __HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_3));
+  __HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_3));
+  return DEVICE_OK;
+}
+
+static void REMOTE_RxCpltCallback(void) {
+  osThreadFlagsSet(thread_alert, SIGNAL_DR16_RAW_REDY);
+// 	detect_hook(DR16_TOE);
+}
+//遥控器初始化
+int8_t REMOTE_Init(void) {
+  if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL;
+
+  BSP_UART_RegisterCallback(BSP_UART_3,BSP_UART_RX_CPLT_CB,
+                           REMOTE_RxCpltCallback);
+  memset(cbuf, 0, sizeof(cbuf));//初始化清空数据包                          
+  return DEVICE_OK;
+}
+
+int8_t REMOTE_StartDmaRecv(void) {
+  if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_3),
+                           (uint8_t *)cbuf,
+                           sizeof(cbuf)) == HAL_OK)
+    return DEVICE_OK;
+  return DEVICE_ERR;
+}
+
+bool REMOTE_WaitDmaCplt(uint32_t timeout) {
+  return (osThreadFlagsWait(SIGNAL_DR16_RAW_REDY, osFlagsWaitAll, timeout) ==
+          SIGNAL_DR16_RAW_REDY);
+}
+
+
+
+/*乐迪用的按键封装 */
+ET16S_SwitchPos_t Keymap(int16_t value) {
+  return (value == 353) ? ET16S_SW_UP :
+         (value == 1024) ? ET16S_SW_MID :
+         (value == 1695) ? ET16S_SW_DOWN : ET16S_SW_ERR;
+}
+
+int8_t ET16s_ParseRaw( ET16s_t *et16s){		
+//ET16s
+	et16s->raw_data.ch[0] = (cbuf[1] | (cbuf[2] << 8)) & 0x07ff;        //Channel 1
+	et16s->raw_data.ch[3] = ((cbuf[2] >> 3) | (cbuf[3] << 5)) & 0x07ff; //Channel 2
+	et16s->raw_data.ch[1] = ((cbuf[3] >> 6) | (cbuf[4] << 2) |          //Channel 3
+                       (cbuf[5] << 10)) &0x07ff;
+	et16s->raw_data.ch[2] = ((cbuf[5] >> 1) | (cbuf[6] << 7)) & 0x07ff; //Channel 4
+
+	et16s->raw_data.sw[6] = ((int16_t)cbuf[6] >> 4 | ((int16_t)cbuf[7] << 4 )) & 0x07FF;                   								//Channel 5
+	et16s->raw_data.sw[0] = ((int16_t)cbuf[7] >> 7 | ((int16_t)cbuf[8] << 1 ) | (int16_t)cbuf[9] << 9 ) & 0x07FF;    	//Channel 6
+	et16s->raw_data.sw[5] = ((int16_t)cbuf[9] >> 2 | ((int16_t)cbuf[10] << 6 )) & 0x07FF;;                    							//Channel 7
+	et16s->raw_data.sw[4] = ((int16_t)cbuf[10] >> 5 | ((int16_t)cbuf[11] << 3 )) & 0x07FF;                    							//Channel 8
+	et16s->raw_data.sw[3] = ((int16_t)cbuf[12] << 0 | ((int16_t)cbuf[13] << 8 )) & 0x07FF;                  								//Channel 9
+	et16s->raw_data.sw[2] = ((int16_t)cbuf[13] >> 3 | ((int16_t)cbuf[14] << 5 )) & 0x07FF;                               	//Channel 10
+	et16s->raw_data.sw[1] = ((int16_t)cbuf[14] >> 6 | ((int16_t)cbuf[15] << 2 ) | (int16_t)cbuf[16] << 10 ) & 0x07FF;	//Channel 11
+	et16s->raw_data.sw[7] = ((int16_t)cbuf[16] >> 1 | ((int16_t)cbuf[17] << 7 )) & 0x07FF;  
+	
+	et16s->raw_data.ch[1] = map_fp32(	et16s->raw_data.ch[1],353,1695,-1,1);		//lx
+	et16s->raw_data.ch[0] = map_fp32(	et16s->raw_data.ch[0],353,1695,-1,1);		//ly
+	et16s->raw_data.ch[2] = map_fp32(	et16s->raw_data.ch[2],353,1695,-1,1);		//rx
+	et16s->raw_data.ch[3] = map_fp32(	et16s->raw_data.ch[3],353,1695,-1,1);     //ry
+	
+	if(	et16s->raw_data.ch[0]>-0.01&&et16s->raw_data.ch[0]<=0.01) et16s->raw_data.ch[0]=0;              
+	if(	et16s->raw_data.ch[1]>-0.01&&et16s->raw_data.ch[1]<=0.01) et16s->raw_data.ch[1]=0;                
+	if(	et16s->raw_data.ch[2]>-0.01&&et16s->raw_data.ch[2]<=0.01) et16s->raw_data.ch[2]=0;              
+	if( et16s->raw_data.ch[3]>-0.01&&et16s->raw_data.ch[3]<=0.01) et16s->raw_data.ch[3]=0;       
+  return 1;
+
+}
+
+int8_t ET16S_ParseRC(ET16s_t *et16s) {
+  if (et16s == NULL) return DEVICE_ERR_NULL;
+
+#ifdef DR16 
+  rc->rc_type=RC_DR16; 
+  DR16_ParseRaw(dr16);
+  if (DR16_DataCorrupted(dr16)) {
+		return DEVICE_ERR;
+  } else {
+    memset(rc, 0, sizeof(*rc));
+  }
+
+  float full_range = (float)(REMOTE_CH_VALUE_MAX - REMOTE_CH_VALUE_MIN);
+
+  rc->DJ.ch_r_x = 2 * ((float)dr16->data.ch_r_y - REMOTE_CH_VALUE_MID) / full_range;
+  rc->DJ.ch_r_y = 2 * ((float)dr16->data.ch_r_x - REMOTE_CH_VALUE_MID) / full_range;
+  rc->DJ.ch_l_x = 2 * ((float)dr16->data.ch_l_y - REMOTE_CH_VALUE_MID) / full_range;
+  rc->DJ.ch_l_y = 2 * ((float)dr16->data.ch_l_x - REMOTE_CH_VALUE_MID) / full_range;
+
+  rc->DJ.sw_l = (ET16S_SwitchPos_t)dr16->data.sw_l;
+  rc->DJ.sw_r = (ET16S_SwitchPos_t)dr16->data.sw_r;
+
+ // rc->dr16.key = dr16->data.key;
+
+//  rc->ch_res = ((float)dr16->data.res - DR16_CH_VALUE_MID) / full_range;
+
+#elif defined(R12DS)	
+	
+	/*天地飞*/
+	et16s->rc_type=RC_ET16s;
+	ET16s_ParseRaw(et16s);
+	
+  et16s->ET16s.ch_r_x = et16s->raw_data.ch[2] ;
+  et16s->ET16s.ch_r_y = et16s->raw_data.ch[3] ;
+	et16s->ET16s.ch_l_x = et16s->raw_data.ch[0] ;
+  et16s->ET16s.ch_l_y = et16s->raw_data.ch[1] ;
+
+  et16s->ET16s.key_A = Keymap(et16s->raw_data.sw[0]);
+  et16s->ET16s.key_B = Keymap(et16s->raw_data.sw[1]);
+  et16s->ET16s.key_C = Keymap(et16s->raw_data.sw[2]);
+  et16s->ET16s.key_D = Keymap(et16s->raw_data.sw[3]);
+  et16s->ET16s.key_E = Keymap(et16s->raw_data.sw[4]);
+  et16s->ET16s.key_F = Keymap(et16s->raw_data.sw[5]);
+  et16s->ET16s.key_G = Keymap(et16s->raw_data.sw[6]);	
+  et16s->ET16s.key_H = Keymap(et16s->raw_data.sw[7]);
+  // rc->ET16s.knob_left = ET16s->sw[7]; //200 330 479 629 778 928 1075 1224 1373 1522 1670 1800
+
+//  /*离线处理，和dr16位置不同*/
+	if(et16s->raw_data.sw[6]==1695)
+	{
+		ET16s_HandleOffline(et16s);
+//    memset(cbuf, 0, sizeof(cbuf));	//有时候会出现消息数组错位，所以直接清空，在离线和指定按键不对的情况下，原数据不可信	
+	}
+#endif
+  return DEVICE_OK;
+}
+
+
+
+int8_t ET16s_HandleOffline(ET16s_t *rc) {
+  if (rc == NULL) return DEVICE_ERR_NULL;
+  
+  rc->rc_type =Control_loss ;
+	memset(&rc->ET16s , 0, sizeof(ET16s_raw_t));
+	return 0;
+}

User/device/ET16s.h

@@ -0,0 +1,115 @@
+/*
+ * 云台模组
+ */
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include "cmsis_os2.h" 
+#include <stdbool.h>
+#include <stdint.h>
+/* Exported constants ------------------------------------------------------- */
+#define ET16s_OK (0)        /* 运行正常 */
+#define ET16s_ERR (-1)      /* 运行时发现了其他错误 */
+#define ET16s_ERR_NULL (-2) /* 运行时发现NULL指针 */
+#define ET16s_ERR_MODE (-3) /* 运行时配置了错误的CMD_GimbalMode_t */
+
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+
+
+/* 遥控器类型 */
+// 遥控器状态
+typedef enum
+{
+    RC_DR16,
+    RC_LD,
+    RC_ET16s,
+    Control_loss
+} rc_type_t;
+
+/* 拨杆位置 */
+typedef enum
+{
+    ET16S_SW_ERR = 0,
+    ET16S_SW_UP = 1,
+    ET16S_SW_MID = 3,
+    ET16S_SW_DOWN = 2,
+} ET16S_SwitchPos_t;
+
+
+//乐迪
+typedef struct
+{
+  float ch[4];
+  int16_t sw[8]; 
+
+} LD_raw_t;
+
+typedef struct
+{
+  float ch[4];
+  int16_t sw[8]; 
+} ET16s_raw_t;
+
+// 遥控器数据
+typedef struct
+{
+
+    rc_type_t rc_type;
+
+    struct
+    {
+        float ch_l_x; /* 遥控器左侧摇杆横轴值，上为正 */
+        float ch_l_y; /* 遥控器左侧摇杆纵轴值，右为正 */
+        float ch_r_x; /* 遥控器右侧摇杆横轴值，上为正 */
+        float ch_r_y; /* 遥控器右侧摇杆纵轴值，右为正 */
+
+        ET16S_SwitchPos_t sw_r; /* 右侧拨杆位置 */
+        ET16S_SwitchPos_t sw_l; /* 左侧拨杆位置 */
+
+    } __attribute__((packed)) DJ;
+
+    struct
+    {
+        float ch_l_x; /* 遥控器左侧摇杆横轴值，上为正 */
+        float ch_l_y; /* 遥控器左侧摇杆纵轴值，右为正 */
+        float ch_r_x; /* 遥控器右侧摇杆横轴值，上为正 */
+        float ch_r_y; /* 遥控器右侧摇杆纵轴值，右为正 */
+
+        ET16S_SwitchPos_t key_A;
+        ET16S_SwitchPos_t key_B;
+        ET16S_SwitchPos_t key_C;
+        ET16S_SwitchPos_t key_D;
+        ET16S_SwitchPos_t key_E;
+        ET16S_SwitchPos_t key_F;
+        ET16S_SwitchPos_t key_G;
+        ET16S_SwitchPos_t key_H;
+
+        int16_t knob_left;  // 左旋钮
+        int16_t knob_right; // 右旋钮
+    } __attribute__((packed)) ET16s;
+				ET16s_raw_t raw_data;
+} __attribute__((packed)) ET16s_t;
+
+/* Exported functions prototypes -------------------------------------------- */
+int8_t REMOTE_Restart(void);
+int8_t REMOTE_Init(void);
+int8_t REMOTE_StartDmaRecv(void);
+bool REMOTE_WaitDmaCplt(uint32_t timeout);
+
+
+int8_t ET16S_ParseRC(ET16s_t *et16s);
+
+int8_t LD_HandleOffline(const LD_raw_t *LD, ET16s_t *rc); 
+int8_t ET16s_HandleOffline(ET16s_t *rc);
+
+
+#ifdef __cplusplus
+}
+#endif
+

User/device/device_config.yaml

@@ -22,6 +22,9 @@ motor:
 motor_dm:
   bsp_config: {}
   enabled: true
+motor_lk:
+  bsp_config: {}
+  enabled: true
 motor_lz:
   bsp_config: {}
   enabled: true

User/device/motor_dm.c

@@ -111,11 +111,21 @@ static int8_t MOTOR_DM_SendMITCmd(MOTOR_DM_t *motor, MOTOR_MIT_Output_t *output)
 	uint16_t pos_tmp,vel_tmp,kp_tmp,kd_tmp,tor_tmp;
 	uint16_t id = motor->param.can_id;
 
-	pos_tmp = float_to_uint(output->angle, DM_P_MIN , DM_P_MAX, 16);
+    /* 根据 reverse 参数调整控制值 */
-	vel_tmp = float_to_uint(output->velocity, DM_V_MIN , DM_V_MAX, 12);
+    float angle = output->angle;
+    float velocity = output->velocity;
+    float torque = output->torque;
+    if (motor->param.reverse) {
+        angle = -angle;
+        velocity = -velocity;
+        torque = -torque;
+    }
+
+	pos_tmp = float_to_uint(angle, DM_P_MIN , DM_P_MAX, 16);
+	vel_tmp = float_to_uint(velocity, DM_V_MIN , DM_V_MAX, 12);
 	kp_tmp  = float_to_uint(output->kp,   DM_KP_MIN, DM_KP_MAX, 12);
 	kd_tmp  = float_to_uint(output->kd,   DM_KD_MIN, DM_KD_MAX, 12);
-	tor_tmp = float_to_uint(output->torque, DM_T_MIN , DM_T_MAX, 12);
+	tor_tmp = float_to_uint(torque, DM_T_MIN , DM_T_MAX, 12);
 
     /* 打包数据 */
 	data[0] = (pos_tmp >> 8);
@@ -151,6 +161,11 @@ static int8_t MOTOR_DM_SendPosVelCmd(MOTOR_DM_t *motor, float pos, float vel) {
     
     uint8_t data[8] = {0};
 
+    /* 根据 reverse 参数调整控制值 */
+    if (motor->param.reverse) {
+        pos = -pos;
+        vel = -vel;
+    }
     
     /* 直接发送浮点数数据 */
     memcpy(&data[0], &pos, 4);  // 位置，低位在前
@@ -179,6 +194,11 @@ static int8_t MOTOR_DM_SendVelCmd(MOTOR_DM_t *motor, float vel) {
     
     uint8_t data[4] = {0};
     
+    /* 根据 reverse 参数调整控制值 */
+    if (motor->param.reverse) {
+        vel = -vel;
+    }
+    
     /* 直接发送浮点数数据 */
     memcpy(&data[0], &vel, 4);  // 速度，低位在前
     

User/device/motor_lk.c

@@ -0,0 +1,329 @@
+/*
+  LK电机驱动
+*/
+/* Includes ----------------------------------------------------------------- */
+#include "motor_lk.h"
+#include <stdbool.h>
+#include <string.h>
+#include "bsp/can.h"
+#include "bsp/mm.h"
+#include "bsp/time.h"
+#include "component/user_math.h"
+
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* Private define ----------------------------------------------------------- */
+#define LK_CTRL_ID_BASE        (0x140)
+#define LK_FB_ID_BASE          (0x240)
+
+// LK电机命令字节定义
+#define LK_CMD_FEEDBACK        (0x9C)    // 反馈命令字节
+#define LK_CMD_MOTOR_OFF       (0x80)    // 电机关闭命令
+#define LK_CMD_MOTOR_ON        (0x88)    // 电机运行命令
+#define LK_CMD_TORQUE_CTRL     (0xA1)    // 转矩闭环控制命令
+
+// LK电机参数定义
+#define LK_CURR_LSB_MF         (33.0f / 4096.0f)   // MF电机转矩电流分辨率 A/LSB
+#define LK_CURR_LSB_MG         (66.0f / 4096.0f)   // MG电机转矩电流分辨率 A/LSB
+#define LK_POWER_RANGE_MS      (1000)              // MS电机功率范围 ±1000
+#define LK_TORQUE_RANGE        (2048)              // 转矩控制值范围 ±2048
+#define LK_TORQUE_CURRENT_MF   (16.5f)             // MF电机最大转矩电流 A
+#define LK_TORQUE_CURRENT_MG   (33.0f)             // MG电机最大转矩电流 A
+
+#define MOTOR_TX_BUF_SIZE      (8)
+#define MOTOR_RX_BUF_SIZE      (8)
+
+// 编码器分辨率定义
+#define LK_ENC_14BIT_MAX       (16383)   // 14位编码器最大值
+#define LK_ENC_15BIT_MAX       (32767)   // 15位编码器最大值  
+#define LK_ENC_16BIT_MAX       (65535)   // 16位编码器最大值
+
+/* USER DEFINE BEGIN */
+
+/* USER DEFINE END */
+
+/* Private macro ------------------------------------------------------------ */
+/* Private typedef ---------------------------------------------------------- */
+/* USER STRUCT BEGIN */
+
+/* USER STRUCT END */
+
+/* Private variables -------------------------------------------------------- */
+static MOTOR_LK_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL};
+
+/* Private functions -------------------------------------------------------- */
+/* USER FUNCTION BEGIN */
+
+/* USER FUNCTION END */
+
+static float MOTOR_LK_GetCurrentLSB(MOTOR_LK_Module_t module) {
+    switch (module) {
+        case MOTOR_LK_MF9025:
+        case MOTOR_LK_MF9035:
+            return LK_CURR_LSB_MF;
+        default:
+            return LK_CURR_LSB_MG;  // 默认使用MG的分辨率
+    }
+}
+
+static uint16_t MOTOR_LK_GetEncoderMax(MOTOR_LK_Module_t module) {
+    // 根据电机型号返回编码器最大值，这里假设都使用16位编码器
+    // 实际使用时需要根据具体电机型号配置
+    return LK_ENC_16BIT_MAX;
+}
+
+static MOTOR_LK_CANManager_t* MOTOR_LK_GetCANManager(BSP_CAN_t can) {
+    if (can >= BSP_CAN_NUM) return NULL;
+    return can_managers[can];
+}
+
+static int8_t MOTOR_LK_CreateCANManager(BSP_CAN_t can) {
+    if (can >= BSP_CAN_NUM) return DEVICE_ERR;
+    if (can_managers[can] != NULL) return DEVICE_OK;
+    
+    can_managers[can] = (MOTOR_LK_CANManager_t*)BSP_Malloc(sizeof(MOTOR_LK_CANManager_t));
+    if (can_managers[can] == NULL) return DEVICE_ERR;
+    
+    memset(can_managers[can], 0, sizeof(MOTOR_LK_CANManager_t));
+    can_managers[can]->can = can;
+    return DEVICE_OK;
+}
+
+static void MOTOR_LK_Decode(MOTOR_LK_t *motor, BSP_CAN_Message_t *msg) {
+
+    // 检查命令字节是否为反馈命令
+    if (msg->data[0] != LK_CMD_FEEDBACK) {
+        // 如果不是标准反馈命令，可能是其他格式的数据
+        // 临时跳过命令字节检查，直接解析数据
+        // return;
+    }
+    
+    // 解析温度 (DATA[1])
+    motor->motor.feedback.temp = (int8_t)msg->data[1];
+
+    // 解析转矩电流值或功率值 (DATA[2], DATA[3])
+    int16_t raw_current_or_power = (int16_t)((msg->data[3] << 8) | msg->data[2]);
+
+    // 根据电机类型解析电流或功率
+    switch (motor->param.module) {
+        case MOTOR_LK_MF9025:
+        case MOTOR_LK_MF9035:
+            motor->motor.feedback.torque_current = raw_current_or_power * MOTOR_LK_GetCurrentLSB(motor->param.module);
+            break;
+        default:
+            motor->motor.feedback.torque_current = (float)raw_current_or_power;
+            break;
+    }
+
+    // 解析转速 (DATA[4], DATA[5]) - 单位：1dps/LSB
+    int16_t raw_speed = (int16_t)((msg->data[5] << 8) | msg->data[4]);
+    motor->motor.feedback.rotor_speed = motor->param.reverse ? -raw_speed : raw_speed;
+
+    // 解析编码器值 (DATA[6], DATA[7])
+    uint16_t raw_encoder = (uint16_t)((msg->data[7] << 8) | msg->data[6]);
+    uint16_t encoder_max = MOTOR_LK_GetEncoderMax(motor->param.module);
+
+    // 将编码器值转换为弧度 (0 ~ 2π)
+    float angle = (float)raw_encoder / (float)encoder_max * M_2PI;
+    motor->motor.feedback.rotor_abs_angle = motor->param.reverse ? (M_2PI - angle) : angle;
+}
+
+/* Exported functions ------------------------------------------------------- */
+
+int8_t MOTOR_LK_Register(MOTOR_LK_Param_t *param) {
+    if (param == NULL) return DEVICE_ERR_NULL;
+    
+    if (MOTOR_LK_CreateCANManager(param->can) != DEVICE_OK) return DEVICE_ERR;
+    MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager(param->can);
+    if (manager == NULL) return DEVICE_ERR;
+    
+    // 检查是否已注册
+    for (int i = 0; i < manager->motor_count; i++) {
+        if (manager->motors[i] && manager->motors[i]->param.id == param->id) {
+            return DEVICE_ERR_INITED;
+        }
+    }
+    
+    // 检查数量
+    if (manager->motor_count >= MOTOR_LK_MAX_MOTORS) return DEVICE_ERR;
+    
+    // 创建新电机实例
+    MOTOR_LK_t *new_motor = (MOTOR_LK_t*)BSP_Malloc(sizeof(MOTOR_LK_t));
+    if (new_motor == NULL) return DEVICE_ERR;
+    
+    memcpy(&new_motor->param, param, sizeof(MOTOR_LK_Param_t));
+    memset(&new_motor->motor, 0, sizeof(MOTOR_t));
+    new_motor->motor.reverse = param->reverse;
+    
+    // 对于某些LK电机，反馈数据可能通过命令ID发送
+    // 根据实际测试，使用命令ID接收反馈数据
+    uint16_t feedback_id = param->id;  // 使用命令ID作为反馈ID
+    
+    // 注册CAN接收ID
+    if (BSP_CAN_RegisterId(param->can, feedback_id, 3) != BSP_OK) {
+        BSP_Free(new_motor);
+        return DEVICE_ERR;
+    }
+    
+    manager->motors[manager->motor_count] = new_motor;
+    manager->motor_count++;
+    return DEVICE_OK;
+}
+
+int8_t MOTOR_LK_Update(MOTOR_LK_Param_t *param) {
+    if (param == NULL) return DEVICE_ERR_NULL;
+    
+    MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager(param->can);
+    if (manager == NULL) return DEVICE_ERR_NO_DEV;
+    
+    for (int i = 0; i < manager->motor_count; i++) {
+        MOTOR_LK_t *motor = manager->motors[i];
+        if (motor && motor->param.id == param->id) {
+            // 对于某些LK电机，反馈数据通过命令ID发送
+            uint16_t feedback_id = param->id;
+            
+            BSP_CAN_Message_t rx_msg;
+            if (BSP_CAN_GetMessage(param->can, feedback_id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) {
+                uint64_t now_time = BSP_TIME_Get();
+                if (now_time - motor->motor.header.last_online_time > 1000) {
+                    motor->motor.header.online = false;
+                    return DEVICE_ERR_NO_DEV;
+                }
+                return DEVICE_ERR;
+            }
+            
+            motor->motor.header.online = true;
+            motor->motor.header.last_online_time = BSP_TIME_Get();
+            MOTOR_LK_Decode(motor, &rx_msg);
+            return DEVICE_OK;
+        }
+    }
+    return DEVICE_ERR_NO_DEV;
+}
+
+int8_t MOTOR_LK_UpdateAll(void) {
+    int8_t ret = DEVICE_OK;
+    for (int can = 0; can < BSP_CAN_NUM; can++) {
+        MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager((BSP_CAN_t)can);
+        if (manager == NULL) continue;
+        
+        for (int i = 0; i < manager->motor_count; i++) {
+            MOTOR_LK_t *motor = manager->motors[i];
+            if (motor != NULL) {
+                if (MOTOR_LK_Update(&motor->param) != DEVICE_OK) {
+                    ret = DEVICE_ERR;
+                }
+            }
+        }
+    }
+    return ret;
+}
+
+int8_t MOTOR_LK_SetOutput(MOTOR_LK_Param_t *param, float value) {
+    if (param == NULL) return DEVICE_ERR_NULL;
+
+    MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager(param->can);
+    if (manager == NULL) return DEVICE_ERR_NO_DEV;
+
+    // 限制输出值范围
+    if (value > 1.0f) value = 1.0f;
+    if (value < -1.0f) value = -1.0f;
+
+    MOTOR_LK_t *motor = MOTOR_LK_GetMotor(param);
+    if (motor == NULL) return DEVICE_ERR_NO_DEV;
+
+    // 根据反转参数调整输出
+    float output = param->reverse ? -value : value;
+
+    // 转矩闭环控制命令 - 将输出值转换为转矩控制值
+    int16_t torque_control = (int16_t)(output * (float)LK_TORQUE_RANGE);
+
+    // 构建CAN帧
+    BSP_CAN_StdDataFrame_t tx_frame;
+    tx_frame.id = param->id;
+    tx_frame.dlc = MOTOR_TX_BUF_SIZE;
+
+    tx_frame.data[0] = LK_CMD_TORQUE_CTRL;
+    tx_frame.data[1] = 0x00;
+    tx_frame.data[2] = 0x00;
+    tx_frame.data[3] = 0x00;
+    tx_frame.data[4] = (uint8_t)(torque_control & 0xFF);
+    tx_frame.data[5] = (uint8_t)((torque_control >> 8) & 0xFF);
+    tx_frame.data[6] = 0x00;
+    tx_frame.data[7] = 0x00;
+    return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
+}
+
+int8_t MOTOR_LK_Ctrl(MOTOR_LK_Param_t *param) {
+    // 对于LK电机，每次设置输出时就直接发送控制命令
+    // 这个函数可以用于发送其他控制命令，如电机开启/关闭
+    return DEVICE_OK;
+}
+
+int8_t MOTOR_LK_MotorOn(MOTOR_LK_Param_t *param) {
+    if (param == NULL) return DEVICE_ERR_NULL;
+    
+    BSP_CAN_StdDataFrame_t tx_frame;
+    tx_frame.id = param->id;
+    tx_frame.dlc = MOTOR_TX_BUF_SIZE;
+    
+    // 电机运行命令
+    tx_frame.data[0] = LK_CMD_MOTOR_ON;     // 命令字节
+    tx_frame.data[1] = 0x00;
+    tx_frame.data[2] = 0x00;
+    tx_frame.data[3] = 0x00;
+    tx_frame.data[4] = 0x00;
+    tx_frame.data[5] = 0x00;
+    tx_frame.data[6] = 0x00;
+    tx_frame.data[7] = 0x00;
+    return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
+}
+
+int8_t MOTOR_LK_MotorOff(MOTOR_LK_Param_t *param) {
+    if (param == NULL) return DEVICE_ERR_NULL;
+    
+    BSP_CAN_StdDataFrame_t tx_frame;
+    tx_frame.id = param->id;
+    tx_frame.dlc = MOTOR_TX_BUF_SIZE;
+    
+    // 电机关闭命令
+    tx_frame.data[0] = LK_CMD_MOTOR_OFF;    // 命令字节
+    tx_frame.data[1] = 0x00;
+    tx_frame.data[2] = 0x00;
+    tx_frame.data[3] = 0x00;
+    tx_frame.data[4] = 0x00;
+    tx_frame.data[5] = 0x00;
+    tx_frame.data[6] = 0x00;
+    tx_frame.data[7] = 0x00;
+    return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
+}
+
+MOTOR_LK_t* MOTOR_LK_GetMotor(MOTOR_LK_Param_t *param) {
+    if (param == NULL) return NULL;
+    
+    MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager(param->can);
+    if (manager == NULL) return NULL;
+    
+    for (int i = 0; i < manager->motor_count; i++) {
+        MOTOR_LK_t *motor = manager->motors[i];
+        if (motor && motor->param.id == param->id) {
+            return motor;
+        }
+    }
+    return NULL;
+}
+
+int8_t MOTOR_LK_Relax(MOTOR_LK_Param_t *param) {
+    return MOTOR_LK_SetOutput(param, 0.0f);
+}
+
+int8_t MOTOR_LK_Offine(MOTOR_LK_Param_t *param) {
+    MOTOR_LK_t *motor = MOTOR_LK_GetMotor(param);
+    if (motor) {
+        motor->motor.header.online = false;
+        return DEVICE_OK;
+    }
+    return DEVICE_ERR_NO_DEV;
+}

User/device/motor_lk.h

@@ -0,0 +1,119 @@
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+#include "device/device.h"
+#include "device/motor.h"
+#include "bsp/can.h"
+
+/* Exported constants ------------------------------------------------------- */
+#define MOTOR_LK_MAX_MOTORS 32
+
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+typedef enum {
+    MOTOR_LK_MF9025,
+    MOTOR_LK_MF9035,
+} MOTOR_LK_Module_t;
+
+
+/*每个电机需要的参数*/
+typedef struct {
+    BSP_CAN_t can;
+    uint16_t id;
+    MOTOR_LK_Module_t module;
+    bool reverse;
+} MOTOR_LK_Param_t;
+
+/*电机实例*/
+typedef struct{
+    MOTOR_LK_Param_t param;
+    MOTOR_t motor;
+} MOTOR_LK_t;
+
+/*CAN管理器，管理一个CAN总线上所有的电机*/
+typedef struct {
+    BSP_CAN_t can;
+    MOTOR_LK_t *motors[MOTOR_LK_MAX_MOTORS];
+    uint8_t motor_count;
+} MOTOR_LK_CANManager_t;
+
+/* Exported functions prototypes -------------------------------------------- */
+
+/**
+ * @brief 注册一个LK电机
+ * @param param 电机参数
+ * @return 
+ */
+int8_t MOTOR_LK_Register(MOTOR_LK_Param_t *param);
+
+/**
+ * @brief 更新指定电机数据
+ * @param param 电机参数
+ * @return 
+ */
+int8_t MOTOR_LK_Update(MOTOR_LK_Param_t *param);
+
+/**
+ * @brief 设置一个电机的输出
+ * @param param 电机参数
+ * @param value 输出值，范围[-1.0, 1.0]
+ * @return 
+ */
+int8_t MOTOR_LK_SetOutput(MOTOR_LK_Param_t *param, float value);
+
+/**
+ * @brief 发送控制命令到电机，注意一个CAN可以控制多个电机，所以只需要发送一次即可
+ * @param param 电机参数
+ * @return 
+ */
+int8_t MOTOR_LK_Ctrl(MOTOR_LK_Param_t *param);
+
+/**
+ * @brief 发送电机开启命令
+ * @param param 电机参数
+ * @return 
+ */
+int8_t MOTOR_LK_MotorOn(MOTOR_LK_Param_t *param);
+
+/**
+ * @brief 发送电机关闭命令
+ * @param param 电机参数
+ * @return 
+ */
+int8_t MOTOR_LK_MotorOff(MOTOR_LK_Param_t *param);
+
+/**
+ * @brief 获取指定电机的实例指针
+ * @param param 电机参数
+ * @return 
+ */
+MOTOR_LK_t* MOTOR_LK_GetMotor(MOTOR_LK_Param_t *param);
+
+/**
+ * @brief 使电机松弛（设置输出为0）
+ * @param param 
+ * @return 
+ */
+int8_t MOTOR_LK_Relax(MOTOR_LK_Param_t *param);
+
+/**
+ * @brief 使电机离线（设置在线状态为false）
+ * @param param 
+ * @return 
+ */
+int8_t MOTOR_LK_Offine(MOTOR_LK_Param_t *param);
+
+/**
+ * @brief 
+ * @param  
+ * @return 
+ */
+int8_t MOTOR_LK_UpdateAll(void);
+
+#ifdef __cplusplus
+}
+#endif

User/device/motor_rm.c

@@ -56,12 +56,12 @@ static int8_t MOTOR_RM_GetLogicalIndex(uint16_t can_id, MOTOR_RM_Module_t module
     switch (module) {
         case MOTOR_M2006:
         case MOTOR_M3508:
-            if (can_id >= M3508_M2006_FB_ID_BASE && can_id < M3508_M2006_FB_ID_BASE + 7) {
+            if (can_id >= M3508_M2006_FB_ID_BASE && can_id <= M3508_M2006_FB_ID_BASE + 7) {
                 return can_id - M3508_M2006_FB_ID_BASE;
             }
             break;
         case MOTOR_GM6020:
-            if (can_id >= GM6020_FB_ID_BASE && can_id < GM6020_FB_ID_BASE + 6) {
+            if (can_id >= GM6020_FB_ID_BASE && can_id <= GM6020_FB_ID_BASE + 6) {
                 return can_id - GM6020_FB_ID_BASE + 4;
             }
             break;

User/module/config.c

@@ -222,37 +222,6 @@ Config_RobotParam_t robot_config = {
 						
 					}       
 		},
-	.telescoping={
-		.telescoping_motor={BSP_CAN_1,0x201,MOTOR_M2006,false,false},
-		.circle=-1000.0,//负数是顺时针
-		.pid={
-		.Telescoping_velocity={
-            .k=1.0f,
-            .p=1.0f,
-            .i=0.0f,
-            .d=0.0f,
-            .i_limit=0.2f,
-            .out_limit=1.0f,
-            .d_cutoff_freq=-1.0f,
-            .range=-1.0f,
-		},
-		.Telescoping_angle={
-            .k=1.0f,//空载时pid参数超调
-            .p=25.0f,
-            .i=0.3f,
-            .d=2.0f,//1.5
-            .i_limit=0.4f,
-            .out_limit=20000.0f,
-            .d_cutoff_freq=-1.0f,
-            .range=0,		
-		},
-	},
-		
-	.low_pass_cutoff_freq={
-	.out=30.0f,
-	},	
-		
-	},
 	
     .shoot_param = {
         .basic={

User/module/config.h

@@ -16,13 +16,11 @@ extern "C" {
 #include "shoot.h"
 #include "module/chassis.h"
 #include "module/cmd/cmd.h"
-#include "telescoping_gimal.h"
 
 typedef struct {
     Gimbal_Params_t gimbal_param;
     Shoot_Params_t shoot_param;
 		Chassis_Param_t	chassis;
-		Telescoping_Params_t telescoping;
 		CMD_Config_t cmd_param;
 } Config_RobotParam_t;
 

User/module/step_motor.c

@@ -0,0 +1,22 @@
+/* 底盘固定模组,用步进 */
+#include "main.h"
+#include "step_motor.h"
+#include "bsp/gpio.h"
+#include "cmsis_os2.h"     
+
+int8_t Step_Motor_Ctrl(int pulse,float time){
+
+	/* 控制方向 */
+	BSP_GPIO_WritePin(BSP_GPIO_STEP__DIRECTION, true);
+	osDelay(1); // 方向稳定时间
+	
+	for(int i;i >= pulse;i++){
+	BSP_GPIO_WritePin(BSP_GPIO_STEP_MOTOR,true);
+	osDelay(time);
+	BSP_GPIO_WritePin(BSP_GPIO_STEP_MOTOR,false);	
+	osDelay(time);
+	}
+	return 0;
+}
+
+

User/module/step_motor.h

@@ -0,0 +1,25 @@
+/*
+ * 云台模组
+ */
+
+#pragma once
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/* Includes ----------------------------------------------------------------- */
+
+
+/* Exported constants ------------------------------------------------------- */
+
+
+/* Exported macro ----------------------------------------------------------- */
+/* Exported types ----------------------------------------------------------- */
+
+
+
+#ifdef __cplusplus
+}
+#endif
+

User/module/telecoping_gimbal.c

@@ -1,172 +0,0 @@
-
-/*
- * 伸缩云台模组
- 模式自行选择：遥控器改变或自行初始化
- delta获取从遥控器自行获取
- */
-
-/* Includes ----------------------------------------------------------------- */
-#include "telescoping_gimal.h"
-#include "gimbal.h"
-#include "bsp/can.h"
-#include "bsp/time.h"
-#include "component/filter.h"
-#include "component/pid.h"
-#include "device/motor_rm.h"
-#include "device/motor_dm.h"
-/* Private typedef ---------------------------------------------------------- */
-/* Private define ----------------------------------------------------------- */
-/* Private macro ------------------------------------------------------------ */
-/* Private variables -------------------------------------------------------- */
-/* Private function  -------------------------------------------------------- */
-float motor_add_angle(float current_angle){
-	static int cirle;
-	static float prev_angle;
-	float delta = current_angle - prev_angle;
-	prev_angle=current_angle;
-	if(delta>M_PI){
-		cirle-=1;
-	}else if(delta<-M_PI){
-			cirle+=1;
-	}	
-		current_angle+=cirle*M_2PI;
-
-	return current_angle;
-}
-
-
-/**
- * \brief 设置伸缩模式
- *
- * \param c 包含云台数据的结构体
- * \param mode 要设置的模式
- *
- * \return 函数运行结果
- */
-static int8_t Telescoping_SetMode(Telescoping_t *t, Telescoping_Mode_t mode) {
-    if (t == NULL)
-		return -1;
-	  if (mode == t->mode)
-    return TELESCOPING_OK;
-
-  PID_Reset(&t->pid.telescoping_angle);
-  PID_Reset(&t->pid.telescoping_velocity);
-
-  LowPassFilter2p_Reset(&t->filter_out.telescoping, 0.0f);
-		
-  AHRS_ResetEulr(&(t->setpoint.telescoping)); /* 切换模式后重置设定值 */
-	
-	return 0;
-}
-
-/* Exported functions ------------------------------------------------------- */
-
-/**
-* \brief 初始化伸缩电机
- *
- * \param g 包含伸缩电机数据的结构体
- * \param param 包含伸缩电机参数的结构体指针
- * \param target_freq 任务预期的运行频率
- *
- * \return 函数运行结果
- */
-int8_t Telescoping_Init(Telescoping_t *t,Telescoping_Params_t *param,
-                   float target_freq) {
-  if (t == NULL)
-    return -1;
-	t->param = param;
-  t->mode = TELESCOPING_MODE_CLOCKWISE; /* 设置默认模式 */
-	
-  /* 初始化云台电机控制PID和LPF */
-  PID_Init(&(t->pid.telescoping_angle), KPID_MODE_CALC_D, target_freq,
-           &(t->param->pid.Telescoping_angle));
-	
-  PID_Init(&(t->pid.telescoping_velocity), KPID_MODE_CALC_D, target_freq,
-           &(t->param->pid.Telescoping_velocity));	
-	
-	  LowPassFilter2p_Init(&t->filter_out.telescoping, target_freq,
-                       t->param->low_pass_cutoff_freq.out);
-	
-	  BSP_CAN_Init();
-		MOTOR_RM_Register(&(t->param->telescoping_motor));
-
-  return 0;
-}
-
-/**
- * \brief 通过CAN设备更新云台反馈信息
- *
- * \param gimbal 云台
- * \param can CAN设备
- *
- * \return 函数运行结果
- */
-int8_t Telescoping_UpdateFeedback(Telescoping_t *telescoping) {
-  if (telescoping == NULL)
-    return -1;
-	
-		MOTOR_RM_Update(&(telescoping->param->telescoping_motor));
-		MOTOR_RM_t *telescoping_motor_yaw = MOTOR_RM_GetMotor(&(telescoping->param->telescoping_motor));
-		telescoping->feedback.motor.telescoping = telescoping_motor_yaw->feedback;
-		telescoping->feedback.motor.gearbox_total_angle=motor_add_angle(telescoping->feedback.motor.telescoping.rotor_abs_angle);
-  return 0;
-}
-
-
-/**
-
- *
-* \param t 包含伸缩电机数据的结构体
- * \param t_cmd 伸缩电机控制指令
- *
- * \return 函数运行结果
- */
-
-int8_t Telescoping_Control(Telescoping_t *t, Telescoping_CMD_t *t_cmd) {
-  if (t == NULL || t_cmd == NULL) {
-    return -1;
-  }
-  t->dt = (BSP_TIME_Get_us() - t->lask_wakeup) / 1000000.0f;
-  t->lask_wakeup = BSP_TIME_Get_us();
-//  Telescoping_SetMode(t, t_cmd->mode);
-//		  switch (t->mode){
-//			case TELESCOPING_MODE_RELAX:			
-//				t->out.telescoping=0.0f;
-//			break;
-//			case TELESCOPING_MODE_CLOCKWISE:
-//				/*顺时针*/
-//				t->setpoint.telescoping=t->param->circle*M_2PI;
-//			break;			
-//			case TELESCOPING_MODE_ANTI_CLOCKWISE:
-//				t->setpoint.telescoping=(-t->param->circle)*M_2PI;				
-//				break;		
-//		}
-   /* 控制相关逻辑 */
-//	t->setpoint.telescoping=t->param->circle*M_2PI;
-//  float telescoping_t_set_point;
-//	
-//	telescoping_t_set_point = PID_Calc(&(t->pid.telescoping_angle), t->setpoint.telescoping,
-//                          t->feedback.motor.gearbox_total_angle, 0.0f, t->dt);
-//  t->out.telescoping = PID_Calc(&(t->pid.telescoping_velocity), telescoping_t_set_point,
-//                          t->feedback.motor.telescoping.rotor_speed, 0.f, t->dt);
-
-		t->out.telescoping=t_cmd->out;
-
-	    /* 输出滤波 */
-//    t->out.telescoping = LowPassFilter2p_Apply(&t->filter_out.telescoping, t->out.telescoping);
-}
-
-
-void Telescoping_Output(Telescoping_t *t){
-
-	
-//		t->out.telescoping=out;
-	MOTOR_RM_Ctrl(&t->param->telescoping_motor);
-	MOTOR_RM_SetOutput(&t->param->telescoping_motor, t->out.telescoping);
-}
-
-
-
-
-
-

User/module/telescoping_gimal.h

@@ -1,110 +0,0 @@
-/*
- * 伸缩模组
- */
-
-#pragma once
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-/* Includes ----------------------------------------------------------------- */
-#include "component/ahrs.h"
-#include "component/filter.h"
-#include "component/pid.h"
-#include "device/motor.h"
-#include "device/motor_dm.h"
-#include "device/motor_rm.h"
-/* Exported constants ------------------------------------------------------- */
-#define TELESCOPING_OK (0)        /* 运行正常 */
-#define TELESCOPING_ERR (-1)      /* 运行时发现了其他错误 */
-#define TELESCOPING_ERR_NULL (-2) /* 运行时发现NULL指针 */
-#define TELESCOPING_ERR_MODE (-3) /* 运行时配置了错误的Telescoping_CMD_t */
-/* Exported macro ----------------------------------------------------------- */
-/* Exported types ----------------------------------------------------------- */
-typedef enum {
-  TELESCOPING_MODE_RELAX,    /* 放松模式，电机不输出。 */
-  TELESCOPING_MODE_CLOCKWISE, /*顺时针模式  */
-  TELESCOPING_MODE_ANTI_CLOCKWISE, /*逆时针模式 */
-} Telescoping_Mode_t;
-
-typedef struct {
-  Telescoping_Mode_t mode;
-  float angle;
-	float out;
-} Telescoping_CMD_t;
-
-/* 云台参数的结构体，包含所有初始化用的参数，通常是const，存好几组。*/
-typedef struct {
-	
-	float circle;
-	
-	MOTOR_RM_Param_t telescoping_motor;
-  struct {
-		KPID_Params_t Telescoping_velocity;
-    KPID_Params_t Telescoping_angle; 		
-  } pid;
-
-  /* 低通滤波器截止频率 */
-  struct {
-    float out;  /* 电机输出 */
-  } low_pass_cutoff_freq;
-
-} Telescoping_Params_t;
-
-
-
-typedef struct {
-  struct {
-    MOTOR_Feedback_t telescoping; /*电机反馈 */
-		float gearbox_total_angle;
-  } motor;
-} Telescoping_Feedback_t;
-
-typedef struct {
-  float telescoping;
-
-} Telescoping_Output_t;
-
-typedef struct {
-  uint64_t lask_wakeup;
-  float dt;
-
-	
-  Telescoping_Params_t *param; /* Init设定 */
-
-  /* 模块通用 */
-  Telescoping_Mode_t mode; /* 模式 */
-
-  /* PID计算的目标值 */
-  struct {
-		float telescoping;
-  } setpoint;
-
-  struct {
-		KPID_t telescoping_velocity; /* 电机速度环PID参数 */
-    KPID_t telescoping_angle; 		/* 电机位置环PID参数 */
-  } pid;
-
-	
-  struct {
-    LowPassFilter2p_t telescoping;
-  } filter_out;
-
-  Telescoping_Output_t out;        /* 云台输出 */
-  Telescoping_Feedback_t feedback; /* 反馈 */
-
-} Telescoping_t;
-
-int8_t Telescoping_Init(Telescoping_t *t,Telescoping_Params_t *param,
-                   float target_freq);
-
-int8_t Telescoping_UpdateFeedback(Telescoping_t *telescoping);
-
-int8_t Telescoping_Control(Telescoping_t *t, Telescoping_CMD_t *t_cmd);
-
-void Telescoping_Output(Telescoping_t *t);
-
-#ifdef __cplusplus
-}
-#endif

User/task/ET16s.c

@@ -0,0 +1,51 @@
+/*
+    ET16s Task
+    
+*/
+
+/* Includes ----------------------------------------------------------------- */
+#include "task/user_task.h"
+/* USER INCLUDE BEGIN */
+#include "ET16s.h"
+/* USER INCLUDE END */
+
+/* Private typedef ---------------------------------------------------------- */
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private variables -------------------------------------------------------- */
+/* USER STRUCT BEGIN */
+ET16s_t et16s;
+/* USER STRUCT END */
+
+/* Private function --------------------------------------------------------- */
+/* Exported functions ------------------------------------------------------- */
+void Task_ET16s(void *argument) {
+  (void)argument; /* 未使用argument，消除警告 */
+
+  
+  /* 计算任务运行到指定频率需要等待的tick数 */
+  const uint32_t delay_tick = osKernelGetTickFreq() / ET16S_FREQ;
+
+  osDelay(ET16S_INIT_DELAY); /* 延时一段时间再开启任务 */
+
+  uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
+  /* USER CODE INIT BEGIN */
+	REMOTE_Init();
+  /* USER CODE INIT END */
+  
+  while (1) {
+    tick += delay_tick; /* 计算下一个唤醒时刻 */
+    /* USER CODE BEGIN */
+    REMOTE_StartDmaRecv();
+    if (REMOTE_WaitDmaCplt(20)) {
+      ET16S_ParseRC(&et16s);
+    } else {
+      ET16s_HandleOffline(&et16s);
+    }
+		osMessageQueueReset(task_runtime.msgq.rc.et16s);
+    osMessageQueuePut(task_runtime.msgq.rc.et16s, &et16s, 0, 0);		
+    /* USER CODE END */
+    osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
+  }
+  
+}

User/task/cmd.c

@@ -6,6 +6,7 @@
 /* Includes ----------------------------------------------------------------- */
 #include "task/user_task.h"
 /* USER INCLUDE BEGIN */
+#include "device/ET16s.h"
 #include "device/dr16.h"
 #include "module/config.h"
 #include "module/cmd/cmd.h"
@@ -22,6 +23,7 @@
 /* USER STRUCT BEGIN */
 #if CMD_RCTypeTable_Index == 0
 DR16_t cmd_dr16;
+ET16s_t cmd_et16s;
 #elif CMD_RCTypeTable_Index == 1
 AT9S_t cmd_at9s;
 #endif
@@ -52,6 +54,7 @@ void Task_cmd(void *argument) {
     /* USER CODE BEGIN */
     #if CMD_RCTypeTable_Index == 0
     osMessageQueueGet(task_runtime.msgq.rc.dr16, &cmd_dr16, NULL, 0);
+		osMessageQueueGet(task_runtime.msgq.rc.et16s, &cmd_et16s, NULL, 0);
     #elif CMD_RCTypeTable_Index == 1
     osMessageQueueGet(task_runtime.msgq.cmd.rc, &cmd_at9s, NULL, 0);
     #endif		

User/task/config.yaml

@@ -26,13 +26,6 @@
   function: Task_shoot_ctrl
   name: shoot_ctrl
   stack: 256
-- delay: 0
-  description: ''
-  freq_control: true
-  frequency: 500.0
-  function: Task_telecoping
-  name: telecoping
-  stack: 256
 - delay: 0
   description: ''
   freq_control: true
@@ -54,3 +47,17 @@
   function: Task_cmd
   name: cmd
   stack: 256
+- delay: 0
+  description: ''
+  freq_control: true
+  frequency: 500.0
+  function: Task_step_motor
+  name: step_motor
+  stack: 256
+- delay: 0
+  description: ''
+  freq_control: true
+  frequency: 500.0
+  function: Task_ET16s
+  name: ET16s
+  stack: 256

User/task/dr16.c

@@ -6,7 +6,7 @@
 /* Includes ----------------------------------------------------------------- */
 #include "task/user_task.h"
 /* USER INCLUDE BEGIN */
-
+#include "device/dr16.h"
 /* USER INCLUDE END */
 
 /* Private typedef ---------------------------------------------------------- */
@@ -14,7 +14,7 @@
 /* Private macro ------------------------------------------------------------ */
 /* Private variables -------------------------------------------------------- */
 /* USER STRUCT BEGIN */
-
+DR16_t dr16;
 /* USER STRUCT END */
 
 /* Private function --------------------------------------------------------- */
@@ -30,13 +30,20 @@ void Task_dr16(void *argument) {
 
   uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
   /* USER CODE INIT BEGIN */
-
+  DR16_Init(&dr16);
   /* USER CODE INIT END */
   
   while (1) {
     tick += delay_tick; /* 计算下一个唤醒时刻 */
     /* USER CODE BEGIN */
-
+//    DR16_StartDmaRecv(&dr16);
+//    if (DR16_WaitDmaCplt(20)) {
+//      DR16_ParseData(&dr16);
+//    } else {
+//      DR16_Offline(&dr16);
+//    }
+    osMessageQueueReset(task_runtime.msgq.rc.dr16);
+    osMessageQueuePut(task_runtime.msgq.rc.dr16, &dr16, 0, 0);		
     /* USER CODE END */
     osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
   }

User/task/init.c

@@ -10,7 +10,7 @@
 #include "module/gimbal.h"
 #include "module/chassis.h"
 #include "device/dr16.h"
-#include "telescoping_gimal.h"
+#include "device/et16s.h"
 /* USER INCLUDE END */
 
 /* Private typedef ---------------------------------------------------------- */
@@ -37,10 +37,11 @@ void Task_Init(void *argument) {
   task_runtime.thread.gimbal_ctrl = osThreadNew(Task_gimbal_ctrl, NULL, &attr_gimbal_ctrl);
   task_runtime.thread.chassis_ctrl = osThreadNew(Task_chassis_ctrl, NULL, &attr_chassis_ctrl);
   task_runtime.thread.shoot_ctrl = osThreadNew(Task_shoot_ctrl, NULL, &attr_shoot_ctrl);
-  task_runtime.thread.telecoping = osThreadNew(Task_telecoping, NULL, &attr_telecoping);
   task_runtime.thread.atti_esti = osThreadNew(Task_atti_esti, NULL, &attr_atti_esti);
   task_runtime.thread.dr16 = osThreadNew(Task_dr16, NULL, &attr_dr16);
   task_runtime.thread.cmd = osThreadNew(Task_cmd, NULL, &attr_cmd);
+  task_runtime.thread.step_motor = osThreadNew(Task_step_motor, NULL, &attr_step_motor);
+  task_runtime.thread.ET16s = osThreadNew(Task_ET16s, NULL, &attr_ET16s);
 
   // 创建消息队列
   /* USER MESSAGE BEGIN */
@@ -49,8 +50,8 @@ void Task_Init(void *argument) {
   task_runtime.msgq.gimbal.cmd= osMessageQueueNew(2u, sizeof(Gimbal_CMD_t), NULL);
 	task_runtime.msgq.chassis.cmd= osMessageQueueNew(2u, sizeof(Chassis_CMD_t), NULL);	
 	task_runtime.msgq.chassis.state= osMessageQueueNew(2u, sizeof(Chassis_t), NULL);	
-	task_runtime.msgq.telescoping.cmd= osMessageQueueNew(2u, sizeof(Telescoping_CMD_t), NULL);	
 	task_runtime.msgq.rc.dr16= osMessageQueueNew(2u, sizeof(DR16_t), NULL);
+	task_runtime.msgq.rc.et16s= osMessageQueueNew(2u, sizeof(ET16s_t), NULL);	
   /* USER MESSAGE END */
 
   osKernelUnlock(); // 解锁内核

User/task/step_motor.c

@@ -0,0 +1,44 @@
+/*
+    step_motor Task
+    
+*/
+
+/* Includes ----------------------------------------------------------------- */
+#include "task/user_task.h"
+/* USER INCLUDE BEGIN */
+
+/* USER INCLUDE END */
+
+/* Private typedef ---------------------------------------------------------- */
+/* Private define ----------------------------------------------------------- */
+/* Private macro ------------------------------------------------------------ */
+/* Private variables -------------------------------------------------------- */
+/* USER STRUCT BEGIN */
+
+/* USER STRUCT END */
+
+/* Private function --------------------------------------------------------- */
+/* Exported functions ------------------------------------------------------- */
+void Task_step_motor(void *argument) {
+  (void)argument; /* 未使用argument，消除警告 */
+
+  
+  /* 计算任务运行到指定频率需要等待的tick数 */
+  const uint32_t delay_tick = osKernelGetTickFreq() / STEP_MOTOR_FREQ;
+
+  osDelay(STEP_MOTOR_INIT_DELAY); /* 延时一段时间再开启任务 */
+
+  uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
+  /* USER CODE INIT BEGIN */
+
+  /* USER CODE INIT END */
+  
+  while (1) {
+    tick += delay_tick; /* 计算下一个唤醒时刻 */
+    /* USER CODE BEGIN */
+
+    /* USER CODE END */
+    osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
+  }
+  
+}

User/task/user_task.c

@@ -29,11 +29,6 @@ const osThreadAttr_t attr_shoot_ctrl = {
     .priority = osPriorityNormal,
     .stack_size = 256 * 4,
 };
-const osThreadAttr_t attr_telecoping = {
-    .name = "telecoping",
-    .priority = osPriorityNormal,
-    .stack_size = 256 * 4,
-};
 const osThreadAttr_t attr_atti_esti = {
     .name = "atti_esti",
     .priority = osPriorityNormal,
@@ -49,3 +44,13 @@ const osThreadAttr_t attr_cmd = {
     .priority = osPriorityNormal,
     .stack_size = 256 * 4,
 };
+const osThreadAttr_t attr_step_motor = {
+    .name = "step_motor",
+    .priority = osPriorityNormal,
+    .stack_size = 256 * 4,
+};
+const osThreadAttr_t attr_ET16s = {
+    .name = "ET16s",
+    .priority = osPriorityNormal,
+    .stack_size = 256 * 4,
+};

User/task/user_task.h

@@ -17,10 +17,11 @@ extern "C" {
 #define GIMBAL_CTRL_FREQ (500.0)
 #define CHASSIS_CTRL_FREQ (500.0)
 #define SHOOT_CTRL_FREQ (500.0)
-#define TELECOPING_FREQ (500.0)
 #define ATTI_ESTI_FREQ (500.0)
 #define DR16_FREQ (500.0)
 #define CMD_FREQ (500.0)
+#define STEP_MOTOR_FREQ (500.0)
+#define ET16S_FREQ (500.0)
 
 /* 任务初始化延时ms */
 #define TASK_INIT_DELAY (100u)
@@ -28,10 +29,11 @@ extern "C" {
 #define GIMBAL_CTRL_INIT_DELAY (0)
 #define CHASSIS_CTRL_INIT_DELAY (0)
 #define SHOOT_CTRL_INIT_DELAY (0)
-#define TELECOPING_INIT_DELAY (0)
 #define ATTI_ESTI_INIT_DELAY (0)
 #define DR16_INIT_DELAY (0)
 #define CMD_INIT_DELAY (0)
+#define STEP_MOTOR_INIT_DELAY (0)
+#define ET16S_INIT_DELAY (0)
 
 /* Exported defines --------------------------------------------------------- */
 /* Exported macro ----------------------------------------------------------- */
@@ -45,10 +47,11 @@ typedef struct {
         osThreadId_t gimbal_ctrl;
         osThreadId_t chassis_ctrl;
         osThreadId_t shoot_ctrl;
-        osThreadId_t telecoping;
         osThreadId_t atti_esti;
         osThreadId_t dr16;
         osThreadId_t cmd;
+        osThreadId_t step_motor;
+        osThreadId_t ET16s;
     } thread;
 
     /* USER MESSAGE BEGIN */
@@ -79,6 +82,7 @@ typedef struct {
         struct {
             osMessageQueueId_t state;
             osMessageQueueId_t dr16;
+            osMessageQueueId_t et16s;					
         }rc;			
     } msgq;
     /* USER MESSAGE END */
@@ -100,10 +104,11 @@ typedef struct {
         UBaseType_t gimbal_ctrl;
         UBaseType_t chassis_ctrl;
         UBaseType_t shoot_ctrl;
-        UBaseType_t telecoping;
         UBaseType_t atti_esti;
         UBaseType_t dr16;
         UBaseType_t cmd;
+        UBaseType_t step_motor;
+        UBaseType_t ET16s;
     } stack_water_mark;
 
     /* 各任务运行频率 */
@@ -112,10 +117,11 @@ typedef struct {
         float gimbal_ctrl;
         float chassis_ctrl;
         float shoot_ctrl;
-        float telecoping;
         float atti_esti;
         float dr16;
         float cmd;
+        float step_motor;
+        float ET16s;
     } freq;
 
     /* 任务最近运行时间 */
@@ -124,10 +130,11 @@ typedef struct {
         float gimbal_ctrl;
         float chassis_ctrl;
         float shoot_ctrl;
-        float telecoping;
         float atti_esti;
         float dr16;
         float cmd;
+        float step_motor;
+        float ET16s;
     } last_up_time;
 
 } Task_Runtime_t;
@@ -141,10 +148,11 @@ extern const osThreadAttr_t attr_ai;
 extern const osThreadAttr_t attr_gimbal_ctrl;
 extern const osThreadAttr_t attr_chassis_ctrl;
 extern const osThreadAttr_t attr_shoot_ctrl;
-extern const osThreadAttr_t attr_telecoping;
 extern const osThreadAttr_t attr_atti_esti;
 extern const osThreadAttr_t attr_dr16;
 extern const osThreadAttr_t attr_cmd;
+extern const osThreadAttr_t attr_step_motor;
+extern const osThreadAttr_t attr_ET16s;
 
 /* 任务函数声明 */
 void Task_Init(void *argument);
@@ -152,10 +160,11 @@ void Task_ai(void *argument);
 void Task_gimbal_ctrl(void *argument);
 void Task_chassis_ctrl(void *argument);
 void Task_shoot_ctrl(void *argument);
-void Task_telecoping(void *argument);
 void Task_atti_esti(void *argument);
 void Task_dr16(void *argument);
 void Task_cmd(void *argument);
+void Task_step_motor(void *argument);
+void Task_ET16s(void *argument);
 
 #ifdef __cplusplus
 }