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

使用最新云台，添加了滤波

Browse Source
This commit is contained in:
yunhai8432 2025-12-13 20:22:06 +08:00
parent cc27fe6cf7
commit 44c282cfad
22 changed files with 5312 additions and 5041 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

88
MDK-ARM/.vscode/c_cpp_properties.json vendored Normal file
View File

@ -0,0 +1,88 @@
{
"configurations": [
{
"name": "gimbal",
"includePath": [
"d:\\yunha\\1\\sling\\gimbal\\Core\\Inc",
"d:\\yunha\\1\\sling\\gimbal\\Drivers\\STM32F4xx_HAL_Driver\\Inc",
"d:\\yunha\\1\\sling\\gimbal\\Drivers\\STM32F4xx_HAL_Driver\\Inc\\Legacy",
"d:\\yunha\\1\\sling\\gimbal\\Drivers\\CMSIS\\Device\\ST\\STM32F4xx\\Include",
"d:\\yunha\\1\\sling\\gimbal\\Drivers\\CMSIS\\Include",
"d:\\yunha\\1\\sling\\gimbal\\User\\bsp",
"d:\\yunha\\1\\sling\\gimbal\\User\\component",
"d:\\yunha\\1\\sling\\gimbal\\User\\device",
"d:\\yunha\\1\\sling\\gimbal\\Middlewares\\Third_Party\\FreeRTOS\\Source\\include",
"d:\\yunha\\1\\sling\\gimbal\\Middlewares\\Third_Party\\FreeRTOS\\Source\\CMSIS_RTOS_V2",
"d:\\yunha\\1\\sling\\gimbal\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\RVDS\\ARM_CM4F",
"d:\\yunha\\1\\sling\\gimbal\\User\\task",
"d:\\yunha\\1\\sling\\gimbal\\User",
"d:\\yunha\\1\\sling\\gimbal\\User\\module",
"d:\\yunha\\1\\sling\\gimbal\\MDK-ARM",
"d:\\yunha\\1\\sling\\gimbal\\Core\\Src",
"d:\\yunha\\1\\sling\\gimbal\\Drivers\\STM32F4xx_HAL_Driver\\Src",
"d:\\yunha\\1\\sling\\gimbal\\Middlewares\\Third_Party\\FreeRTOS\\Source",
"d:\\yunha\\1\\sling\\gimbal\\Middlewares\\Third_Party\\FreeRTOS\\Source\\portable\\MemMang"
],
"defines": [
"USE_HAL_DRIVER",
"STM32F407xx",
"__alignof__(x)=",
"__asm(x)=",
"__asm__(x)=",
"__forceinline=",
"__restrict=",
"__volatile__=",
"__inline=",
"__inline__=",
"__declspec(x)=",
"__attribute__(x)=",
"__nonnull__(x)=",
"__unaligned=",
"__promise(x)=",
"__irq=",
"__swi=",
"__weak=",
"__register=",
"__pure=",
"__value_in_regs=",
"__breakpoint(x)=",
"__current_pc()=0U",
"__current_sp()=0U",
"__disable_fiq()=",
"__disable_irq()=",
"__enable_fiq()=",
"__enable_irq()=",
"__force_stores()=",
"__memory_changed()=",
"__schedule_barrier()=",
"__semihost(x,y)=0",
"__vfp_status(x,y)=0",
"__builtin_arm_nop()=",
"__builtin_arm_wfi()=",
"__builtin_arm_wfe()=",
"__builtin_arm_sev()=",
"__builtin_arm_sevl()=",
"__builtin_arm_yield()=",
"__builtin_arm_isb(x)=",
"__builtin_arm_dsb(x)=",
"__builtin_arm_dmb(x)=",
"__builtin_bswap32(x)=0U",
"__builtin_bswap16(x)=0U",
"__builtin_arm_rbit(x)=0U",
"__builtin_clz(x)=0U",
"__builtin_arm_ldrex(x)=0U",
"__builtin_arm_strex(x,y)=0U",
"__builtin_arm_clrex()=",
"__builtin_arm_ssat(x,y)=0U",
"__builtin_arm_usat(x,y)=0U",
"__builtin_arm_ldaex(x)=0U",
"__builtin_arm_stlex(x,y)=0U",
"__GNUC__=4",
"__GNUC_MINOR__=2",
"__GNUC_PATCHLEVEL__=1"
],
"intelliSenseMode": "${default}"
}
],
"version": 4
}

7
MDK-ARM/.vscode/keil-assistant.log vendored Normal file
View File

@ -0,0 +1,7 @@
[info] Log at : 2025/12/13|19:43:32|GMT+0800
[info] Log at : 2025/12/13|19:43:35|GMT+0800
[info] project closed: gimbal
[info] Log at : 2025/12/13|19:48:53|GMT+0800

0
MDK-ARM/.vscode/uv4.log vendored Normal file
View File

1
MDK-ARM/.vscode/uv4.log.lock vendored Normal file
View File

@ -0,0 +1 @@
2025/12/13 19:49:24

111
MDK-ARM/gimbal.uvguix.yunha
View File

File diff suppressed because one or more lines are too long

46
MDK-ARM/gimbal.uvoptx
View File

@ -1067,18 +1067,6 @@
<File> <File>
<GroupNumber>7</GroupNumber> <GroupNumber>7</GroupNumber>
<FileNumber>67</FileNumber> <FileNumber>67</FileNumber>
<FileType>5</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>..\User\component\Sliding.h</PathWithFileName>
<FilenameWithoutPath>Sliding.h</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>7</GroupNumber>
<FileNumber>68</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1098,7 +1086,7 @@
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<File> <File>
<GroupNumber>8</GroupNumber> <GroupNumber>8</GroupNumber>
<FileNumber>69</FileNumber> <FileNumber>68</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1110,7 +1098,7 @@
</File> </File>
<File> <File>
<GroupNumber>8</GroupNumber> <GroupNumber>8</GroupNumber>
<FileNumber>70</FileNumber> <FileNumber>69</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1122,7 +1110,7 @@
</File> </File>
<File> <File>
<GroupNumber>8</GroupNumber> <GroupNumber>8</GroupNumber>
<FileNumber>71</FileNumber> <FileNumber>70</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1134,7 +1122,7 @@
</File> </File>
<File> <File>
<GroupNumber>8</GroupNumber> <GroupNumber>8</GroupNumber>
<FileNumber>72</FileNumber> <FileNumber>71</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1146,7 +1134,7 @@
</File> </File>
<File> <File>
<GroupNumber>8</GroupNumber> <GroupNumber>8</GroupNumber>
<FileNumber>73</FileNumber> <FileNumber>72</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1158,7 +1146,7 @@
</File> </File>
<File> <File>
<GroupNumber>8</GroupNumber> <GroupNumber>8</GroupNumber>
<FileNumber>74</FileNumber> <FileNumber>73</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1170,7 +1158,7 @@
</File> </File>
<File> <File>
<GroupNumber>8</GroupNumber> <GroupNumber>8</GroupNumber>
<FileNumber>75</FileNumber> <FileNumber>74</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1182,7 +1170,7 @@
</File> </File>
<File> <File>
<GroupNumber>8</GroupNumber> <GroupNumber>8</GroupNumber>
<FileNumber>76</FileNumber> <FileNumber>75</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1194,7 +1182,7 @@
</File> </File>
<File> <File>
<GroupNumber>8</GroupNumber> <GroupNumber>8</GroupNumber>
<FileNumber>77</FileNumber> <FileNumber>76</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1214,7 +1202,7 @@
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<File> <File>
<GroupNumber>9</GroupNumber> <GroupNumber>9</GroupNumber>
<FileNumber>78</FileNumber> <FileNumber>77</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1226,7 +1214,7 @@
</File> </File>
<File> <File>
<GroupNumber>9</GroupNumber> <GroupNumber>9</GroupNumber>
<FileNumber>79</FileNumber> <FileNumber>78</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1238,7 +1226,7 @@
</File> </File>
<File> <File>
<GroupNumber>9</GroupNumber> <GroupNumber>9</GroupNumber>
<FileNumber>80</FileNumber> <FileNumber>79</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1250,7 +1238,7 @@
</File> </File>
<File> <File>
<GroupNumber>9</GroupNumber> <GroupNumber>9</GroupNumber>
<FileNumber>81</FileNumber> <FileNumber>80</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1262,7 +1250,7 @@
</File> </File>
<File> <File>
<GroupNumber>9</GroupNumber> <GroupNumber>9</GroupNumber>
<FileNumber>82</FileNumber> <FileNumber>81</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1274,7 +1262,7 @@
</File> </File>
<File> <File>
<GroupNumber>9</GroupNumber> <GroupNumber>9</GroupNumber>
<FileNumber>83</FileNumber> <FileNumber>82</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1294,7 +1282,7 @@
<RteFlg>0</RteFlg> <RteFlg>0</RteFlg>
<File> <File>
<GroupNumber>10</GroupNumber> <GroupNumber>10</GroupNumber>
<FileNumber>84</FileNumber> <FileNumber>83</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>
@ -1306,7 +1294,7 @@
</File> </File>
<File> <File>
<GroupNumber>10</GroupNumber> <GroupNumber>10</GroupNumber>
<FileNumber>85</FileNumber> <FileNumber>84</FileNumber>
<FileType>1</FileType> <FileType>1</FileType>
<tvExp>0</tvExp> <tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg> <tvExpOptDlg>0</tvExpOptDlg>

5
MDK-ARM/gimbal.uvprojx
View File

@ -1986,11 +1986,6 @@
<FileType>1</FileType> <FileType>1</FileType>
<FilePath>..\User\component\Sliding.c</FilePath> <FilePath>..\User\component\Sliding.c</FilePath>
</File> </File>
<File>
<FileName>Sliding.h</FileName>
<FileType>5</FileType>
<FilePath>..\User\component\Sliding.h</FilePath>
</File>
<File> <File>
<FileName>Kalman.c</FileName> <FileName>Kalman.c</FileName>
<FileType>1</FileType> <FileType>1</FileType>

BIN
MDK-ARM/gimbal/ai.o
View File

Binary file not shown.

BIN
MDK-ARM/gimbal/atti_esti.o
View File

Binary file not shown.

BIN
MDK-ARM/gimbal/config.o
View File

Binary file not shown.

BIN
MDK-ARM/gimbal/gimbal.axf
View File

Binary file not shown.

7
MDK-ARM/gimbal/gimbal.build_log.htm
View File

@ -22,7 +22,7 @@ Dialog DLL: TCM.DLL V1.48.0.0
<h2>Project:</h2> <h2>Project:</h2>
D:\yunha\1\sling\gimbal\MDK-ARM\gimbal.uvprojx D:\yunha\1\sling\gimbal\MDK-ARM\gimbal.uvprojx
Project File Date: 12/08/2025 Project File Date: 12/13/2025
<h2>Output:</h2> <h2>Output:</h2>
*** Using Compiler 'V6.16', folder: 'D:\Keil_v5\ARM\ARMCLANG\Bin' *** Using Compiler 'V6.16', folder: 'D:\Keil_v5\ARM\ARMCLANG\Bin'
@ -32,6 +32,9 @@ Note: source file '..\User\bsp\gpio.c' - object file renamed from 'gimbal\gpio.o
Note: source file '..\User\bsp\i2c.c' - object file renamed from 'gimbal\i2c.o' to 'gimbal\i2c_1.o'. Note: source file '..\User\bsp\i2c.c' - object file renamed from 'gimbal\i2c.o' to 'gimbal\i2c_1.o'.
Note: source file '..\User\bsp\spi.c' - object file renamed from 'gimbal\spi.o' to 'gimbal\spi_1.o'. Note: source file '..\User\bsp\spi.c' - object file renamed from 'gimbal\spi.o' to 'gimbal\spi_1.o'.
Note: source file '..\User\task\vofa.c' - object file renamed from 'gimbal\vofa.o' to 'gimbal\vofa_1.o'. Note: source file '..\User\task\vofa.c' - object file renamed from 'gimbal\vofa.o' to 'gimbal\vofa_1.o'.
compiling config.c...
linking...
Program Size: Code=93824 RO-data=1172 RW-data=320 ZI-data=34584
"gimbal\gimbal.axf" - 0 Error(s), 0 Warning(s). "gimbal\gimbal.axf" - 0 Error(s), 0 Warning(s).
<h2>Software Packages used:</h2> <h2>Software Packages used:</h2>
@ -55,7 +58,7 @@ Package Vendor: Keil
<h2>Collection of Component Files used:</h2> <h2>Collection of Component Files used:</h2>
* Component: ARM::CMSIS:CORE:5.4.0 * Component: ARM::CMSIS:CORE:5.4.0
Build Time Elapsed: 00:00:01 Build Time Elapsed: 00:00:02
</pre> </pre>
</body> </body>
</html> </html>

4653
MDK-ARM/gimbal/gimbal.htm
View File

File diff suppressed because it is too large Load Diff

4950
MDK-ARM/gimbal/gimbal.map
View File

File diff suppressed because it is too large Load Diff

BIN
MDK-ARM/gimbal/gimbal.o
View File

Binary file not shown.

BIN
MDK-ARM/gimbal/gimbal_ctrl.o
View File

Binary file not shown.

21
MDK-ARM/gimbal/gimbal_gimbal.dep
View File

@ -1859,7 +1859,6 @@ I (D:\Keil_v5\ARM\ARMCLANG\include\float.h)(0x6035A4A0)
I (D:\Keil_v5\ARM\ARMCLANG\include\math.h)(0x6035A4A8) I (D:\Keil_v5\ARM\ARMCLANG\include\math.h)(0x6035A4A8)
I (D:\Keil_v5\ARM\ARMCLANG\include\stdbool.h)(0x6035A4A8) I (D:\Keil_v5\ARM\ARMCLANG\include\stdbool.h)(0x6035A4A8)
I (D:\Keil_v5\ARM\ARMCLANG\include\stddef.h)(0x6035A4A8) I (D:\Keil_v5\ARM\ARMCLANG\include\stddef.h)(0x6035A4A8)
F (..\User\component\Sliding.h)(0x693D4B05)()
F (..\User\component\Kalman.c)(0x69353AFC)(-xc -std=c99 --target=arm-arm-none-eabi -mcpu=cortex-m4 -mfpu=fpv4-sp-d16 -mfloat-abi=hard -c -fno-rtti -funsigned-char -fshort-enums -fshort-wchar -gdwarf-3 -O0 -ffunction-sections -w -I ../Core/Inc -I ../Drivers/STM32F4xx_HAL_Driver/Inc -I ../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy -I ../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I ../Drivers/CMSIS/Include -I ../User/bsp -I ../User/component -I ../User/device -I ../Middlewares/Third_Party/FreeRTOS/Source/include -I ../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 -I ../Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F -I ../User/task -I ../User -I ../User/module -I./RTE/_gimbal -ID:/Keil_v5/Arm/Packs/ARM/CMSIS/5.7.0/CMSIS/Core/Include -ID:/Keil_v5/Arm/Packs/Keil/STM32F4xx_DFP/2.17.1/Drivers/CMSIS/Device/ST/STM32F4xx/Include -D__UVISION_VERSION="534" -D_RTE_ -DSTM32F407xx -D_RTE_ -DUSE_HAL_DRIVER -DSTM32F407xx -o gimbal/kalman.o -MD) F (..\User\component\Kalman.c)(0x69353AFC)(-xc -std=c99 --target=arm-arm-none-eabi -mcpu=cortex-m4 -mfpu=fpv4-sp-d16 -mfloat-abi=hard -c -fno-rtti -funsigned-char -fshort-enums -fshort-wchar -gdwarf-3 -O0 -ffunction-sections -w -I ../Core/Inc -I ../Drivers/STM32F4xx_HAL_Driver/Inc -I ../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy -I ../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I ../Drivers/CMSIS/Include -I ../User/bsp -I ../User/component -I ../User/device -I ../Middlewares/Third_Party/FreeRTOS/Source/include -I ../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 -I ../Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F -I ../User/task -I ../User -I ../User/module -I./RTE/_gimbal -ID:/Keil_v5/Arm/Packs/ARM/CMSIS/5.7.0/CMSIS/Core/Include -ID:/Keil_v5/Arm/Packs/Keil/STM32F4xx_DFP/2.17.1/Drivers/CMSIS/Device/ST/STM32F4xx/Include -D__UVISION_VERSION="534" -D_RTE_ -DSTM32F407xx -D_RTE_ -DUSE_HAL_DRIVER -DSTM32F407xx -o gimbal/kalman.o -MD)
I (..\User\component\Kalman.h)(0x69353AFC) I (..\User\component\Kalman.h)(0x69353AFC)
I (D:\Keil_v5\ARM\ARMCLANG\include\stdint.h)(0x6035A4A8) I (D:\Keil_v5\ARM\ARMCLANG\include\stdint.h)(0x6035A4A8)
@ -1923,7 +1922,7 @@ I (D:\Keil_v5\ARM\ARMCLANG\include\math.h)(0x6035A4A8)
I (D:\Keil_v5\ARM\ARMCLANG\include\stdbool.h)(0x6035A4A8) I (D:\Keil_v5\ARM\ARMCLANG\include\stdbool.h)(0x6035A4A8)
I (D:\Keil_v5\ARM\ARMCLANG\include\stdint.h)(0x6035A4A8) I (D:\Keil_v5\ARM\ARMCLANG\include\stdint.h)(0x6035A4A8)
I (D:\Keil_v5\ARM\ARMCLANG\include\stddef.h)(0x6035A4A8) I (D:\Keil_v5\ARM\ARMCLANG\include\stddef.h)(0x6035A4A8)
I (..\User\module\gimbal.h)(0x693D3638) I (..\User\module\gimbal.h)(0x693D590B)
I (..\User\component\Sliding.h)(0x693D4B05) I (..\User\component\Sliding.h)(0x693D4B05)
I (D:\Keil_v5\ARM\ARMCLANG\include\string.h)(0x6035A4A8) I (D:\Keil_v5\ARM\ARMCLANG\include\string.h)(0x6035A4A8)
I (..\User\component\ahrs.h)(0x69352AF2) I (..\User\component\ahrs.h)(0x69352AF2)
@ -2323,7 +2322,7 @@ I (..\Middlewares\Third_Party\FreeRTOS\Source\portable\RVDS\ARM_CM4F\portmacro.h
I (..\Middlewares\Third_Party\FreeRTOS\Source\include\mpu_wrappers.h)(0x68B055DB) I (..\Middlewares\Third_Party\FreeRTOS\Source\include\mpu_wrappers.h)(0x68B055DB)
I (..\Middlewares\Third_Party\FreeRTOS\Source\include\task.h)(0x68B055DB) I (..\Middlewares\Third_Party\FreeRTOS\Source\include\task.h)(0x68B055DB)
I (..\Middlewares\Third_Party\FreeRTOS\Source\include\list.h)(0x68B055DB) I (..\Middlewares\Third_Party\FreeRTOS\Source\include\list.h)(0x68B055DB)
I (..\User\module\gimbal.h)(0x693D3638) I (..\User\module\gimbal.h)(0x693D590B)
I (..\User\component\Sliding.h)(0x693D4B05) I (..\User\component\Sliding.h)(0x693D4B05)
I (D:\Keil_v5\ARM\ARMCLANG\include\string.h)(0x6035A4A8) I (D:\Keil_v5\ARM\ARMCLANG\include\string.h)(0x6035A4A8)
I (..\User\component\user_math.h)(0x69352AF2) I (..\User\component\user_math.h)(0x69352AF2)
@ -2437,7 +2436,7 @@ I (..\User\component\pid.h)(0x69352AF2)
I (..\User\component\filter.h)(0x69352AF2) I (..\User\component\filter.h)(0x69352AF2)
I (..\User\device\bmi088.h)(0x69352AF2) I (..\User\device\bmi088.h)(0x69352AF2)
I (..\User\device\device.h)(0x69352AF2) I (..\User\device\device.h)(0x69352AF2)
I (..\User\module\gimbal.h)(0x693D3638) I (..\User\module\gimbal.h)(0x693D590B)
I (..\User\component\Sliding.h)(0x693D4B05) I (..\User\component\Sliding.h)(0x693D4B05)
I (D:\Keil_v5\ARM\ARMCLANG\include\string.h)(0x6035A4A8) I (D:\Keil_v5\ARM\ARMCLANG\include\string.h)(0x6035A4A8)
I (..\User\device\motor.h)(0x69352AF2) I (..\User\device\motor.h)(0x69352AF2)
@ -2470,7 +2469,7 @@ I (..\User\component\user_math.h)(0x69352AF2)
I (D:\Keil_v5\ARM\ARMCLANG\include\float.h)(0x6035A4A0) I (D:\Keil_v5\ARM\ARMCLANG\include\float.h)(0x6035A4A0)
I (D:\Keil_v5\ARM\ARMCLANG\include\math.h)(0x6035A4A8) I (D:\Keil_v5\ARM\ARMCLANG\include\math.h)(0x6035A4A8)
I (D:\Keil_v5\ARM\ARMCLANG\include\stdbool.h)(0x6035A4A8) I (D:\Keil_v5\ARM\ARMCLANG\include\stdbool.h)(0x6035A4A8)
I (..\User\module\gimbal.h)(0x693D3638) I (..\User\module\gimbal.h)(0x693D590B)
I (..\User\component\Sliding.h)(0x693D4B05) I (..\User\component\Sliding.h)(0x693D4B05)
I (D:\Keil_v5\ARM\ARMCLANG\include\string.h)(0x6035A4A8) I (D:\Keil_v5\ARM\ARMCLANG\include\string.h)(0x6035A4A8)
I (..\User\component\ahrs.h)(0x69352AF2) I (..\User\component\ahrs.h)(0x69352AF2)
@ -2546,7 +2545,7 @@ I (..\Middlewares\Third_Party\FreeRTOS\Source\portable\RVDS\ARM_CM4F\portmacro.h
I (..\Middlewares\Third_Party\FreeRTOS\Source\include\mpu_wrappers.h)(0x68B055DB) I (..\Middlewares\Third_Party\FreeRTOS\Source\include\mpu_wrappers.h)(0x68B055DB)
I (..\Middlewares\Third_Party\FreeRTOS\Source\include\task.h)(0x68B055DB) I (..\Middlewares\Third_Party\FreeRTOS\Source\include\task.h)(0x68B055DB)
I (..\Middlewares\Third_Party\FreeRTOS\Source\include\list.h)(0x68B055DB) I (..\Middlewares\Third_Party\FreeRTOS\Source\include\list.h)(0x68B055DB)
I (..\User\module\gimbal.h)(0x693D3638) I (..\User\module\gimbal.h)(0x693D590B)
I (..\User\component\Sliding.h)(0x693D4B05) I (..\User\component\Sliding.h)(0x693D4B05)
I (D:\Keil_v5\ARM\ARMCLANG\include\string.h)(0x6035A4A8) I (D:\Keil_v5\ARM\ARMCLANG\include\string.h)(0x6035A4A8)
I (..\User\component\user_math.h)(0x69352AF2) I (..\User\component\user_math.h)(0x69352AF2)
@ -2654,7 +2653,7 @@ I (..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_uart.h)(0x68B05645)
I (D:\Keil_v5\ARM\ARMCLANG\include\stdbool.h)(0x6035A4A8) I (D:\Keil_v5\ARM\ARMCLANG\include\stdbool.h)(0x6035A4A8)
I (..\User\bsp\bsp.h)(0x69352AF2) I (..\User\bsp\bsp.h)(0x69352AF2)
I (..\User\device\device.h)(0x69352AF2) I (..\User\device\device.h)(0x69352AF2)
I (..\User\module\gimbal.h)(0x693D3638) I (..\User\module\gimbal.h)(0x693D590B)
I (..\User\component\Sliding.h)(0x693D4B05) I (..\User\component\Sliding.h)(0x693D4B05)
I (D:\Keil_v5\ARM\ARMCLANG\include\string.h)(0x6035A4A8) I (D:\Keil_v5\ARM\ARMCLANG\include\string.h)(0x6035A4A8)
I (..\User\component\user_math.h)(0x69352AF2) I (..\User\component\user_math.h)(0x69352AF2)
@ -2670,8 +2669,8 @@ I (..\Core\Inc\can.h)(0x69352FF3)
I (..\User\bsp\mm.h)(0x69352AF2) I (..\User\bsp\mm.h)(0x69352AF2)
I (..\Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os.h)(0x68B055DB) I (..\Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os.h)(0x68B055DB)
I (..\User\device\motor_rm.h)(0x69352AF2) I (..\User\device\motor_rm.h)(0x69352AF2)
F (..\User\module\gimbal.c)(0x693D4F4D)(-xc -std=c99 --target=arm-arm-none-eabi -mcpu=cortex-m4 -mfpu=fpv4-sp-d16 -mfloat-abi=hard -c -fno-rtti -funsigned-char -fshort-enums -fshort-wchar -gdwarf-3 -O0 -ffunction-sections -w -I ../Core/Inc -I ../Drivers/STM32F4xx_HAL_Driver/Inc -I ../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy -I ../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I ../Drivers/CMSIS/Include -I ../User/bsp -I ../User/component -I ../User/device -I ../Middlewares/Third_Party/FreeRTOS/Source/include -I ../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 -I ../Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F -I ../User/task -I ../User -I ../User/module -I./RTE/_gimbal -ID:/Keil_v5/Arm/Packs/ARM/CMSIS/5.7.0/CMSIS/Core/Include -ID:/Keil_v5/Arm/Packs/Keil/STM32F4xx_DFP/2.17.1/Drivers/CMSIS/Device/ST/STM32F4xx/Include -D__UVISION_VERSION="534" -D_RTE_ -DSTM32F407xx -D_RTE_ -DUSE_HAL_DRIVER -DSTM32F407xx -o gimbal/gimbal.o -MD) F (..\User\module\gimbal.c)(0x693D59F2)(-xc -std=c99 --target=arm-arm-none-eabi -mcpu=cortex-m4 -mfpu=fpv4-sp-d16 -mfloat-abi=hard -c -fno-rtti -funsigned-char -fshort-enums -fshort-wchar -gdwarf-3 -O0 -ffunction-sections -w -I ../Core/Inc -I ../Drivers/STM32F4xx_HAL_Driver/Inc -I ../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy -I ../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I ../Drivers/CMSIS/Include -I ../User/bsp -I ../User/component -I ../User/device -I ../Middlewares/Third_Party/FreeRTOS/Source/include -I ../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 -I ../Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F -I ../User/task -I ../User -I ../User/module -I./RTE/_gimbal -ID:/Keil_v5/Arm/Packs/ARM/CMSIS/5.7.0/CMSIS/Core/Include -ID:/Keil_v5/Arm/Packs/Keil/STM32F4xx_DFP/2.17.1/Drivers/CMSIS/Device/ST/STM32F4xx/Include -D__UVISION_VERSION="534" -D_RTE_ -DSTM32F407xx -D_RTE_ -DUSE_HAL_DRIVER -DSTM32F407xx -o gimbal/gimbal.o -MD)
I (..\User\module\gimbal.h)(0x693D3638) I (..\User\module\gimbal.h)(0x693D590B)
I (..\User\component\Sliding.h)(0x693D4B05) I (..\User\component\Sliding.h)(0x693D4B05)
I (D:\Keil_v5\ARM\ARMCLANG\include\stdint.h)(0x6035A4A8) I (D:\Keil_v5\ARM\ARMCLANG\include\stdint.h)(0x6035A4A8)
I (D:\Keil_v5\ARM\ARMCLANG\include\string.h)(0x6035A4A8) I (D:\Keil_v5\ARM\ARMCLANG\include\string.h)(0x6035A4A8)
@ -2736,7 +2735,7 @@ I (..\Middlewares\Third_Party\FreeRTOS\Source\include\list.h)(0x68B055DB)
I (..\Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.h)(0x68B055DB) I (..\Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.h)(0x68B055DB)
I (..\User\device\motor_rm.h)(0x69352AF2) I (..\User\device\motor_rm.h)(0x69352AF2)
I (..\User\bsp\time.h)(0x69352AF2) I (..\User\bsp\time.h)(0x69352AF2)
F (..\User\module\config.c)(0x693D4F3C)(-xc -std=c99 --target=arm-arm-none-eabi -mcpu=cortex-m4 -mfpu=fpv4-sp-d16 -mfloat-abi=hard -c -fno-rtti -funsigned-char -fshort-enums -fshort-wchar -gdwarf-3 -O0 -ffunction-sections -w -I ../Core/Inc -I ../Drivers/STM32F4xx_HAL_Driver/Inc -I ../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy -I ../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I ../Drivers/CMSIS/Include -I ../User/bsp -I ../User/component -I ../User/device -I ../Middlewares/Third_Party/FreeRTOS/Source/include -I ../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 -I ../Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F -I ../User/task -I ../User -I ../User/module -I./RTE/_gimbal -ID:/Keil_v5/Arm/Packs/ARM/CMSIS/5.7.0/CMSIS/Core/Include -ID:/Keil_v5/Arm/Packs/Keil/STM32F4xx_DFP/2.17.1/Drivers/CMSIS/Device/ST/STM32F4xx/Include -D__UVISION_VERSION="534" -D_RTE_ -DSTM32F407xx -D_RTE_ -DUSE_HAL_DRIVER -DSTM32F407xx -o gimbal/config.o -MD) F (..\User\module\config.c)(0x693D5A29)(-xc -std=c99 --target=arm-arm-none-eabi -mcpu=cortex-m4 -mfpu=fpv4-sp-d16 -mfloat-abi=hard -c -fno-rtti -funsigned-char -fshort-enums -fshort-wchar -gdwarf-3 -O0 -ffunction-sections -w -I ../Core/Inc -I ../Drivers/STM32F4xx_HAL_Driver/Inc -I ../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy -I ../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I ../Drivers/CMSIS/Include -I ../User/bsp -I ../User/component -I ../User/device -I ../Middlewares/Third_Party/FreeRTOS/Source/include -I ../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 -I ../Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F -I ../User/task -I ../User -I ../User/module -I./RTE/_gimbal -ID:/Keil_v5/Arm/Packs/ARM/CMSIS/5.7.0/CMSIS/Core/Include -ID:/Keil_v5/Arm/Packs/Keil/STM32F4xx_DFP/2.17.1/Drivers/CMSIS/Device/ST/STM32F4xx/Include -D__UVISION_VERSION="534" -D_RTE_ -DSTM32F407xx -D_RTE_ -DUSE_HAL_DRIVER -DSTM32F407xx -o gimbal/config.o -MD)
I (..\User\component\user_math.h)(0x69352AF2) I (..\User\component\user_math.h)(0x69352AF2)
I (D:\Keil_v5\ARM\ARMCLANG\include\float.h)(0x6035A4A0) I (D:\Keil_v5\ARM\ARMCLANG\include\float.h)(0x6035A4A0)
I (D:\Keil_v5\ARM\ARMCLANG\include\math.h)(0x6035A4A8) I (D:\Keil_v5\ARM\ARMCLANG\include\math.h)(0x6035A4A8)
@ -2796,7 +2795,7 @@ I (..\Middlewares\Third_Party\FreeRTOS\Source\include\task.h)(0x68B055DB)
I (..\Middlewares\Third_Party\FreeRTOS\Source\include\list.h)(0x68B055DB) I (..\Middlewares\Third_Party\FreeRTOS\Source\include\list.h)(0x68B055DB)
I (..\Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.h)(0x68B055DB) I (..\Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.h)(0x68B055DB)
I (..\User\device\motor_lk.h)(0x69352AF2) I (..\User\device\motor_lk.h)(0x69352AF2)
I (..\User\module\gimbal.h)(0x693D3638) I (..\User\module\gimbal.h)(0x693D590B)
I (..\User\component\Sliding.h)(0x693D4B05) I (..\User\component\Sliding.h)(0x693D4B05)
I (D:\Keil_v5\ARM\ARMCLANG\include\string.h)(0x6035A4A8) I (D:\Keil_v5\ARM\ARMCLANG\include\string.h)(0x6035A4A8)
I (..\User\component\ahrs.h)(0x69352AF2) I (..\User\component\ahrs.h)(0x69352AF2)

BIN
MDK-ARM/gimbal/remote.o
View File

Binary file not shown.

BIN
MDK-ARM/gimbal/vofa_1.o
View File

Binary file not shown.

87
User/module/config.c
View File

@ -18,13 +18,52 @@
Config_RobotParam_t robot_config = { Config_RobotParam_t robot_config = {
.gimbal_param = { .gimbal_param = {
/* 云台欧拉角与角速度自由选择 */
.Direction={
.Eulr={
.pit=Pit,
.yaw=Yaw,
},
.Gyro={
.pit=Gyro_x,
.yaw=Gyro_z,
},
},
/* 现在只是电机测试的参数，如果用陀J需给大 */
.smc={
.yaw={
.u_max=1.0f, //输出限幅
.J=1.0,
.limit=5.1f, //饱和函数上下限
.K=1.6,
.c=0.6,
.epsilon=0.05,
},
.pit={
.u_max=1.0f, //输出限幅
.J=1.0,
.limit=5.1f, //饱和函数上下限
.K=1.6,
.c=0.6,
.epsilon=0.05,
.c1=1.0, //EIsmc参数
.c2=1.0, //EIsmc参数
},
},
/*欧拉角限位和电机角度限位*/ /*欧拉角限位和电机角度限位*/
.Limit_t= { .Limit_t= {
.pit_max=0.69, .pit_max=0.69,
.pit_min=-0.47, .pit_min=-0.47,
.yaw_max= 1.0,//yaw的 /*零点参数*/
.yaw_min=-1.0, .zero={
.yaw_encoder=0,
.travel={
.yaw=1.06,
},
},
}, },
@ -32,27 +71,23 @@ Config_RobotParam_t robot_config = {
.imu = { .imu = {
.yaw=false, .yaw=false,
.pit=false, .pit=false,
.coefficient_yaw=0, .K_yaw=0,
.coefficient_pit=0, .K_pit=0,
}, },
}, },
/*零点参数*/
.zero={
.pit_encoder=0,
.yaw_encoder=0,
},
.motor={ .motor={
/*按自己需求选择电机*/ /*按自己需求选择电机*/
.pit=RM, .pit=DM,
.yaw=RM, .yaw=RM,
/*是否开启限位*/ /*是否开启限位*/
.limit_yaw=false, .limit_yaw=false,
.limit_pit=true, .limit_pit=true,
.pit_rm_motor={BSP_CAN_1,0x205,MOTOR_GM6020,false,false}, .pit_rm_motor={BSP_CAN_2,0x20A,MOTOR_GM6020,false,false},
.yaw_rm_motor={BSP_CAN_2,0x205,MOTOR_GM6020,false,false}, .yaw_rm_motor={BSP_CAN_1,0x205,MOTOR_GM6020,false,false},
/*达妙电机参数自己配*/ /*达妙电机参数自己配*/
.pit_dm_motor={}, .pit_dm_motor={},
.yaw_dm_motor={}, .yaw_dm_motor={},
@ -66,33 +101,13 @@ Config_RobotParam_t robot_config = {
.low_pass_cutoff_freq = { .low_pass_cutoff_freq = {
.out = -1.0f, .out = -1.0f,
.gyro =100.0f, .gyro = 1000.0f,
.gyro_z=10.0f, .gyro_x=200.0f,
.gyro_y=200.0f,
.gyro_z=200.0f,
}, },
.smc={
.yaw={
.u_max=1.0f, //输出限幅
.J=1.0,
.limit=5.1f, //饱和函数上下限
.K=1.6,
.c=0.6,
.epsilon=0.05,
},
.pit={
.u_max=1.0f, //输出限幅
.J=1.0,
.limit=5.1f, //饱和函数上下限
.K=1.6,
.c=0.6,
.epsilon=0.05,
.c1=1.0, //EIsmc参数
.c2=1.0, //EIsmc参数
},
},
}, },
}; };

232
User/module/gimbal.c
View File

@ -22,7 +22,6 @@
/* Includes ----------------------------------------------------------------- */ /* Includes ----------------------------------------------------------------- */
#include "gimbal.h" #include "gimbal.h"
#include "Sliding.h"
#include "bsp/can.h" #include "bsp/can.h"
#include "bsp/time.h" #include "bsp/time.h"
#include "component/filter.h" #include "component/filter.h"
@ -54,6 +53,7 @@ static float motor_imu_offset(float* motor, float* imu){
/** /**
* \brief 设置云台模式 * \brief 设置云台模式
* *
@ -62,19 +62,21 @@ static float motor_imu_offset(float* motor, float* imu){
* *
* \return 函数运行结果 * \return 函数运行结果
*/ */
static int8_t Gimbal_SetMode(Gimbal_t *g, Gimbal_Mode_t mode) { static int8_t Gimbal_SetMode(Gimbal_t *g, Gimbal_Mode_t mode) {
if (g == NULL) if (g == NULL)
return -1; return -1;
if (mode == g->mode) if (mode == g->mode)
return GIMBAL_OK; return GIMBAL_OK;
SMC_Reset(&g->smc.yaw); SMC_Reset(&g->smc.yaw);
SMC_Reset(&g->smc.pit); SMC_Reset(&g->smc.pit);
LowPassFilter2p_Reset(&g->filter_out.major_yaw, 0.0f);
LowPassFilter2p_Reset(&g->filter_out.yaw, 0.0f); LowPassFilter2p_Reset(&g->filter_out.yaw, 0.0f);
LowPassFilter2p_Reset(&g->filter_out.pit, 0.0f); LowPassFilter2p_Reset(&g->filter_out.pit, 0.0f);
LowPassFilter2p_Reset(&g->filter_out.gyro_x, 0.0f);
LowPassFilter2p_Reset(&g->filter_out.gyro_y, 0.0f);
LowPassFilter2p_Reset(&g->filter_out.gyro_z, 0.0f);
AHRS_ResetEulr(&(g->setpoint.eulr)); /* 切换模式后重置设定值 */ AHRS_ResetEulr(&(g->setpoint.eulr)); /* 切换模式后重置设定值 */
@ -106,25 +108,38 @@ int8_t Gimbal_Init(Gimbal_t *g,Gimbal_Params_t *param,
g->param = param; g->param = param;
g->mode = GIMBAL_MODE_RELAX; /* 设置默认模式 */ g->mode = GIMBAL_MODE_RELAX; /* 设置默认模式 */
/* 初始化云台电机控制PID和LPF */ /* 初始化云台电机控制滑膜 */
g->limit.yaw.max = g->param->Limit_t.yaw_max;
g->limit.yaw.min = g->param->Limit_t.yaw_min;
SMC_Init(&g->smc.pit,EISMC,&g->param->smc.pit); SMC_Init(&g->smc.pit,EISMC,&g->param->smc.pit);
SMC_Init(&g->smc.yaw,EXPONENT,&g->param->smc.yaw); SMC_Init(&g->smc.yaw,EXPONENT,&g->param->smc.yaw);
g->limit.yaw.max = g->param->Limit_t.zero.yaw_encoder+g->param->Limit_t.zero.travel.yaw;
g->limit.yaw.min = g->param->Limit_t.zero.yaw_encoder;
LowPassFilter2p_Init(&g->filter_out.major_yaw, target_freq,
g->param->low_pass_cutoff_freq.out);
LowPassFilter2p_Init(&g->filter_out.yaw, target_freq, LowPassFilter2p_Init(&g->filter_out.yaw, target_freq,
g->param->low_pass_cutoff_freq.out); g->param->low_pass_cutoff_freq.out);
LowPassFilter2p_Init(&g->filter_out.pit, target_freq, LowPassFilter2p_Init(&g->filter_out.pit, target_freq,
g->param->low_pass_cutoff_freq.out); g->param->low_pass_cutoff_freq.out);
/* 陀螺仪角速度反馈滤波 */
LowPassFilter2p_Init(&g->filter_out.gyro_x, target_freq,
g->param->low_pass_cutoff_freq.out);
LowPassFilter2p_Init(&g->filter_out.gyro_y, target_freq,
g->param->low_pass_cutoff_freq.out);
LowPassFilter2p_Init(&g->filter_out.gyro_z, target_freq, LowPassFilter2p_Init(&g->filter_out.gyro_z, target_freq,
g->param->low_pass_cutoff_freq.out); g->param->low_pass_cutoff_freq.out);
BSP_CAN_Init(); BSP_CAN_Init();
/* 大yaw电机注册 */
if(g->param->motor.major_yaw==RM)
MOTOR_RM_Register(&(g->param->motor.major_yaw_rm_motor));
if(g->param->motor.major_yaw==DM){
MOTOR_DM_Register(&(g->param->motor.major_yaw_dm_motor));
MOTOR_DM_Enable(&(g->param->motor.major_yaw_dm_motor));
}
/*大疆电机注册*/ /*大疆电机注册*/
if(g->param->motor.yaw==RM) if(g->param->motor.yaw==RM)
MOTOR_RM_Register(&(g->param->motor.yaw_rm_motor)); MOTOR_RM_Register(&(g->param->motor.yaw_rm_motor));
@ -144,7 +159,61 @@ int8_t Gimbal_Init(Gimbal_t *g,Gimbal_Params_t *param,
return 0; return 0;
} }
static int8_t Gimbal_Direction(Gimbal_t *g){
if (g == NULL) {
return -1;
}
/* 欧拉角方向选择 */
switch(g->param->Direction.Eulr.pit){
case Pit:
g->direction.Eulr.pit=g->feedback.imu.eulr.pit;
break;
case Yaw:
g->direction.Eulr.pit=g->feedback.imu.eulr.yaw;
break;
case Rol:
g->direction.Eulr.pit=g->feedback.imu.eulr.rol;
break;
}
switch(g->param->Direction.Eulr.yaw){
case Pit:
g->direction.Eulr.yaw=g->feedback.imu.eulr.pit;
break;
case Yaw:
g->direction.Eulr.yaw=g->feedback.imu.eulr.yaw;
break;
case Rol:
g->direction.Eulr.yaw=g->feedback.imu.eulr.rol;
break;
}
/* 角速度方向选择 */
switch(g->param->Direction.Gyro.pit){
case Gyro_x:
g->direction.Gyro.pit=g->feedback.imu.gyro.x;
break;
case Gyro_y:
g->direction.Gyro.pit=g->feedback.imu.gyro.y;
break;
case Gyro_z:
g->direction.Gyro.pit=g->feedback.imu.gyro.z;
break;
}
switch(g->param->Direction.Gyro.yaw){
case Gyro_x:
g->direction.Gyro.yaw=g->feedback.imu.gyro.x;
break;
case Gyro_y:
g->direction.Gyro.yaw=g->feedback.imu.gyro.y;
break;
case Gyro_z:
g->direction.Gyro.yaw=g->feedback.imu.gyro.z;
break;
}
return 0;
}
/** /**
* \brief 通过CAN设备更新云台反馈信息 * \brief 通过CAN设备更新云台反馈信息
* *
@ -157,15 +226,29 @@ int8_t Gimbal_UpdateFeedback(Gimbal_t *gimbal) {
if (gimbal == NULL) if (gimbal == NULL)
return -1; return -1;
/* 更新大yaw电机反馈数据（RM和DM） */
if(gimbal->param->motor.major_yaw==RM){
MOTOR_RM_Update(&(gimbal->param->motor.major_yaw_rm_motor));
MOTOR_RM_t *rm_motor_yaw = MOTOR_RM_GetMotor(&(gimbal->param->motor.major_yaw_rm_motor));
if(rm_motor_yaw != NULL)
gimbal->feedback.motor.major_yaw = rm_motor_yaw->feedback;
}
if(gimbal->param->motor.major_yaw==DM){
MOTOR_DM_Update(&(gimbal->param->motor.major_yaw_dm_motor));
MOTOR_DM_t *dm_motor = MOTOR_DM_GetMotor(&(gimbal->param->motor.major_yaw_dm_motor));
if (dm_motor != NULL) {
gimbal->feedback.motor.major_yaw = dm_motor->motor.feedback;
}
}
/* 更新RM电机反馈数据 */ /* 更新RM电机反馈数据 */
if(gimbal->param->motor.yaw==RM){ if(gimbal->param->motor.yaw==RM){
MOTOR_RM_Update(&(gimbal->param->motor.yaw_rm_motor)); MOTOR_RM_Update(&(gimbal->param->motor.yaw_rm_motor));
MOTOR_RM_t *rm_motor_yaw = MOTOR_RM_GetMotor(&(gimbal->param->motor.yaw_rm_motor)); MOTOR_RM_t *rm_motor_yaw = MOTOR_RM_GetMotor(&(gimbal->param->motor.yaw_rm_motor));
if(rm_motor_yaw != NULL) if(rm_motor_yaw != NULL)
gimbal->feedback.motor.yaw = rm_motor_yaw->feedback; gimbal->feedback.motor.yaw = rm_motor_yaw->feedback;
/* 角速度 */
gimbal->feedback.motor.yaw_omega=(gimbal->feedback.motor.yaw.rotor_speed*M_2PI/60)*0.002;
gimbal->feedback.motor.pit_omega=(gimbal->feedback.motor.pit.rotor_speed*M_2PI/60)*0.002;
} }
@ -202,7 +285,29 @@ int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu){
} }
gimbal->feedback.imu.gyro = imu->gyro; gimbal->feedback.imu.gyro = imu->gyro;
gimbal->feedback.imu.eulr = imu->eulr; gimbal->feedback.imu.eulr = imu->eulr;
Gimbal_Direction(gimbal);
} }
/**
* \brief 运行大yaw控制逻辑
*
* \param g 包含云台数据的结构体
* \param
*
* \return 函数运行结果
*/
static int8_t major_yaw_Control(Gimbal_t *g){
/*处理大yaw控制命令，软件限位 - 使用电机绝对角度*/
/*获得小YAW的中点位置，如果小YAW大于中点的一定范围，大YAW开始运动，
跟随小YAW向相同方向运动，将遥控数据进行分配*/
/*小YAW中点*/
float small_yaw_center_offset = g->param->Limit_t.zero.yaw_encoder +
g->param->Limit_t.zero.travel.yaw* 0.5f;
g->setpoint.major_yaw=small_yaw_center_offset;
return 0;
}
/** /**
* \brief 运行云台控制逻辑 * \brief 运行云台控制逻辑
@ -212,30 +317,23 @@ int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu){
* *
* \return 函数运行结果 * \return 函数运行结果
*/ */
int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) { int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
if (g == NULL || g_cmd == NULL) { if (g == NULL || g_cmd == NULL) {
return -1; return -1;
} }
g->dt = (BSP_TIME_Get_us() - g->lask_wakeup) / 1000000.0f; g->dt = (BSP_TIME_Get_us() - g->lask_wakeup) / 1000000.0f;
g->lask_wakeup = BSP_TIME_Get_us(); g->lask_wakeup = BSP_TIME_Get_us();
Gimbal_SetMode(g, g_cmd->mode); Gimbal_SetMode(g, g_cmd->mode);
/* 双yaw的大yaw,setpoint从这设立 */
major_yaw_Control(g);
/* 欧拉角控制相关逻辑 */ /* 欧拉角控制相关逻辑 */
float yaw_omega_set_point, pit_omega_set_point; float yaw_omega_set_point, pit_omega_set_point,major_yaw_omega_set_point;
/* 电机角度控制相关逻辑 */
float yaw_velocity_set_point, pit_velocity_set_point;
float delta_ecd_yaw = g_cmd->delta_yaw*g->dt;
float delta_ecd_pit = g_cmd->delta_pit*g->dt;
float delta_yaw = g_cmd->delta_yaw*g->dt; float delta_yaw = g_cmd->delta_yaw*g->dt;
float delta_pit = g_cmd->delta_pit*g->dt; float delta_pit = g_cmd->delta_pit*g->dt;
float yaw_motor_imu_offset=motor_imu_offset(&g->feedback.motor.yaw.rotor_abs_angle, float yaw_motor_imu_offset=motor_imu_offset(&g->feedback.motor.yaw.rotor_abs_angle,
&g->feedback.imu.eulr.yaw); &g->feedback.imu.eulr.yaw);
@ -244,6 +342,11 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
float delta_min = CircleError(g->limit.yaw.min, float delta_min = CircleError(g->limit.yaw.min,
(g->setpoint.eulr.yaw + yaw_motor_imu_offset + delta_yaw), M_2PI); (g->setpoint.eulr.yaw + yaw_motor_imu_offset + delta_yaw), M_2PI);
/*角速度滤波*/
g->feedback.imu.gyro.x = LowPassFilter2p_Apply(&g->filter_out.gyro_x, g->feedback.imu.gyro.x);
g->feedback.imu.gyro.y = LowPassFilter2p_Apply(&g->filter_out.gyro_y, g->feedback.imu.gyro.y);
g->feedback.imu.gyro.z = LowPassFilter2p_Apply(&g->filter_out.gyro_z, g->feedback.imu.gyro.z);
switch (g->mode) { switch (g->mode) {
case GIMBAL_MODE_RELAX:/*放松模式*/ case GIMBAL_MODE_RELAX:/*放松模式*/
g->out.yaw = 0.0f; g->out.yaw = 0.0f;
@ -252,7 +355,6 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
case GIMBAL_MODE_ABSOLUTE:/*绝对模式*/ case GIMBAL_MODE_ABSOLUTE:/*绝对模式*/
g->setpoint.eulr.yaw+=delta_yaw; g->setpoint.eulr.yaw+=delta_yaw;
// g->setpoint.eulr.yaw=g_cmd->delta_yaw;
if (g->param->motor.limit_yaw==true) if (g->param->motor.limit_yaw==true)
Clip(&(g->setpoint.eulr.yaw),delta_min,delta_max); Clip(&(g->setpoint.eulr.yaw),delta_min,delta_max);
@ -263,42 +365,49 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
/* 处理pitch控制命令，软件限位 - 使用电机绝对角度 */ /* 处理pitch控制命令，软件限位 - 使用电机绝对角度 */
g->setpoint.eulr.pit+=delta_pit; g->setpoint.eulr.pit+=delta_pit;
// g->setpoint.eulr.pit=g_cmd->delta_pit;
/* 限制pit控制命令 */ /* 限制pit控制命令 */
if (g->param->motor.limit_pit == true) if (g->param->motor.limit_pit == true)
Clip(&(g->setpoint.eulr.pit),g->param->Limit_t.pit_min,g->param->Limit_t.pit_max); Clip(&(g->setpoint.eulr.pit),g->param->Limit_t.pit_min,g->param->Limit_t.pit_max);
SMC_PErrorUpdate( &g->smc.yaw, /*4310大YAW控制
g->setpoint.eulr.yaw, 这里是单环控制的，有需要加双环
g->feedback.motor.yaw.rotor_abs_angle, */
g->feedback.motor.yaw_omega,g->dt);
SMC_PErrorUpdate( &g->smc.pit, /* 这两条仅用于电机测试 */
g->setpoint.eulr.pit,
g->feedback.motor.pit.rotor_abs_angle,
g->feedback.motor.pit_omega,g->dt);
/*角速度滤波*/
// g->feedback.imu.gyro.z = LowPassFilter2p_Apply(&g->filter_out.gyro_z, g->feedback.imu.gyro.z);
// SMC_PErrorUpdate( &g->smc.yaw, // SMC_PErrorUpdate( &g->smc.yaw,
// g->setpoint.eulr.yaw, // g->setpoint.eulr.yaw,
// g->feedback.imu.eulr.yaw, // g->feedback.motor.yaw.rotor_abs_angle,
// g->feedback.imu.gyro.z*0.002,g->dt); // g->feedback.motor.yaw_omega*0.002,g->dt);
// SMC_PErrorUpdate( &g->smc.pit,
// g->setpoint.eulr.pit,
// g->feedback.motor.pit.rotor_abs_angle,
// g->feedback.motor.pit_omega*0.002,g->dt);
SMC_PErrorUpdate(&g->smc.yaw,
g->setpoint.eulr.yaw,
g->feedback.imu.eulr.yaw,
g->feedback.imu.gyro.z*0.002,g->dt);
SMC_PErrorUpdate(&g->smc.pit,
g->setpoint.eulr.pit,
g->feedback.imu.eulr.pit,
g->feedback.imu.gyro.x*0.002,g->dt);
g->out.yaw=Smc_Calc(&g->smc.yaw); g->out.yaw=Smc_Calc(&g->smc.yaw);
g->out.pit=Smc_Calc(&g->smc.pit); g->out.pit=Smc_Calc(&g->smc.pit);
/*前馈添加*/ /*前馈添加*/
if(g->param->feedforward.imu.yaw==true) if(g->param->feedforward.imu.yaw==true)
g->out.yaw+=g->param->feedforward.imu.coefficient_yaw*(yaw_omega_set_point - g->feedback.imu.gyro.z); g->out.yaw+=g->param->feedforward.imu.K_yaw*(yaw_omega_set_point - g->feedback.imu.gyro.z);
if(g->param->feedforward.imu.pit==true) if(g->param->feedforward.imu.pit==true)
g->out.pit+=g->param->feedforward.imu.coefficient_pit*(pit_omega_set_point - g->feedback.imu.gyro.x); g->out.pit+=g->param->feedforward.imu.K_pit*(pit_omega_set_point - g->feedback.imu.gyro.x);
break; break;
case GIMBAL_MODE_RELATIVE: case GIMBAL_MODE_RELATIVE:
/*计算零点*/ /*计算零点*/
g->zero.travel.yaw=motor_imu_offset(&g->feedback.motor.yaw.rotor_abs_angle,&g->feedback.imu.eulr.yaw); // g->zero.travel.yaw=motor_imu_offset(&g->feedback.motor.yaw.rotor_abs_angle,&g->feedback.imu.eulr.yaw);
g->zero.travel.yaw=motor_imu_offset(&g->feedback.motor.pit.rotor_abs_angle,&g->feedback.imu.eulr.pit); // g->zero.travel.pit=motor_imu_offset(&g->feedback.motor.pit.rotor_abs_angle,&g->feedback.imu.eulr.pit);
g->zero.yaw = g->param->zero.yaw_encoder - g->zero.travel.yaw; // g->zero.yaw = g->param->zero.yaw_encoder - g->zero.travel.yaw;
g->zero.pit = g->param->zero.pit_encoder - g->zero.travel.pit; // g->zero.pit = g->param->zero.pit_encoder - g->zero.travel.pit;
/*基于零点的设定角度*/ /*基于零点的设定角度*/
/*加的相对角度应该限制在3.14~-3.14*/ /*加的相对角度应该限制在3.14~-3.14*/
CircleAdd(&(g->relative_angle.yaw),delta_yaw,M_2PI); CircleAdd(&(g->relative_angle.yaw),delta_yaw,M_2PI);
@ -323,23 +432,24 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
Clip(&(g->setpoint.eulr.pit),g->param->Limit_t.pit_min,g->param->Limit_t.pit_max); Clip(&(g->setpoint.eulr.pit),g->param->Limit_t.pit_min,g->param->Limit_t.pit_max);
/*前馈添加*/ /*前馈添加*/
if(g->param->feedforward.imu.yaw==true) if(g->param->feedforward.imu.yaw==true)
g->out.yaw+=g->param->feedforward.imu.coefficient_yaw*(yaw_omega_set_point - g->feedback.imu.gyro.z); g->out.yaw+=g->param->feedforward.imu.K_yaw*(yaw_omega_set_point - g->feedback.imu.gyro.z);
if(g->param->feedforward.imu.pit==true) if(g->param->feedforward.imu.pit==true)
g->out.pit+=g->param->feedforward.imu.coefficient_pit*(pit_omega_set_point - g->feedback.imu.gyro.x); g->out.pit+=g->param->feedforward.imu.K_pit*(pit_omega_set_point - g->feedback.imu.gyro.x);
break; break;
} }
/* 输出滤波 */ /* 输出滤波 */
g->out.major_yaw = LowPassFilter2p_Apply(&g->filter_out.major_yaw, g->out.major_yaw);
g->out.yaw = LowPassFilter2p_Apply(&g->filter_out.yaw, g->out.yaw); g->out.yaw = LowPassFilter2p_Apply(&g->filter_out.yaw, g->out.yaw);
g->out.pit = LowPassFilter2p_Apply(&g->filter_out.pit, g->out.pit); g->out.pit = LowPassFilter2p_Apply(&g->filter_out.pit, g->out.pit);
return 0; return 0;
} }
/** /**
* \brief 云台输出 * \brief 云台输出
* *
@ -347,6 +457,25 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
* \param out CAN设备云台输出结构体 * \param out CAN设备云台输出结构体
*/ */
void Gimbal_Output(Gimbal_t *g){ void Gimbal_Output(Gimbal_t *g){
/*大yaw电机输出*/
if(g->param->motor.major_yaw==RM){
MOTOR_RM_Ctrl(&g->param->motor.major_yaw_rm_motor);
MOTOR_RM_SetOutput(&g->param->motor.major_yaw_rm_motor, g->out.major_yaw);
}
if(g->param->motor.major_yaw==DM){
g->out.major_yaw_dm.angle=g->param->dm_Params_t.major_yaw_dm.angle;
g->out.major_yaw_dm.kd=g->param->dm_Params_t.major_yaw_dm.kd;
g->out.major_yaw_dm.kp=g->param->dm_Params_t.major_yaw_dm.kp;
g->out.major_yaw_dm.velocity=g->param->dm_Params_t.major_yaw_dm.velocity;
g->out.major_yaw_dm.torque= g->out.pit * g->param->dm_Params_t.major_yaw_dm_Reduction_ratio; // 乘以减速比
MOTOR_DM_MITCtrl(&g->param->motor.major_yaw_dm_motor,&(g->out.major_yaw_dm));
}
/*大疆电机输出*/ /*大疆电机输出*/
if(g->param->motor.yaw==RM){ if(g->param->motor.yaw==RM){
MOTOR_RM_Ctrl(&g->param->motor.yaw_rm_motor); MOTOR_RM_Ctrl(&g->param->motor.yaw_rm_motor);
@ -381,4 +510,3 @@ void Gimbal_Output(Gimbal_t *g){
} }
} }

99
User/module/gimbal.h
View File

@ -36,6 +36,18 @@ typedef enum {
RM, /*大疆 */ RM, /*大疆 */
} Gimbal_MOTOR; } Gimbal_MOTOR;
typedef enum {
Gyro_x,
Gyro_y,
Gyro_z,
} Gimbal_Gyro;
typedef enum {
Pit,
Yaw,
Rol,
} Gimbal_Eulr;
typedef struct { typedef struct {
Gimbal_Mode_t mode; Gimbal_Mode_t mode;
float delta_yaw; float delta_yaw;
@ -55,14 +67,35 @@ typedef struct{
}travel; }travel;
float pit; /*零点*/ float pit; /*零点*/
float yaw; float yaw;
} Gimbal_zero_t; } Gimbal_zero_t;
typedef struct { typedef struct {
struct{
float pit;
float yaw;
float rol;
}Gyro;
struct{
float pit;
float yaw;
float rol;
}Eulr;
} Gimbal_Direction_t;
typedef struct {
bool limit_yaw;/*是否开启限位*/ bool limit_yaw;/*是否开启限位*/
bool limit_pit; bool limit_pit;
Gimbal_MOTOR major_yaw;
Gimbal_MOTOR pit; Gimbal_MOTOR pit;
Gimbal_MOTOR yaw; Gimbal_MOTOR yaw;
MOTOR_DM_Param_t major_yaw_dm_motor; /* 大yaw轴电机参数 */
MOTOR_RM_Param_t major_yaw_rm_motor; /* 大yaw轴电机参数 */
MOTOR_DM_Param_t pit_dm_motor; /* pitch轴电机参数 */ MOTOR_DM_Param_t pit_dm_motor; /* pitch轴电机参数 */
MOTOR_DM_Param_t yaw_dm_motor; /* yaw轴电机参数 */ MOTOR_DM_Param_t yaw_dm_motor; /* yaw轴电机参数 */
@ -75,25 +108,32 @@ typedef struct {
typedef struct { typedef struct {
Gimbal_MOTOR_Param_t motor; Gimbal_MOTOR_Param_t motor;
struct { struct {
SlidingParam pit; SlidingParam pit;
SlidingParam yaw; SlidingParam yaw;
} smc; } smc;
struct { struct {
float pit_encoder; /*零点*/ struct{
float yaw_encoder; Gimbal_Gyro pit;
Gimbal_Gyro yaw;
Gimbal_Gyro rol;
}Gyro;
struct{
Gimbal_Eulr pit;
Gimbal_Eulr yaw;
Gimbal_Eulr rol;
}Eulr;
} Direction;
}zero;
/* 前馈系数 */ /* 前馈系数 */
struct { struct {
struct{ struct{
bool yaw; bool yaw;
bool pit; bool pit;
float coefficient_yaw; float K_yaw;
float coefficient_pit; float K_pit;
}imu; }imu;
}feedforward; }feedforward;
@ -102,8 +142,9 @@ typedef struct {
struct { struct {
float out; /* 电机输出 */ float out; /* 电机输出 */
float gyro; /* 陀螺仪数据 */ float gyro; /* 陀螺仪数据 */
float gyro_x;
float gyro_z; /* 陀螺仪数据 */ float gyro_y;
float gyro_z;
} low_pass_cutoff_freq; } low_pass_cutoff_freq;
@ -112,9 +153,24 @@ typedef struct {
float pit_min; float pit_min;
float yaw_max; /*yaw的限位*/ float yaw_max; /*yaw的限位*/
float yaw_min; float yaw_min;
struct {
float pit_encoder; /*零点*/
float yaw_encoder;
struct{
float yaw;
float pit;
}travel;
}zero;
}Limit_t; }Limit_t;
struct { struct {
/*大yaw的参数*/
MOTOR_MIT_Output_t major_yaw_dm;
float major_yaw_dm_Reduction_ratio;
MOTOR_MIT_Output_t yaw_dm; /*达妙的参数*/ MOTOR_MIT_Output_t yaw_dm; /*达妙的参数*/
MOTOR_MIT_Output_t pit_dm; MOTOR_MIT_Output_t pit_dm;
float yaw_dm_Reduction_ratio; float yaw_dm_Reduction_ratio;
@ -122,7 +178,6 @@ typedef struct {
}dm_Params_t; }dm_Params_t;
} Gimbal_Params_t; } Gimbal_Params_t;
typedef struct { typedef struct {
AHRS_Gyro_t gyro; AHRS_Gyro_t gyro;
AHRS_Eulr_t eulr; AHRS_Eulr_t eulr;
@ -131,17 +186,20 @@ typedef struct {
typedef struct { typedef struct {
Gimbal_IMU_t imu; Gimbal_IMU_t imu;
struct { struct {
MOTOR_Feedback_t major_yaw; /* 大yaw轴电机反馈 */
MOTOR_Feedback_t yaw; /* yaw轴电机反馈 */ MOTOR_Feedback_t yaw; /* yaw轴电机反馈 */
MOTOR_Feedback_t pit; /* pitch轴电机反馈 */ MOTOR_Feedback_t pit; /* pitch轴电机反馈 */
float yaw_omega;
float pit_omega;
} motor; } motor;
} Gimbal_Feedback_t; } Gimbal_Feedback_t;
/* 云台输出数据的结构体*/ /* 云台输出数据的结构体*/
typedef struct { typedef struct {
float major_yaw;
MOTOR_MIT_Output_t major_yaw_dm;
float yaw; /* yaw轴电机输出 */ float yaw; /* yaw轴电机输出 */
float pit; /* pitch轴电机输出 */ float pit; /* pitch轴电机输出 */
MOTOR_MIT_Output_t yaw_dm; /*达妙的参数*/ MOTOR_MIT_Output_t yaw_dm; /*达妙的参数*/
MOTOR_MIT_Output_t pit_dm; MOTOR_MIT_Output_t pit_dm;
@ -154,19 +212,22 @@ typedef struct {
uint64_t lask_wakeup; uint64_t lask_wakeup;
float dt; float dt;
Gimbal_Params_t *param; /* 云台的参数，用Gimbal_Init设定 */ Gimbal_Params_t *param; /* 云台的参数，用Gimbal_Init设定 */
/* 模块通用 */ /* 模块通用 */
Gimbal_Mode_t mode; /* 云台模式 */ Gimbal_Mode_t mode; /* 云台模式 */
Gimbal_MOTOR MOTOR; Gimbal_MOTOR MOTOR;
Gimbal_zero_t zero; Gimbal_zero_t zero;
Gimbal_Direction_t direction;
/* PID计算的目标值 */ /* PID计算的目标值 */
struct { struct {
float major_yaw;
AHRS_Eulr_t eulr; /* 表示云台姿态的欧拉角 */ AHRS_Eulr_t eulr; /* 表示云台姿态的欧拉角 */
AHRS_Eulr_t ecd; AHRS_Eulr_t ecd;
} setpoint; } setpoint;
struct { struct {
Sliding pit; Sliding pit;
Sliding yaw; Sliding yaw;
@ -178,16 +239,20 @@ typedef struct {
} limit; } limit;
struct { struct {
float major_yaw;
float yaw; float yaw;
float pit; float pit;
} relative_angle; } relative_angle;
struct { struct {
LowPassFilter2p_t gyro_x;
LowPassFilter2p_t gyro_y;
LowPassFilter2p_t gyro_z;
LowPassFilter2p_t major_yaw;
LowPassFilter2p_t yaw; LowPassFilter2p_t yaw;
LowPassFilter2p_t pit; LowPassFilter2p_t pit;
LowPassFilter2p_t gyro_z;
LowPassFilter2p_t gyro_x;
} filter_out; } filter_out;
Gimbal_Output_t out; /* 云台输出 */ Gimbal_Output_t out; /* 云台输出 */