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

解决了过零点问题，pit电机测没问题

Browse Source
This commit is contained in:
yunhai8432 2025-12-13 19:37:55 +08:00
parent 464b37b66c
commit cc27fe6cf7
19 changed files with 2063 additions and 2156 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

File diff suppressed because one or more lines are too long

30
MDK-ARM/gimbal.uvoptx
View File

@ -165,36 +165,6 @@
<WinNumber>1</WinNumber>
<ItemText>gimbal_cmd</ItemText>
</Ww>
<Ww>
<count>3</count>
<WinNumber>1</WinNumber>
<ItemText>cali_bmi088</ItemText>
</Ww>
<Ww>
<count>4</count>
<WinNumber>1</WinNumber>
<ItemText>vofa_imu</ItemText>
</Ww>
<Ww>
<count>5</count>
<WinNumber>1</WinNumber>
<ItemText>channels</ItemText>
</Ww>
<Ww>
<count>6</count>
<WinNumber>1</WinNumber>
<ItemText>remote_cmd</ItemText>
</Ww>
<Ww>
<count>7</count>
<WinNumber>1</WinNumber>
<ItemText>task_runtime.msgq.gimbal</ItemText>
</Ww>
<Ww>
<count>8</count>
<WinNumber>1</WinNumber>
<ItemText>gimbal_cmd</ItemText>
</Ww>
</WatchWindow1>
<Tracepoint>
<THDelay>0</THDelay>

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

Binary file not shown.

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

Binary file not shown.

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

@ -32,9 +32,6 @@ 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\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'.
compiling gimbal_ctrl.c...
linking...
Program Size: Code=92784 RO-data=1172 RW-data=252 ZI-data=34476
"gimbal\gimbal.axf" - 0 Error(s), 0 Warning(s).
<h2>Software Packages used:</h2>
@ -58,7 +55,7 @@ Package Vendor: Keil
<h2>Collection of Component Files used:</h2>
* Component: ARM::CMSIS:CORE:5.4.0
Build Time Elapsed: 00:00:02
Build Time Elapsed: 00:00:01
</pre>
</body>
</html>

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

@ -3,7 +3,7 @@
<title>Static Call Graph - [gimbal\gimbal.axf]</title></head>
<body><HR>
<H1>Static Call Graph for image gimbal\gimbal.axf</H1><HR>
<BR><P>#&#060CALLGRAPH&#062# ARM Linker, 6160001: Last Updated: Mon Dec 8 20:13:34 2025
<BR><P>#&#060CALLGRAPH&#062# ARM Linker, 6160001: Last Updated: Sat Dec 13 19:34:39 2025
<BR><P>
<H3>Maximum Stack Usage = 576 bytes + Unknown(Functions without stacksize, Cycles, Untraceable Function Pointers)</H3><H3>
Call chain for Maximum Stack Depth:</H3>
@ -1136,7 +1136,7 @@ Global Symbols
<LI><a href="#[1a4]">&gt;&gt;</a>&nbsp;&nbsp;&nbsp;MX_CAN1_Init
</UL>
<P><STRONG><a name="[f4]"></a>Gimbal_Control</STRONG> (Thumb, 1112 bytes, Stack size 80 bytes, gimbal.o(.text.Gimbal_Control))
<P><STRONG><a name="[f4]"></a>Gimbal_Control</STRONG> (Thumb, 1156 bytes, Stack size 80 bytes, gimbal.o(.text.Gimbal_Control))
<BR><BR>[Stack]<UL><LI>Max Depth = 544<LI>Call Chain = Gimbal_Control &rArr; Smc_Calc &rArr; __hardfp_pow &rArr; sqrt &rArr; _dsqrt &rArr; __fpl_dnaninf
</UL>
<BR>[Calls]<UL><LI><a href="#[fc]">&gt;&gt;</a>&nbsp;&nbsp;&nbsp;Smc_Calc
@ -1177,8 +1177,8 @@ Global Symbols
<BR>[Called By]<UL><LI><a href="#[73]">&gt;&gt;</a>&nbsp;&nbsp;&nbsp;Task_gimbal_ctrl
</UL>
<P><STRONG><a name="[10c]"></a>Gimbal_UpdateFeedback</STRONG> (Thumb, 468 bytes, Stack size 40 bytes, gimbal.o(.text.Gimbal_UpdateFeedback))
<BR><BR>[Stack]<UL><LI>Max Depth = 364<LI>Call Chain = Gimbal_UpdateFeedback &rArr; MOTOR_DM_Update &rArr; BSP_CAN_GetMessage &rArr; osMutexRelease &rArr; xQueueGiveMutexRecursive &rArr; xQueueGenericSend &rArr; prvCopyDataToQueue &rArr; xTaskPriorityDisinherit &rArr; vListInsertEnd
<P><STRONG><a name="[10c]"></a>Gimbal_UpdateFeedback</STRONG> (Thumb, 528 bytes, Stack size 56 bytes, gimbal.o(.text.Gimbal_UpdateFeedback))
<BR><BR>[Stack]<UL><LI>Max Depth = 380<LI>Call Chain = Gimbal_UpdateFeedback &rArr; MOTOR_DM_Update &rArr; BSP_CAN_GetMessage &rArr; osMutexRelease &rArr; xQueueGiveMutexRecursive &rArr; xQueueGenericSend &rArr; prvCopyDataToQueue &rArr; xTaskPriorityDisinherit &rArr; vListInsertEnd
</UL>
<BR>[Calls]<UL><LI><a href="#[99]">&gt;&gt;</a>&nbsp;&nbsp;&nbsp;__aeabi_f2d
<LI><a href="#[10f]">&gt;&gt;</a>&nbsp;&nbsp;&nbsp;__aeabi_dmul
@ -2485,7 +2485,7 @@ Global Symbols
<BR>[Called By]<UL><LI><a href="#[ef]">&gt;&gt;</a>&nbsp;&nbsp;&nbsp;DMA1_Stream1_IRQHandler1
</UL>
<P><STRONG><a name="[100]"></a>SMC_Init</STRONG> (Thumb, 108 bytes, Stack size 24 bytes, sliding.o(.text.SMC_Init))
<P><STRONG><a name="[100]"></a>SMC_Init</STRONG> (Thumb, 110 bytes, Stack size 24 bytes, sliding.o(.text.SMC_Init))
<BR><BR>[Stack]<UL><LI>Max Depth = 52<LI>Call Chain = SMC_Init &rArr; SMC_SetParam &rArr; OutContinuation
</UL>
<BR>[Calls]<UL><LI><a href="#[1bc]">&gt;&gt;</a>&nbsp;&nbsp;&nbsp;SMC_SetParam
@ -2493,7 +2493,7 @@ Global Symbols
<BR>[Called By]<UL><LI><a href="#[ff]">&gt;&gt;</a>&nbsp;&nbsp;&nbsp;Gimbal_Init
</UL>
<P><STRONG><a name="[fb]"></a>SMC_PErrorUpdate</STRONG> (Thumb, 156 bytes, Stack size 20 bytes, sliding.o(.text.SMC_PErrorUpdate))
<P><STRONG><a name="[fb]"></a>SMC_PErrorUpdate</STRONG> (Thumb, 260 bytes, Stack size 20 bytes, sliding.o(.text.SMC_PErrorUpdate))
<BR><BR>[Stack]<UL><LI>Max Depth = 20<LI>Call Chain = SMC_PErrorUpdate
</UL>
<BR>[Called By]<UL><LI><a href="#[f4]">&gt;&gt;</a>&nbsp;&nbsp;&nbsp;Gimbal_Control
@ -2508,7 +2508,7 @@ Global Symbols
<P><STRONG><a name="[6]"></a>SVC_Handler</STRONG> (Thumb, 36 bytes, Stack size 0 bytes, port.o(.text.SVC_Handler))
<BR>[Address Reference Count : 1]<UL><LI> startup_stm32f407xx.o(RESET)
</UL>
<P><STRONG><a name="[fc]"></a>Smc_Calc</STRONG> (Thumb, 1688 bytes, Stack size 208 bytes, sliding.o(.text.Smc_Calc))
<P><STRONG><a name="[fc]"></a>Smc_Calc</STRONG> (Thumb, 1760 bytes, Stack size 208 bytes, sliding.o(.text.Smc_Calc))
<BR><BR>[Stack]<UL><LI>Max Depth = 464<LI>Call Chain = Smc_Calc &rArr; __hardfp_pow &rArr; sqrt &rArr; _dsqrt &rArr; __fpl_dnaninf
</UL>
<BR>[Calls]<UL><LI><a href="#[1c1]">&gt;&gt;</a>&nbsp;&nbsp;&nbsp;Sat
@ -2625,7 +2625,7 @@ Global Symbols
</UL>
<BR>[Address Reference Count : 1]<UL><LI> init.o(.text.Task_Init)
</UL>
<P><STRONG><a name="[73]"></a>Task_gimbal_ctrl</STRONG> (Thumb, 276 bytes, Stack size 32 bytes, gimbal_ctrl.o(.text.Task_gimbal_ctrl))
<P><STRONG><a name="[73]"></a>Task_gimbal_ctrl</STRONG> (Thumb, 244 bytes, Stack size 32 bytes, gimbal_ctrl.o(.text.Task_gimbal_ctrl))
<BR><BR>[Stack]<UL><LI>Max Depth = 576 + Unknown Stack Size
<LI>Call Chain = Task_gimbal_ctrl &rArr; Gimbal_Control &rArr; Smc_Calc &rArr; __hardfp_pow &rArr; sqrt &rArr; _dsqrt &rArr; __fpl_dnaninf
</UL>
@ -2648,13 +2648,14 @@ Global Symbols
</UL>
<BR>[Address Reference Count : 1]<UL><LI> init.o(.text.Task_Init)
</UL>
<P><STRONG><a name="[74]"></a>Task_remote</STRONG> (Thumb, 160 bytes, Stack size 32 bytes, remote.o(.text.Task_remote))
<BR><BR>[Stack]<UL><LI>Max Depth = 228<LI>Call Chain = Task_remote &rArr; osMessageQueuePut &rArr; xQueueGenericSendFromISR &rArr; prvCopyDataToQueue &rArr; xTaskPriorityDisinherit &rArr; vListInsertEnd
<P><STRONG><a name="[74]"></a>Task_remote</STRONG> (Thumb, 176 bytes, Stack size 40 bytes, remote.o(.text.Task_remote))
<BR><BR>[Stack]<UL><LI>Max Depth = 236<LI>Call Chain = Task_remote &rArr; osMessageQueuePut &rArr; xQueueGenericSendFromISR &rArr; prvCopyDataToQueue &rArr; xTaskPriorityDisinherit &rArr; vListInsertEnd
</UL>
<BR>[Calls]<UL><LI><a href="#[1dc]">&gt;&gt;</a>&nbsp;&nbsp;&nbsp;remote_ParseHost
<LI><a href="#[1da]">&gt;&gt;</a>&nbsp;&nbsp;&nbsp;delay_init
<LI><a href="#[1d6]">&gt;&gt;</a>&nbsp;&nbsp;&nbsp;osMessageQueueReset
<LI><a href="#[cd]">&gt;&gt;</a>&nbsp;&nbsp;&nbsp;osMessageQueuePut
<LI><a href="#[bc]">&gt;&gt;</a>&nbsp;&nbsp;&nbsp;osMessageQueueGet
<LI><a href="#[e2]">&gt;&gt;</a>&nbsp;&nbsp;&nbsp;osKernelGetTickFreq
<LI><a href="#[1d1]">&gt;&gt;</a>&nbsp;&nbsp;&nbsp;osKernelGetTickCount
<LI><a href="#[1d7]">&gt;&gt;</a>&nbsp;&nbsp;&nbsp;osDelayUntil
@ -2861,6 +2862,7 @@ Global Symbols
</UL>
<BR>[Called By]<UL><LI><a href="#[ba]">&gt;&gt;</a>&nbsp;&nbsp;&nbsp;BSP_CAN_GetMessage
<LI><a href="#[75]">&gt;&gt;</a>&nbsp;&nbsp;&nbsp;Task_vofa
<LI><a href="#[74]">&gt;&gt;</a>&nbsp;&nbsp;&nbsp;Task_remote
<LI><a href="#[73]">&gt;&gt;</a>&nbsp;&nbsp;&nbsp;Task_gimbal_ctrl
</UL>
@ -5355,7 +5357,7 @@ Local Symbols
<BR>[Called By]<UL><LI><a href="#[19b]">&gt;&gt;</a>&nbsp;&nbsp;&nbsp;MOTOR_DM_ParseFeedbackFrame
</UL>
<P><STRONG><a name="[f7]"></a>Gimbal_SetMode</STRONG> (Thumb, 160 bytes, Stack size 24 bytes, gimbal.o(.text.Gimbal_SetMode))
<P><STRONG><a name="[f7]"></a>Gimbal_SetMode</STRONG> (Thumb, 148 bytes, Stack size 24 bytes, gimbal.o(.text.Gimbal_SetMode))
<BR><BR>[Stack]<UL><LI>Max Depth = 96<LI>Call Chain = Gimbal_SetMode &rArr; LowPassFilter2p_Reset &rArr; LowPassFilter2p_Apply &rArr; __ARM_isinf
</UL>
<BR>[Calls]<UL><LI><a href="#[109]">&gt;&gt;</a>&nbsp;&nbsp;&nbsp;SMC_Reset

3832
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.

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

@ -1850,8 +1850,8 @@ I (..\User\component\calc_lib.h)(0x692C2F1E)
F (..\User\component\calc_lib.c)(0x692C2F1E)(-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/calc_lib.o -MD)
I (..\User\component\calc_lib.h)(0x692C2F1E)
I (..\User\module\struct_typedef.h)(0x692C2F1E)
F (..\User\component\Sliding.c)(0x6936C0A7)(-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/sliding.o -MD)
I (..\User\component\Sliding.h)(0x6936C0A7)
F (..\User\component\Sliding.c)(0x693D4B3E)(-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/sliding.o -MD)
I (..\User\component\Sliding.h)(0x693D4B05)
I (D:\Keil_v5\ARM\ARMCLANG\include\stdint.h)(0x6035A4A8)
I (D:\Keil_v5\ARM\ARMCLANG\include\string.h)(0x6035A4A8)
I (..\User\component\user_math.h)(0x69352AF2)
@ -1859,7 +1859,7 @@ 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\stdbool.h)(0x6035A4A8)
I (D:\Keil_v5\ARM\ARMCLANG\include\stddef.h)(0x6035A4A8)
F (..\User\component\Sliding.h)(0x6936C0A7)()
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)
I (..\User\component\Kalman.h)(0x69353AFC)
I (D:\Keil_v5\ARM\ARMCLANG\include\stdint.h)(0x6035A4A8)
@ -1923,8 +1923,8 @@ 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\stdint.h)(0x6035A4A8)
I (D:\Keil_v5\ARM\ARMCLANG\include\stddef.h)(0x6035A4A8)
I (..\User\module\gimbal.h)(0x6934E912)
I (..\User\component\Sliding.h)(0x6936C0A7)
I (..\User\module\gimbal.h)(0x693D3638)
I (..\User\component\Sliding.h)(0x693D4B05)
I (D:\Keil_v5\ARM\ARMCLANG\include\string.h)(0x6035A4A8)
I (..\User\component\ahrs.h)(0x69352AF2)
I (..\User\component\filter.h)(0x69352AF2)
@ -2323,8 +2323,8 @@ 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\task.h)(0x68B055DB)
I (..\Middlewares\Third_Party\FreeRTOS\Source\include\list.h)(0x68B055DB)
I (..\User\module\gimbal.h)(0x6934E912)
I (..\User\component\Sliding.h)(0x6936C0A7)
I (..\User\module\gimbal.h)(0x693D3638)
I (..\User\component\Sliding.h)(0x693D4B05)
I (D:\Keil_v5\ARM\ARMCLANG\include\string.h)(0x6035A4A8)
I (..\User\component\user_math.h)(0x69352AF2)
I (D:\Keil_v5\ARM\ARMCLANG\include\float.h)(0x6035A4A0)
@ -2437,8 +2437,8 @@ I (..\User\component\pid.h)(0x69352AF2)
I (..\User\component\filter.h)(0x69352AF2)
I (..\User\device\bmi088.h)(0x69352AF2)
I (..\User\device\device.h)(0x69352AF2)
I (..\User\module\gimbal.h)(0x6934E912)
I (..\User\component\Sliding.h)(0x6936C0A7)
I (..\User\module\gimbal.h)(0x693D3638)
I (..\User\component\Sliding.h)(0x693D4B05)
I (D:\Keil_v5\ARM\ARMCLANG\include\string.h)(0x6035A4A8)
I (..\User\device\motor.h)(0x69352AF2)
I (..\User\device\motor_dm.h)(0x69352AF2)
@ -2448,7 +2448,7 @@ I (..\User\bsp\mm.h)(0x69352AF2)
I (..\Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os.h)(0x68B055DB)
I (..\User\device\motor_rm.h)(0x69352AF2)
I (..\User\device\ist8310.h)(0x69352AF2)
F (..\User\task\remote.c)(0x69352AEF)(-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/remote.o -MD)
F (..\User\task\remote.c)(0x693D4BF6)(-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/remote.o -MD)
I (..\User\task\user_task.h)(0x69352BFA)
I (..\Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.h)(0x68B055DB)
I (D:\Keil_v5\ARM\ARMCLANG\include\stdint.h)(0x6035A4A8)
@ -2470,8 +2470,8 @@ I (..\User\component\user_math.h)(0x69352AF2)
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\stdbool.h)(0x6035A4A8)
I (..\User\module\gimbal.h)(0x6934E912)
I (..\User\component\Sliding.h)(0x6936C0A7)
I (..\User\module\gimbal.h)(0x693D3638)
I (..\User\component\Sliding.h)(0x693D4B05)
I (D:\Keil_v5\ARM\ARMCLANG\include\string.h)(0x6035A4A8)
I (..\User\component\ahrs.h)(0x69352AF2)
I (..\User\component\filter.h)(0x69352AF2)
@ -2532,7 +2532,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\task.h)(0x68B055DB)
I (..\Middlewares\Third_Party\FreeRTOS\Source\include\list.h)(0x68B055DB)
F (..\User\task\gimbal_ctrl.c)(0x6936C0EC)(-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_ctrl.o -MD)
F (..\User\task\gimbal_ctrl.c)(0x693D2B87)(-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_ctrl.o -MD)
I (..\User\task\user_task.h)(0x69352BFA)
I (..\Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.h)(0x68B055DB)
I (D:\Keil_v5\ARM\ARMCLANG\include\stdint.h)(0x6035A4A8)
@ -2546,8 +2546,8 @@ 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\task.h)(0x68B055DB)
I (..\Middlewares\Third_Party\FreeRTOS\Source\include\list.h)(0x68B055DB)
I (..\User\module\gimbal.h)(0x6934E912)
I (..\User\component\Sliding.h)(0x6936C0A7)
I (..\User\module\gimbal.h)(0x693D3638)
I (..\User\component\Sliding.h)(0x693D4B05)
I (D:\Keil_v5\ARM\ARMCLANG\include\string.h)(0x6035A4A8)
I (..\User\component\user_math.h)(0x69352AF2)
I (D:\Keil_v5\ARM\ARMCLANG\include\float.h)(0x6035A4A0)
@ -2654,8 +2654,8 @@ I (..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_uart.h)(0x68B05645)
I (D:\Keil_v5\ARM\ARMCLANG\include\stdbool.h)(0x6035A4A8)
I (..\User\bsp\bsp.h)(0x69352AF2)
I (..\User\device\device.h)(0x69352AF2)
I (..\User\module\gimbal.h)(0x6934E912)
I (..\User\component\Sliding.h)(0x6936C0A7)
I (..\User\module\gimbal.h)(0x693D3638)
I (..\User\component\Sliding.h)(0x693D4B05)
I (D:\Keil_v5\ARM\ARMCLANG\include\string.h)(0x6035A4A8)
I (..\User\component\user_math.h)(0x69352AF2)
I (D:\Keil_v5\ARM\ARMCLANG\include\float.h)(0x6035A4A0)
@ -2670,9 +2670,9 @@ I (..\Core\Inc\can.h)(0x69352FF3)
I (..\User\bsp\mm.h)(0x69352AF2)
I (..\Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os.h)(0x68B055DB)
I (..\User\device\motor_rm.h)(0x69352AF2)
F (..\User\module\gimbal.c)(0x6936C0E6)(-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)(0x6934E912)
I (..\User\component\Sliding.h)(0x6936C0A7)
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)
I (..\User\module\gimbal.h)(0x693D3638)
I (..\User\component\Sliding.h)(0x693D4B05)
I (D:\Keil_v5\ARM\ARMCLANG\include\stdint.h)(0x6035A4A8)
I (D:\Keil_v5\ARM\ARMCLANG\include\string.h)(0x6035A4A8)
I (..\User\component\user_math.h)(0x69352AF2)
@ -2736,7 +2736,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 (..\User\device\motor_rm.h)(0x69352AF2)
I (..\User\bsp\time.h)(0x69352AF2)
F (..\User\module\config.c)(0x6936BDB8)(-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)(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)
I (..\User\component\user_math.h)(0x69352AF2)
I (D:\Keil_v5\ARM\ARMCLANG\include\float.h)(0x6035A4A0)
I (D:\Keil_v5\ARM\ARMCLANG\include\math.h)(0x6035A4A8)
@ -2796,8 +2796,8 @@ 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\CMSIS_RTOS_V2\cmsis_os2.h)(0x68B055DB)
I (..\User\device\motor_lk.h)(0x69352AF2)
I (..\User\module\gimbal.h)(0x6934E912)
I (..\User\component\Sliding.h)(0x6936C0A7)
I (..\User\module\gimbal.h)(0x693D3638)
I (..\User\component\Sliding.h)(0x693D4B05)
I (D:\Keil_v5\ARM\ARMCLANG\include\string.h)(0x6035A4A8)
I (..\User\component\ahrs.h)(0x69352AF2)
I (..\User\component\filter.h)(0x69352AF2)

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

Binary file not shown.

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

Binary file not shown.

24
User/component/Sliding.c
View File

@ -144,7 +144,7 @@ void SMC_Reset(Sliding *s){
*/
void SMC_Init(Sliding *s,Smc_mode mode,SlidingParam *param){
s->J = 0;
s->param->J = 0;
s->param->K = 0;
s->param->c = 0;
s->param->epsilon = 0;
@ -189,6 +189,14 @@ void SMC_PErrorUpdate(Sliding *s,float target, float pos_now, float vol_now,floa
s->position.error = pos_now - target;
s->velocity.error = vol_now - s->position.tar_differential;
/* 将误差限制在 -PI 到 +PI 之间 */
if (s->position.error > M_PI) {
s->position.error -= M_2PI;
} else if (s->position.error < -M_PI) {
s->position.error += M_2PI;
}
/* 更新上一次的值 */
s->position.tar_last = s->position.tar_now;
/* 位置误差积分项 */
@ -276,7 +284,7 @@ float Smc_Calc(Sliding *s){
s->s = s->param->c * s->position.error + s->velocity.error; //滑模面
fun = Sat(s->s,s);//饱和函数消除抖动
// u = smc.param.J * ( (-smc.param.c * smc.error.v_error) - smc.param.K * smc.s - smc.param.K * (std::pow(std::fabs(smc.s),smc.param.epsilon)) * fun + smc.error.tar_differential_second); //控制器计算,幂次趋近率
u = s->J * ( (-s->param->c * s->velocity.error) - s->param->K * s->s - s->param->K * (pow(fabs(s->s),s->param->epsilon)) * fun); //控制器计算,幂次趋近率
u = s->param->J * ( (-s->param->c * s->velocity.error) - s->param->K * s->s - s->param->K * (pow(fabs(s->s),s->param->epsilon)) * fun); //控制器计算,幂次趋近率
break;
case TFSMC:///tfsmc
@ -295,7 +303,7 @@ float Smc_Calc(Sliding *s){
fun = Sat(s->s,s);
if(s->position.error!=0){
u = s->J * (s->position.tar_differential_second//目标值的二阶导 暂定是否删除，需测试
u = s->param->J * (s->position.tar_differential_second//目标值的二阶导 暂定是否删除，需测试
-s->param->K * s->s //s*K
-s->param->epsilon * fun //epsilon*SAT(S)
-s->velocity.error * ((s->param->q * s->param->beta) * pos_pow) / (s->param->p * s->position.error)); //控制器计算
@ -308,7 +316,7 @@ float Smc_Calc(Sliding *s){
fun = Sat(s->s,s);
/*控制器计算，速度控制*/
u = s->J * (s->velocity.tar_differential - (s->param->c * s->velocity.error) - s->param->K * s->s - s->param->epsilon * fun);
u = s->param->J * (s->velocity.tar_differential - (s->param->c * s->velocity.error) - s->param->K * s->s - s->param->epsilon * fun);
break;
case EISMC:///比例积分滑模面，指数趋近律，位置控制
@ -320,8 +328,12 @@ float Smc_Calc(Sliding *s){
s->s = s->param->c1 * s->position.error + s->velocity.error + s->param->c2 * s->position.error_integral;
fun = Sat(s->s,s);
/* 控制器计算,指数趋近率 */
u = s->J * ( (-s->param->c1 * s->velocity.error)- s->param->c2 * s->position.error - s->param->K * s->s - s->param->epsilon * fun);
break;
u = s->param->J * ( (-s->param->c1 * s->velocity.error)- s->param->c2 * s->position.error - s->param->K * s->s - s->param->epsilon * fun);
s->s1= -s->param->c1 * s->velocity.error;
s->s2= -s->param->c2 * s->position.error;
s->s3= -s->param->K * s->s ;
s->s4= -s->param->epsilon * fun;
break;
}
/* 更新上一步的误差 */
s->position.error_last = s->position.error;

1
User/component/Sliding.h
View File

@ -74,7 +74,6 @@ typedef struct {
float fun; //控制器输出
float u; //控制器输出
float s; //滑模面计算储存
float J;
SlidingParam *param;
SlidingParam *param_last;
Smc_mode mode;

22
User/module/config.c
View File

@ -46,13 +46,13 @@ Config_RobotParam_t robot_config = {
.motor={
/*按自己需求选择电机*/
.pit=DM,
.pit=RM,
.yaw=RM,
/*是否开启限位*/
.limit_yaw=false,
.limit_pit=true,
// .pit_rm_motor={BSP_CAN_1,0x20A,MOTOR_GM6020,false,false},
.yaw_rm_motor={BSP_CAN_1,0x205,MOTOR_GM6020,false,false},
.pit_rm_motor={BSP_CAN_1,0x205,MOTOR_GM6020,false,false},
.yaw_rm_motor={BSP_CAN_2,0x205,MOTOR_GM6020,false,false},
/*达妙电机参数自己配*/
.pit_dm_motor={},
.yaw_dm_motor={},
@ -77,15 +77,19 @@ Config_RobotParam_t robot_config = {
.K=1.6,
.c=0.6,
.epsilon=0.05,
// float c1; //EIsmc参数
// float c2; //EIsmc参数
// float p; //tfsmc参数，正奇数 p>q
// float q; //tfsmc参数，正奇数
// float beta; //tfsmc参数，正数
},
.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参数
},
},

18
User/module/gimbal.c
View File

@ -62,7 +62,7 @@ static float motor_imu_offset(float* motor, float* imu){
*
* \return 函数运行结果
*/
int hhhh;
static int8_t Gimbal_SetMode(Gimbal_t *g, Gimbal_Mode_t mode) {
if (g == NULL)
return -1;
@ -75,8 +75,8 @@ static int8_t Gimbal_SetMode(Gimbal_t *g, Gimbal_Mode_t mode) {
LowPassFilter2p_Reset(&g->filter_out.yaw, 0.0f);
LowPassFilter2p_Reset(&g->filter_out.pit, 0.0f);
hhhh+=1;
AHRS_ResetEulr(&(g->setpoint.eulr)); /* 切换模式后重置设定值 */
g->setpoint.eulr.pit = g->feedback.imu.eulr.pit;
@ -115,6 +115,7 @@ int8_t Gimbal_Init(Gimbal_t *g,Gimbal_Params_t *param,
SMC_Init(&g->smc.pit,EISMC,&g->param->smc.pit);
SMC_Init(&g->smc.yaw,EXPONENT,&g->param->smc.yaw);
LowPassFilter2p_Init(&g->filter_out.yaw, target_freq,
g->param->low_pass_cutoff_freq.out);
LowPassFilter2p_Init(&g->filter_out.pit, target_freq,
@ -164,8 +165,7 @@ int8_t Gimbal_UpdateFeedback(Gimbal_t *gimbal) {
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.yaw_torque=gimbal->feedback.motor.yaw.torque_current/1000;
gimbal->feedback.motor.pit_omega=(gimbal->feedback.motor.pit.rotor_speed*M_2PI/60)*0.002;
}
@ -263,6 +263,7 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
/* 处理pitch控制命令，软件限位 - 使用电机绝对角度 */
g->setpoint.eulr.pit+=delta_pit;
// g->setpoint.eulr.pit=g_cmd->delta_pit;
/* 限制pit控制命令 */
if (g->param->motor.limit_pit == true)
Clip(&(g->setpoint.eulr.pit),g->param->Limit_t.pit_min,g->param->Limit_t.pit_max);
@ -270,7 +271,11 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
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,
@ -278,6 +283,7 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
// g->feedback.imu.eulr.yaw,
// g->feedback.imu.gyro.z*0.002,g->dt);
g->out.yaw=Smc_Calc(&g->smc.yaw);
g->out.pit=Smc_Calc(&g->smc.pit);
/*前馈添加*/
if(g->param->feedforward.imu.yaw==true)

2
User/module/gimbal.h
View File

@ -134,7 +134,7 @@ typedef struct {
MOTOR_Feedback_t yaw; /* yaw轴电机反馈 */
MOTOR_Feedback_t pit; /* pitch轴电机反馈 */
float yaw_omega;
float yaw_torque;
float pit_omega;
} motor;
} Gimbal_Feedback_t;

2
User/task/gimbal_ctrl.c
View File

@ -43,7 +43,7 @@ void Task_gimbal_ctrl(void *argument) {
if(osMessageQueueGet(task_runtime.msgq.gimbal.imu, &gimbal_imu, NULL, 0)==osOK){
Gimbal_UpdateIMU(&gimbal, &gimbal_imu);}
gimbal_cmd.mode=GIMBAL_MODE_ABSOLUTE;
if(osMessageQueueGet(task_runtime.msgq.gimbal.cmd, &gimbal_cmd, NULL, 0)==osOK)
// if(osMessageQueueGet(task_runtime.msgq.gimbal.cmd, &gimbal_cmd, NULL, 0)==osOK)
Gimbal_UpdateFeedback(&gimbal);
Gimbal_Control(&gimbal,&gimbal_cmd);
Gimbal_Output(&gimbal);

2
User/task/remote.c
View File

@ -41,7 +41,7 @@ void Task_remote(void *argument) {
tick += delay_tick; /* 计算下一个唤醒时刻 */
/* USER CODE BEGIN */
// osMessageQueueGet(task_runtime.msgq.gimbal.imu, &imu, NULL, 0);
osMessageQueueGet(task_runtime.msgq.gimbal.imu, &imu, NULL, 0);
remote_ParseHost(&remote_cmd,&rc_ctrl,&imu);
osMessageQueueReset(task_runtime.msgq.gimbal.cmd);