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

1111

Browse Source
This commit is contained in:
yunhai8432 2025-11-12 22:18:15 +08:00
parent 445e5bb93f
commit 0f335cbc9e
14 changed files with 2061 additions and 2078 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

File diff suppressed because one or more lines are too long

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.

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

@ -3,7 +3,7 @@
<title>Static Call Graph - [gimbal\gimbal.axf]</title></head> <title>Static Call Graph - [gimbal\gimbal.axf]</title></head>
<body><HR> <body><HR>
<H1>Static Call Graph for image gimbal\gimbal.axf</H1><HR> <H1>Static Call Graph for image gimbal\gimbal.axf</H1><HR>
<BR><P>#&#060CALLGRAPH&#062# ARM Linker, 6160001: Last Updated: Wed Nov 12 21:26:23 2025 <BR><P>#&#060CALLGRAPH&#062# ARM Linker, 6160001: Last Updated: Wed Nov 12 22:13:05 2025
<BR><P> <BR><P>
<H3>Maximum Stack Usage = 432 bytes + Unknown(Functions without stacksize, Cycles, Untraceable Function Pointers)</H3><H3> <H3>Maximum Stack Usage = 432 bytes + Unknown(Functions without stacksize, Cycles, Untraceable Function Pointers)</H3><H3>
Call chain for Maximum Stack Depth:</H3> Call chain for Maximum Stack Depth:</H3>
@ -169,9 +169,9 @@ Global Symbols
<P><STRONG><a name="[248]"></a>__scatterload_null</STRONG> (Thumb, 0 bytes, Stack size unknown bytes, __scatter.o(!!!scatter), UNUSED) <P><STRONG><a name="[248]"></a>__scatterload_null</STRONG> (Thumb, 0 bytes, Stack size unknown bytes, __scatter.o(!!!scatter), UNUSED)
<P><STRONG><a name="[249]"></a>__decompress</STRONG> (Thumb, 58 bytes, Stack size unknown bytes, __dczerorl.o(!!dczerorl), UNUSED) <P><STRONG><a name="[249]"></a>__decompress</STRONG> (Thumb, 90 bytes, Stack size unknown bytes, __dczerorl2.o(!!dczerorl2), UNUSED)
<P><STRONG><a name="[24a]"></a>__decompress0</STRONG> (Thumb, 0 bytes, Stack size unknown bytes, __dczerorl.o(!!dczerorl), UNUSED) <P><STRONG><a name="[24a]"></a>__decompress1</STRONG> (Thumb, 0 bytes, Stack size unknown bytes, __dczerorl2.o(!!dczerorl2), UNUSED)
<P><STRONG><a name="[24b]"></a>__scatterload_zeroinit</STRONG> (Thumb, 28 bytes, Stack size unknown bytes, __scatter_zi.o(!!handler_zi), UNUSED) <P><STRONG><a name="[24b]"></a>__scatterload_zeroinit</STRONG> (Thumb, 28 bytes, Stack size unknown bytes, __scatter_zi.o(!!handler_zi), UNUSED)
@ -1114,7 +1114,7 @@ Global Symbols
<LI><a href="#[197]">&gt;&gt;</a>&nbsp;&nbsp;&nbsp;MX_CAN1_Init <LI><a href="#[197]">&gt;&gt;</a>&nbsp;&nbsp;&nbsp;MX_CAN1_Init
</UL> </UL>
<P><STRONG><a name="[eb]"></a>Gimbal_Control</STRONG> (Thumb, 1304 bytes, Stack size 88 bytes, gimbal.o(.text.Gimbal_Control)) <P><STRONG><a name="[eb]"></a>Gimbal_Control</STRONG> (Thumb, 1312 bytes, Stack size 88 bytes, gimbal.o(.text.Gimbal_Control))
<BR><BR>[Stack]<UL><LI>Max Depth = 208<LI>Call Chain = Gimbal_Control &rArr; PID_Calc &rArr; LowPassFilter2p_Apply &rArr; __ARM_isinf <BR><BR>[Stack]<UL><LI>Max Depth = 208<LI>Call Chain = Gimbal_Control &rArr; PID_Calc &rArr; LowPassFilter2p_Apply &rArr; __ARM_isinf
</UL> </UL>
<BR>[Calls]<UL><LI><a href="#[f0]">&gt;&gt;</a>&nbsp;&nbsp;&nbsp;Clip <BR>[Calls]<UL><LI><a href="#[f0]">&gt;&gt;</a>&nbsp;&nbsp;&nbsp;Clip
@ -1128,7 +1128,7 @@ Global Symbols
<BR>[Called By]<UL><LI><a href="#[70]">&gt;&gt;</a>&nbsp;&nbsp;&nbsp;Task_gimbal_ctrl <BR>[Called By]<UL><LI><a href="#[70]">&gt;&gt;</a>&nbsp;&nbsp;&nbsp;Task_gimbal_ctrl
</UL> </UL>
<P><STRONG><a name="[f3]"></a>Gimbal_Init</STRONG> (Thumb, 472 bytes, Stack size 32 bytes, gimbal.o(.text.Gimbal_Init)) <P><STRONG><a name="[f3]"></a>Gimbal_Init</STRONG> (Thumb, 400 bytes, Stack size 32 bytes, gimbal.o(.text.Gimbal_Init))
<BR><BR>[Stack]<UL><LI>Max Depth = 372 + Unknown Stack Size <BR><BR>[Stack]<UL><LI>Max Depth = 372 + Unknown Stack Size
<LI>Call Chain = Gimbal_Init &rArr; BSP_CAN_Init &rArr; osMutexNew &rArr; xQueueCreateMutexStatic &rArr; prvInitialiseMutex &rArr; xQueueGenericSend &rArr; prvCopyDataToQueue &rArr; xTaskPriorityDisinherit &rArr; vListInsertEnd <LI>Call Chain = Gimbal_Init &rArr; BSP_CAN_Init &rArr; osMutexNew &rArr; xQueueCreateMutexStatic &rArr; prvInitialiseMutex &rArr; xQueueGenericSend &rArr; prvCopyDataToQueue &rArr; xTaskPriorityDisinherit &rArr; vListInsertEnd
</UL> </UL>

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

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

@ -2126,7 +2126,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)(0x691489EC) I (..\User\module\gimbal.h)(0x691491D4)
I (..\User\component\ahrs.h)(0x68F3410E) I (..\User\component\ahrs.h)(0x68F3410E)
I (..\User\component\filter.h)(0x68F22FA9) I (..\User\component\filter.h)(0x68F22FA9)
I (..\User\component\pid.h)(0x68F22FA9) I (..\User\component\pid.h)(0x68F22FA9)
@ -2264,7 +2264,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)(0x691489EC) I (..\User\module\gimbal.h)(0x691491D4)
I (..\User\component\ahrs.h)(0x68F3410E) I (..\User\component\ahrs.h)(0x68F3410E)
I (..\User\component\user_math.h)(0x68F22FA9) I (..\User\component\user_math.h)(0x68F22FA9)
I (D:\Keil_v5\ARM\ARMCLANG\include\float.h)(0x6035A4A0) I (D:\Keil_v5\ARM\ARMCLANG\include\float.h)(0x6035A4A0)
@ -2376,7 +2376,7 @@ I (..\User\component\pid.h)(0x68F22FA9)
I (..\User\component\filter.h)(0x68F22FA9) I (..\User\component\filter.h)(0x68F22FA9)
I (..\User\device\bmi088.h)(0x68F22F87) I (..\User\device\bmi088.h)(0x68F22F87)
I (..\User\device\device.h)(0x68F22FA9) I (..\User\device\device.h)(0x68F22FA9)
I (..\User\module\gimbal.h)(0x691489EC) I (..\User\module\gimbal.h)(0x691491D4)
I (..\User\device\motor.h)(0x68F22F87) I (..\User\device\motor.h)(0x68F22F87)
I (..\User\device\motor_dm.h)(0x68F22F87) I (..\User\device\motor_dm.h)(0x68F22F87)
I (..\User\bsp\can.h)(0x68F22FA9) I (..\User\bsp\can.h)(0x68F22FA9)
@ -2407,7 +2407,7 @@ I (..\User\component\user_math.h)(0x68F22FA9)
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)(0x691489EC) I (..\User\module\gimbal.h)(0x691491D4)
I (..\User\component\ahrs.h)(0x68F3410E) I (..\User\component\ahrs.h)(0x68F3410E)
I (..\User\component\filter.h)(0x68F22FA9) I (..\User\component\filter.h)(0x68F22FA9)
I (..\User\component\pid.h)(0x68F22FA9) I (..\User\component\pid.h)(0x68F22FA9)
@ -2481,7 +2481,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)(0x691489EC) I (..\User\module\gimbal.h)(0x691491D4)
I (..\User\component\ahrs.h)(0x68F3410E) I (..\User\component\ahrs.h)(0x68F3410E)
I (..\User\component\user_math.h)(0x68F22FA9) I (..\User\component\user_math.h)(0x68F22FA9)
I (D:\Keil_v5\ARM\ARMCLANG\include\float.h)(0x6035A4A0) I (D:\Keil_v5\ARM\ARMCLANG\include\float.h)(0x6035A4A0)
@ -2534,8 +2534,8 @@ I (..\User\device\motor_rm.h)(0x68F22F87)
I (..\User\module\config.h)(0x68DD3463) I (..\User\module\config.h)(0x68DD3463)
I (..\User\device\motor_lz.h)(0x68F22F87) I (..\User\device\motor_lz.h)(0x68F22F87)
I (..\User\device\motor_lk.h)(0x68F22F87) I (..\User\device\motor_lk.h)(0x68F22F87)
F (..\User\module\gimbal.c)(0x69148AFD)(-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)(0x691495F0)(-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)(0x691489EC) I (..\User\module\gimbal.h)(0x691491D4)
I (..\User\component\ahrs.h)(0x68F3410E) I (..\User\component\ahrs.h)(0x68F3410E)
I (..\User\component\user_math.h)(0x68F22FA9) I (..\User\component\user_math.h)(0x68F22FA9)
I (D:\Keil_v5\ARM\ARMCLANG\include\float.h)(0x6035A4A0) I (D:\Keil_v5\ARM\ARMCLANG\include\float.h)(0x6035A4A0)
@ -2598,7 +2598,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)(0x68F22F87) I (..\User\device\motor_rm.h)(0x68F22F87)
I (..\User\bsp\time.h)(0x68F22FA9) I (..\User\bsp\time.h)(0x68F22FA9)
F (..\User\module\config.c)(0x691488E2)(-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)(0x691493B1)(-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)(0x68F22FA9) I (..\User\component\user_math.h)(0x68F22FA9)
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)
@ -2658,7 +2658,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)(0x68F22F87) I (..\User\device\motor_lk.h)(0x68F22F87)
I (..\User\module\gimbal.h)(0x691489EC) I (..\User\module\gimbal.h)(0x691491D4)
I (..\User\component\ahrs.h)(0x68F3410E) I (..\User\component\ahrs.h)(0x68F3410E)
I (..\User\component\filter.h)(0x68F22FA9) I (..\User\component\filter.h)(0x68F22FA9)
I (..\User\component\pid.h)(0x68F22FA9) I (..\User\component\pid.h)(0x68F22FA9)

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

Binary file not shown.

6
User/module/config.c
View File

@ -20,11 +20,11 @@ Config_RobotParam_t robot_config = {
.gimbal_param = { .gimbal_param = {
/*欧拉角限位和电机角度限位*/ /*欧拉角限位和电机角度限位*/
.Set_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_max= 1.0,//yaw的
// .yaw_min=-1.0, .yaw_min=-1.0,
}, },

42
User/module/gimbal.c
View File

@ -106,29 +106,16 @@ int8_t Gimbal_Init(Gimbal_t *g,Gimbal_Params_t *param,
PID_Init(&(g->pid.pit_velocity), KPID_MODE_CALC_D, target_freq, PID_Init(&(g->pid.pit_velocity), KPID_MODE_CALC_D, target_freq,
&(g->param->pid.pit_velocity)); &(g->param->pid.pit_velocity));
g->limit.yaw.max = g->param->Limit_t.yaw_max;
g->limit.yaw.min = g->param->Limit_t.yaw_min;
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);
// g->limit.yaw.max = g->param->mech_zero.yaw + g->param->travel.yaw;
// g->limit.yaw.min = g->param->mech_zero.yaw;
// g->limit.pit.max = g->param->mech_zero.pit + g->param->travel.pit;
// g->limit.pit.min = g->param->mech_zero.pit;
/*输出限位*/
g->limit.set_pit.max=g->param->Set_Limit_t.pit_max;
g->limit.set_pit.min=g->param->Set_Limit_t.pit_min;
g->limit.set_yaw.max=g->param->Set_Limit_t.yaw_max;
g->limit.set_yaw.min=g->param->Set_Limit_t.yaw_min;
g->zero.yaw=g->param->zero.yaw;
g->zero.pit=g->param->zero.pit;
g->zero.yaw_encoder=g->param->zero.yaw_encoder;
g->zero.pit_encoder=g->param->zero.pit_encoder;
g->zero.travel.yaw=g->param->zero.travel.yaw;
g->zero.travel.pit=g->param->zero.travel.pit;
BSP_CAN_Init(); BSP_CAN_Init();
/*大疆电机注册*/ /*大疆电机注册*/
@ -249,9 +236,9 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
if (motor_imu_offset > M_PI) motor_imu_offset -= M_2PI; if (motor_imu_offset > M_PI) motor_imu_offset -= M_2PI;
if (motor_imu_offset < -M_PI) motor_imu_offset += M_2PI; if (motor_imu_offset < -M_PI) motor_imu_offset += M_2PI;
/* 计算到限位边界的距离 （这个限位给yaw）*/ /* 计算到限位边界的距离 （这个限位给yaw）*/
float delta_max = CircleError(g->limit.set_yaw.max, float delta_max = CircleError(g->limit.yaw.max,
(g->setpoint.eulr.yaw + motor_imu_offset + delta_yaw), M_2PI); (g->setpoint.eulr.yaw + motor_imu_offset + delta_yaw), M_2PI);
float delta_min = CircleError(g->limit.set_yaw.min, float delta_min = CircleError(g->limit.yaw.min,
(g->setpoint.eulr.yaw + motor_imu_offset + delta_yaw), M_2PI); (g->setpoint.eulr.yaw + motor_imu_offset + delta_yaw), M_2PI);
switch (g->mode) { switch (g->mode) {
@ -271,10 +258,9 @@ 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;
/* 限制pit控制命令 */
if (g->param->motor.limit_pit == true) if (g->param->motor.limit_pit == true)
/* 限制pit控制命令 */ Clip(&(g->setpoint.eulr.pit),g->param->Limit_t.pit_min,g->param->Limit_t.pit_max);
Clip(&(g->setpoint.eulr.pit),g->limit.set_pit.min,g->limit.set_pit.max);
yaw_omega_set_point = PID_Calc(&(g->pid.yaw_angle), g->setpoint.eulr.yaw, yaw_omega_set_point = PID_Calc(&(g->pid.yaw_angle), g->setpoint.eulr.yaw,
g->feedback.imu.eulr.yaw, 0.0f, g->dt); g->feedback.imu.eulr.yaw, 0.0f, g->dt);
@ -295,12 +281,12 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
break; break;
case GIMBAL_MODE_RELATIVE: case GIMBAL_MODE_RELATIVE:
/*计算零点*/ /*计算零点*/
g->zero.yaw = g->zero.yaw_encoder - g->zero.travel.yaw; g->zero.yaw = g->param->zero.yaw_encoder - g->param->zero.travel.yaw;
g->zero.pit = g->zero.pit_encoder - g->zero.travel.pit; g->zero.pit = g->param->zero.pit_encoder - g->param->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);
if((g->relative_angle.yaw)>=M_2PI)g->relative_angle.yaw-=(M_2PI); if((g->relative_angle.yaw)>=M_2PI)g->relative_angle.yaw-=M_2PI;
g->relative_angle.pit=+delta_pit; g->relative_angle.pit=+delta_pit;
@ -309,14 +295,14 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
/*yaw的限位*/ /*yaw的限位*/
if (g->param->motor.limit_yaw==true) if (g->param->motor.limit_yaw==true)
Clip(&(g->setpoint.eulr.yaw),g->limit.set_yaw.min,g->limit.set_yaw.max); Clip(&(g->setpoint.eulr.yaw),delta_min,delta_max);
else{ else{
/*限制在-3.14~3.14*/ /*限制在-3.14~3.14*/
if(g->setpoint.eulr.yaw > M_PI) g->setpoint.eulr.yaw -= M_2PI; if(g->setpoint.eulr.yaw > M_PI) g->setpoint.eulr.yaw -= M_2PI;
} }
/* pit限制控制命令 */ /* pit限制控制命令 */
if (g->param->motor.limit_pit==true) if (g->param->motor.limit_pit==true)
Clip(&(g->setpoint.eulr.pit),g->limit.set_pit.min,g->limit.set_pit.max); Clip(&(g->setpoint.eulr.pit),g->param->Limit_t.pit_min,g->param->Limit_t.pit_max);
yaw_omega_set_point = PID_Calc(&(g->pid.yaw_angle), g->setpoint.eulr.yaw, yaw_omega_set_point = PID_Calc(&(g->pid.yaw_angle), g->setpoint.eulr.yaw,
g->feedback.imu.eulr.yaw, 0.0f, g->dt); g->feedback.imu.eulr.yaw, 0.0f, g->dt);

9
User/module/gimbal.h
View File

@ -113,10 +113,7 @@ typedef struct {
float pit_min; float pit_min;
float yaw_max; /*yaw的限位*/ float yaw_max; /*yaw的限位*/
float yaw_min; float yaw_min;
}Set_Limit_t; }Limit_t;
struct { struct {
MOTOR_MIT_Output_t yaw_dm; /*达妙的参数*/ MOTOR_MIT_Output_t yaw_dm; /*达妙的参数*/
@ -182,8 +179,8 @@ typedef struct {
} pid; } pid;
struct { struct {
Gimbal_Limit_t set_yaw; Gimbal_Limit_t yaw;
Gimbal_Limit_t set_pit; Gimbal_Limit_t pit;
} limit; } limit;
struct { struct {