yunhai/Steering_Wheel_Infatry
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

添加ET16s遥控器

Browse Source
This commit is contained in:
yunhai8432 2026-01-21 21:35:01 +08:00
parent 8121922102
commit 4cd28e17ca
80 changed files with 19074 additions and 9400 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
Core/Inc/main.h
View File

@ -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

2
Core/Src/freertos.c
View File

@ -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 */

20
Core/Src/gpio.c
View File

@ -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;

4
Core/Src/usart.c
View File

@ -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;

7501
MDK-ARM/JLinkLog.txt Normal file
View File

File diff suppressed because it is too large Load Diff

45
MDK-ARM/JLinkSettings.ini Normal file
View File

@ -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

563
MDK-ARM/Steering Wheel_Infatry.uvguix.yunha
View File

File diff suppressed because one or more lines are too long

119
MDK-ARM/Steering Wheel_Infatry.uvoptx
View File

@ -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>
</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>
<ItemText>cmd</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>
<FilenameWithoutPath>remote_control.c</FilenameWithoutPath>
<PathWithFileName>..\User\device\ai.c</PathWithFileName>
<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>
<FilenameWithoutPath>ai.c</FilenameWithoutPath>
<PathWithFileName>..\User\device\ET16s.c</PathWithFileName>
<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>
<FilenameWithoutPath>telecoping.c</FilenameWithoutPath>
<PathWithFileName>..\User\task\step_motor.c</PathWithFileName>
<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>

36
MDK-ARM/Steering Wheel_Infatry.uvprojx
View File

@ -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>

BIN
MDK-ARM/Steering Wheel_Infatry/Steering Wheel_Infatry.axf
View File

Binary file not shown.

10
MDK-ARM/Steering Wheel_Infatry/Steering Wheel_Infatry.build_log.htm
View File

@ -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>

7083
MDK-ARM/Steering Wheel_Infatry/Steering Wheel_Infatry.hex
View File

File diff suppressed because it is too large Load Diff

4987
MDK-ARM/Steering Wheel_Infatry/Steering Wheel_Infatry.htm
View File

File diff suppressed because it is too large Load Diff

8
MDK-ARM/Steering Wheel_Infatry/Steering Wheel_Infatry.lnp
View File

@ -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"

4554
MDK-ARM/Steering Wheel_Infatry/Steering Wheel_Infatry.map
View File

File diff suppressed because it is too large Load Diff

1110
MDK-ARM/Steering Wheel_Infatry/Steering Wheel_Infatry_Steering Wheel_Infatry.dep
View File

File diff suppressed because it is too large Load Diff

BIN
MDK-ARM/Steering Wheel_Infatry/atti_esti.o
View File

Binary file not shown.

BIN
MDK-ARM/Steering Wheel_Infatry/bmi088.o
View File

Binary file not shown.

3
MDK-ARM/Steering Wheel_Infatry/chassis_ctrl.d
View File

@ -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\telescoping_gimal.h
..\User\module\gimbal.h ..\User\module\shoot.h

25
MDK-ARM/Steering Wheel_Infatry/cmd.d
View File

@ -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\stdbool.h \
..\User\device\device.h ..\User\module\config.h \
..\User\module\gimbal.h ..\User\component\ahrs.h \
..\User\component\user_math.h ..\User\component\filter.h \
..\User\component\pid.h ..\User\component\filter.h \
..\User\device\motor.h ..\User\device\device.h \
..\User\device\motor_dm.h ..\User\bsp\can.h ..\Core\Inc\can.h \
..\Core\Inc\main.h ..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal.h \
D:\Keil_v5\ARM\ARMCLANG\Bin\..\include\math.h ..\User\device\device.h \
..\User\module\config.h ..\User\module\gimbal.h \
..\User\component\ahrs.h ..\User\component\user_math.h \
..\User\component\filter.h ..\User\component\pid.h \
..\User\component\filter.h ..\User\device\motor.h \
..\User\device\device.h ..\User\device\motor_dm.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 \
@ -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_behavior.h ..\User\module\cmd\cmd_types.h
..\User\module\cmd\cmd_adapter.h ..\User\module\cmd\cmd_behavior.h \
..\User\module\cmd\cmd_types.h

BIN
MDK-ARM/Steering Wheel_Infatry/cmd.o
View File

Binary file not shown.

2
MDK-ARM/Steering Wheel_Infatry/config.d
View File

@ -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

BIN
MDK-ARM/Steering Wheel_Infatry/config.o
View File

Binary file not shown.

7
MDK-ARM/Steering Wheel_Infatry/dr16_1.d
View File

@ -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

BIN
MDK-ARM/Steering Wheel_Infatry/dr16_1.o
View File

Binary file not shown.

43
MDK-ARM/Steering Wheel_Infatry/et16s.d Normal file
View File

@ -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

BIN
MDK-ARM/Steering Wheel_Infatry/et16s.o Normal file
View File

Binary file not shown.

16
MDK-ARM/Steering Wheel_Infatry/et16s_1.d Normal file
View File

@ -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

BIN
MDK-ARM/Steering Wheel_Infatry/et16s_1.o Normal file
View File

Binary file not shown.

2
MDK-ARM/Steering Wheel_Infatry/fixation.d Normal file
View File

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

BIN
MDK-ARM/Steering Wheel_Infatry/fixation.o Normal file
View File

Binary file not shown.

2
MDK-ARM/Steering Wheel_Infatry/gimbal_ctrl.d
View File

@ -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

BIN
MDK-ARM/Steering Wheel_Infatry/gimbal_ctrl.o
View File

Binary file not shown.

BIN
MDK-ARM/Steering Wheel_Infatry/gpio.o
View File

Binary file not shown.

BIN
MDK-ARM/Steering Wheel_Infatry/gpio_1.o
View File

Binary file not shown.

3
MDK-ARM/Steering Wheel_Infatry/init.d
View File

@ -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\module\telescoping_gimal.h
..\User\device\dr16.h ..\User\device\device.h ..\User\device\et16s.h

BIN
MDK-ARM/Steering Wheel_Infatry/init.o
View File

Binary file not shown.

BIN
MDK-ARM/Steering Wheel_Infatry/ist8310.o
View File

Binary file not shown.

BIN
MDK-ARM/Steering Wheel_Infatry/motor_dm.o
View File

Binary file not shown.

54
MDK-ARM/Steering Wheel_Infatry/motor_lk.d Normal file
View File

@ -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

BIN
MDK-ARM/Steering Wheel_Infatry/motor_lk.o Normal file
View File

Binary file not shown.

BIN
MDK-ARM/Steering Wheel_Infatry/motor_rm.o
View File

Binary file not shown.

2
MDK-ARM/Steering Wheel_Infatry/shoot_ctrl.d
View File

@ -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

BIN
MDK-ARM/Steering Wheel_Infatry/shoot_ctrl.o
View File

Binary file not shown.

BIN
MDK-ARM/Steering Wheel_Infatry/startup_stm32f407xx.o
View File

Binary file not shown.

38
MDK-ARM/Steering Wheel_Infatry/step_motor.d Normal file
View File

@ -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

BIN
MDK-ARM/Steering Wheel_Infatry/step_motor.o Normal file
View File

Binary file not shown.

14
MDK-ARM/Steering Wheel_Infatry/step_motor_1.d Normal file
View File

@ -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

BIN
MDK-ARM/Steering Wheel_Infatry/step_motor_1.o Normal file
View File

Binary file not shown.

BIN
MDK-ARM/Steering Wheel_Infatry/usart.o
View File

Binary file not shown.

BIN
MDK-ARM/Steering Wheel_Infatry/user_task.o
View File

Binary file not shown.

47
Steering Wheel_Infatry.ioc
View File

@ -123,24 +123,26 @@ Mcu.Pin22=PF1
Mcu.Pin23=PG6
Mcu.Pin24=PF6
Mcu.Pin25=PG3
Mcu.Pin26=PA0-WKUP
Mcu.Pin27=PA4
Mcu.Pin28=PC4
Mcu.Pin29=PC5
Mcu.Pin26=PC1
Mcu.Pin27=PA0-WKUP
Mcu.Pin28=PA4
Mcu.Pin29=PC4
Mcu.Pin3=PB3
Mcu.Pin30=PB12
Mcu.Pin31=PA7
Mcu.Pin32=PB0
Mcu.Pin33=VP_FREERTOS_VS_CMSIS_V2
Mcu.Pin34=VP_SYS_VS_Systick
Mcu.Pin35=VP_TIM10_VS_ClockSourceINT
Mcu.Pin30=PC5
Mcu.Pin31=PB12
Mcu.Pin32=PA7
Mcu.Pin33=PB1
Mcu.Pin34=PB0
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

10
User/bsp/bsp_config.yaml
View File

@ -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

2
User/bsp/gpio.c
View File

@ -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},

2
User/bsp/gpio.h
View File

@ -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,

417
User/component/ahrs/ahrs.c Normal file
View File

@ -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 */

114
User/component/ahrs/ahrs.h Normal file
View File

@ -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

185
User/component/filter/filter.c Normal file
View File

@ -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);
}

120
User/component/filter/filter.h Normal file
View File

@ -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

181
User/device/ET16s.c Normal file
View File

@ -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;
}

115
User/device/ET16s.h Normal file
View File

@ -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

3
User/device/device_config.yaml
View File

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

26
User/device/motor_dm.c
View File

@ -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);
vel_tmp = float_to_uint(output->velocity, DM_V_MIN , DM_V_MAX, 12);
/* 根据 reverse 参数调整控制值 */
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); // 速度，低位在前

329
User/device/motor_lk.c Normal file
View File

@ -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;
}

119
User/device/motor_lk.h Normal file
View File

@ -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

4
User/device/motor_rm.c
View File

@ -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;

31
User/module/config.c
View File

@ -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={

2
User/module/config.h
View File

@ -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;

22
User/module/step_motor.c Normal file
View File

@ -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;
}

25
User/module/step_motor.h Normal file
View File

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

172
User/module/telecoping_gimbal.c
View File

@ -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);
}

110
User/module/telescoping_gimal.h
View File

@ -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

51
User/task/ET16s.c Normal file
View File

@ -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); /* 运行结束，等待下一次唤醒 */
}
}

3
User/task/cmd.c
View File

@ -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

21
User/task/config.yaml
View File

@ -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

15
User/task/dr16.c
View File

@ -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); /* 运行结束，等待下一次唤醒 */
}

7
User/task/init.c
View File

@ -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(); // 解锁内核

44
User/task/step_motor.c Normal file
View File

@ -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); /* 运行结束，等待下一次唤醒 */
}
}

15
User/task/user_task.c
View File

@ -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,
@ -48,4 +43,14 @@ const osThreadAttr_t attr_cmd = {
.name = "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,
};

25
User/task/user_task.h
View File

@ -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
}