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

修改了达妙和大疆电机选择bug

Browse Source
This commit is contained in:
yunhai8432 2025-10-28 22:19:29 +08:00
parent 34d2c9d42b
commit 8fa85eb88a
16 changed files with 2500 additions and 2482 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

8
MDK-ARM/gimbal/gimbal.__i
View File

@ -1,8 +0,0 @@
-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 "../User/module/gimbal.c"

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

Binary file not shown.

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

@ -31,9 +31,15 @@ Note: source file '..\User\bsp\can.c' - object file renamed from 'gimbal\can.o'
Note: source file '..\User\bsp\gpio.c' - object file renamed from 'gimbal\gpio.o' to 'gimbal\gpio_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'.
compiling gimbal_ctrl.c...
compiling ai.c...
compiling init.c...
compiling remote.c...
compiling config.c...
compiling gimbal.c...
compiling atti_esti.c...
linking...
Program Size: Code=84320 RO-data=996 RW-data=416 ZI-data=34280
Program Size: Code=84384 RO-data=996 RW-data=416 ZI-data=34280
"gimbal\gimbal.axf" - 0 Error(s), 0 Warning(s).
<h2>Software Packages used:</h2>
@ -58,7 +64,7 @@ Package Vendor: Keil
* Component: ARM::CMSIS:CORE:5.4.0
Include file: CMSIS\Core\Include\tz_context.h
Build Time Elapsed: 00:00:01
Build Time Elapsed: 00:00:02
</pre>
</body>
</html>

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

File diff suppressed because it is too large Load Diff

2936
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\stdint.h)(0x6035A4A8)
I (D:\Keil_v5\ARM\ARMCLANG\include\stddef.h)(0x6035A4A8)
I (..\User\module\gimbal.h)(0x68FA3585)
I (..\User\module\gimbal.h)(0x6900D064)
I (..\User\component\ahrs.h)(0x68F3410E)
I (..\User\component\filter.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\task.h)(0x68B055DB)
I (..\Middlewares\Third_Party\FreeRTOS\Source\include\list.h)(0x68B055DB)
I (..\User\module\gimbal.h)(0x68FA3585)
I (..\User\module\gimbal.h)(0x6900D064)
I (..\User\component\ahrs.h)(0x68F3410E)
I (..\User\component\user_math.h)(0x68F22FA9)
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\device\bmi088.h)(0x68F22F87)
I (..\User\device\device.h)(0x68F22FA9)
I (..\User\module\gimbal.h)(0x68FA3585)
I (..\User\module\gimbal.h)(0x6900D064)
I (..\User\device\motor.h)(0x68F22F87)
I (..\User\device\motor_dm.h)(0x68F22F87)
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\math.h)(0x6035A4A8)
I (D:\Keil_v5\ARM\ARMCLANG\include\stdbool.h)(0x6035A4A8)
I (..\User\module\gimbal.h)(0x68FA3585)
I (..\User\module\gimbal.h)(0x6900D064)
I (..\User\component\ahrs.h)(0x68F3410E)
I (..\User\component\filter.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\task.h)(0x68B055DB)
I (..\Middlewares\Third_Party\FreeRTOS\Source\include\list.h)(0x68B055DB)
I (..\User\module\gimbal.h)(0x68FA3585)
I (..\User\module\gimbal.h)(0x6900D064)
I (..\User\component\ahrs.h)(0x68F3410E)
I (..\User\component\user_math.h)(0x68F22FA9)
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\device\motor_lz.h)(0x68F22F87)
I (..\User\device\motor_lk.h)(0x68F22F87)
F (..\User\module\gimbal.c)(0x68FC3C51)(-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)(0x68FA3585)
F (..\User\module\gimbal.c)(0x6900D050)(-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)(0x6900D064)
I (..\User\component\ahrs.h)(0x68F3410E)
I (..\User\component\user_math.h)(0x68F22FA9)
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 (..\User\device\motor_rm.h)(0x68F22F87)
I (..\User\bsp\time.h)(0x68F22FA9)
F (..\User\module\config.c)(0x68FC3C64)(-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)(0x6900D050)(-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 (D:\Keil_v5\ARM\ARMCLANG\include\float.h)(0x6035A4A0)
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\CMSIS_RTOS_V2\cmsis_os2.h)(0x68B055DB)
I (..\User\device\motor_lk.h)(0x68F22F87)
I (..\User\module\gimbal.h)(0x68FA3585)
I (..\User\module\gimbal.h)(0x6900D064)
I (..\User\component\ahrs.h)(0x68F3410E)
I (..\User\component\filter.h)(0x68F22FA9)
I (..\User\component\pid.h)(0x68F22FA9)

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

Binary file not shown.

31
User/module/config.c
View File

@ -46,17 +46,18 @@ Config_RobotParam_t robot_config = {
.ecd_pit=1.0f,
},
.pit_rm_motor={BSP_CAN_2,0x20A,MOTOR_GM6020,false,false},
.yaw_rm_motor={BSP_CAN_1,0x206,MOTOR_GM6020,false,false},
.motor={
/*按自己需求选择电机*/
.pit=RM,
.yaw=RM,
.pit_rm_motor={BSP_CAN_2,0x20A,MOTOR_GM6020,false,false},
.yaw_rm_motor={BSP_CAN_1,0x205,MOTOR_GM6020,false,false},
/*达妙电机参数自己配*/
.pit_dm_motor={},
.yaw_dm_motor={},
},
/*达妙电机参数自己配*/
.dm_Params_t={
.yaw_dm={.kd=0.3,},
.pit_dm={.kd=0.3,},
},
// .yaw_dm_motor={}
// .pit_dm_motor={}
.low_pass_cutoff_freq = {
.out = -1.0f,
.gyro = 1000.0f,
@ -106,8 +107,8 @@ Config_RobotParam_t robot_config = {
},
/*电机控制参数*/
.yaw_velocity = {
.k = 0.0f,
.p = 0.0f,
.k = 1.0f,
.p = 1.0f,
.i = 0.0f,
.d = 0.000f,//0
.i_limit = 1.0f,
@ -116,8 +117,8 @@ Config_RobotParam_t robot_config = {
.range = -1.0f,
},
.yaw_ecd_angle = {
.k = 0.0f,
.p = 0.0f ,
.k = 1.0f,
.p = 1.0f ,
.i = 0.0f,
.d = 0.0f,
.i_limit = 0.0f,
@ -136,7 +137,7 @@ Config_RobotParam_t robot_config = {
.range = -1.0f,
},
.pit_ecd_angle = {
.k = 0.0f,
.k = 1.0f,
.p = 0.0f,
.i = 0.0f,
.d = 0.0f,

82
User/module/gimbal.c
View File

@ -119,7 +119,6 @@ int8_t Gimbal_Init(Gimbal_t *g,Gimbal_Params_t *param,
// g->limit.pit.min = g->param->mech_zero.pit;
/*输出限位*/
g->yaw_num=g->param->yaw_num;
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;
@ -133,20 +132,20 @@ int8_t Gimbal_Init(Gimbal_t *g,Gimbal_Params_t *param,
BSP_CAN_Init();
/*大疆电机注册*/
if(g->param->yaw_rm_motor.module==MOTOR_GM6020)
MOTOR_RM_Register(&(g->param->yaw_rm_motor));
if(g->param->pit_rm_motor.module==MOTOR_GM6020)
MOTOR_RM_Register(&(g->param->pit_rm_motor));
if(g->param->motor.yaw==RM)
MOTOR_RM_Register(&(g->param->motor.yaw_rm_motor));
if(g->param->motor.pit==RM)
MOTOR_RM_Register(&(g->param->motor.pit_rm_motor));
/*达妙电机注册*/
if(g->param->yaw_dm_motor.module==MOTOR_DM_J4310){
MOTOR_DM_Register(&(g->param->yaw_dm_motor));
MOTOR_DM_Enable(&(g->param->yaw_dm_motor));
if(g->param->motor.yaw==DM){
MOTOR_DM_Register(&(g->param->motor.yaw_dm_motor));
MOTOR_DM_Enable(&(g->param->motor.yaw_dm_motor));
}
if(g->param->pit_dm_motor.module==MOTOR_DM_J4310){
MOTOR_DM_Register(&(g->param->pit_dm_motor));
MOTOR_DM_Enable(&(g->param->pit_dm_motor));
if(g->param->motor.pit==DM){
MOTOR_DM_Register(&(g->param->motor.pit_dm_motor));
MOTOR_DM_Enable(&(g->param->motor.pit_dm_motor));
}
@ -167,30 +166,32 @@ int8_t Gimbal_UpdateFeedback(Gimbal_t *gimbal) {
return -1;
/* 更新RM电机反馈数据 */
if(gimbal->param->yaw_rm_motor.module==MOTOR_GM6020){
MOTOR_RM_Update(&(gimbal->param->yaw_rm_motor));
MOTOR_RM_t *rm_motor_yaw = MOTOR_RM_GetMotor(&(gimbal->param->yaw_rm_motor));
if(gimbal->param->motor.yaw==RM){
MOTOR_RM_Update(&(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)
gimbal->feedback.motor.yaw = rm_motor_yaw->feedback;
}
if(gimbal->param->pit_rm_motor.module==MOTOR_GM6020){
MOTOR_RM_Update(&(gimbal->param->pit_rm_motor));
MOTOR_RM_t *rm_motor_pit = MOTOR_RM_GetMotor(&(gimbal->param->pit_rm_motor));
if(gimbal->param->motor.pit==RM){
MOTOR_RM_Update(&(gimbal->param->motor.pit_rm_motor));
MOTOR_RM_t *rm_motor_pit = MOTOR_RM_GetMotor(&(gimbal->param->motor.pit_rm_motor));
if (rm_motor_pit != NULL)
gimbal->feedback.motor.pit = rm_motor_pit->feedback;
}
/* 更新DM电机反馈数据 */
if(gimbal->param->yaw_dm_motor.module==MOTOR_DM_J4310){
MOTOR_DM_Update(&(gimbal->param->yaw_dm_motor));
MOTOR_DM_t *dm_motor = MOTOR_DM_GetMotor(&(gimbal->param->yaw_dm_motor));
if(gimbal->param->motor.yaw==DM){
MOTOR_DM_Update(&(gimbal->param->motor.yaw_dm_motor));
MOTOR_DM_t *dm_motor = MOTOR_DM_GetMotor(&(gimbal->param->motor.yaw_dm_motor));
if (dm_motor != NULL) {
gimbal->feedback.motor.yaw = dm_motor->motor.feedback;
}
}
if(gimbal->param->pit_dm_motor.module==MOTOR_DM_J4310){
MOTOR_DM_Update(&(gimbal->param->pit_dm_motor));
MOTOR_DM_t *dm_motor = MOTOR_DM_GetMotor(&(gimbal->param->pit_dm_motor));
if(gimbal->param->motor.pit==DM){
MOTOR_DM_Update(&(gimbal->param->motor.pit_dm_motor));
MOTOR_DM_t *dm_motor = MOTOR_DM_GetMotor(&(gimbal->param->motor.pit_dm_motor));
if (dm_motor != NULL) {
gimbal->feedback.motor.pit = dm_motor->motor.feedback;
}
@ -239,11 +240,18 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
float delta_yaw = g_cmd->delta_yaw*g->dt;
float delta_pit = g_cmd->delta_pit*g->dt;
float motor_imu_offset = g->feedback.motor.yaw.rotor_abs_angle - g->feedback.imu.eulr.yaw;
/* 处理跨越±π的情况 */
if (motor_imu_offset > M_PI) motor_imu_offset -= M_2PI;
if (motor_imu_offset < -M_PI) motor_imu_offset += M_2PI;
/* 计算到限位边界的距离 （这个限位给双yaw）*/
float delta_max = CircleError(g->limit.set_yaw.max,
(g->setpoint.eulr.yaw + motor_imu_offset + delta_yaw), M_2PI);
float delta_min = CircleError(g->limit.set_yaw.min,
(g->setpoint.eulr.yaw + motor_imu_offset + delta_yaw), M_2PI);
switch (g->mode) {
case GIMBAL_MODE_RELAX:/*放松模式*/
g->out.yaw = 0.0f;
@ -254,7 +262,7 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
g->setpoint.eulr.yaw+=delta_yaw;
g->setpoint.small_yaw+=delta_yaw;
if (g->param->travel.yaw > 0)
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{
CircleAdd(&(g->setpoint.eulr.yaw), delta_yaw, M_2PI);
/*限制在-3.14~3.14*/
@ -326,25 +334,23 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
*/
void Gimbal_Output(Gimbal_t *g){
/*大疆电机输出*/
if(g->param->yaw_rm_motor.module==MOTOR_GM6020){
MOTOR_RM_Ctrl(&g->param->yaw_rm_motor);
MOTOR_RM_SetOutput(&g->param->yaw_rm_motor, g->out.yaw);
if(g->param->motor.yaw==RM){
MOTOR_RM_Ctrl(&g->param->motor.yaw_rm_motor);
MOTOR_RM_SetOutput(&g->param->motor.yaw_rm_motor, g->out.yaw);
}
if(g->param->pit_rm_motor.module==MOTOR_GM6020){
MOTOR_RM_Ctrl(&g->param->pit_rm_motor);
MOTOR_RM_SetOutput(&g->param->pit_rm_motor, g->out.pit);//
if(g->param->motor.pit==RM){
MOTOR_RM_Ctrl(&g->param->motor.pit_rm_motor);
MOTOR_RM_SetOutput(&g->param->motor.pit_rm_motor, g->out.pit);//
}
/*达妙电机输出*/
if(g->param->pit_dm_motor.module==MOTOR_DM_J4310){
if(g->param->motor.pit==DM){
g->out.pit_dm.torque= g->out.pit * 5.0f; // 乘以减速比
MOTOR_DM_MITCtrl(&g->param->pit_dm_motor,&(g->out.pit_dm));
MOTOR_DM_MITCtrl(&g->param->motor.pit_dm_motor,&(g->out.pit_dm));
}
if(g->param->yaw_dm_motor.module==MOTOR_DM_J4310){
if(g->param->motor.yaw==DM){
g->out.yaw_dm.torque= g->out.yaw * 5.0f; // 乘以减速比
MOTOR_DM_MITCtrl(&g->param->yaw_dm_motor,&(g->out.yaw_dm));
MOTOR_DM_MITCtrl(&g->param->motor.yaw_dm_motor,&(g->out.yaw_dm));
}
}

33
User/module/gimbal.h
View File

@ -30,6 +30,11 @@ typedef enum {
GIMBAL_MODE_RELATIVE, /* 相对坐标系控制，控制相对于底盘的姿态 */
} Gimbal_Mode_t;
typedef enum {
DM, /*大妙*/
RM, /*大疆 */
} Gimbal_MOTOR;
typedef struct {
Gimbal_Mode_t mode;
float delta_yaw;
@ -42,15 +47,23 @@ typedef struct {
float min;
} Gimbal_Limit_t;
typedef struct {
Gimbal_MOTOR pit;
Gimbal_MOTOR yaw;
MOTOR_DM_Param_t pit_dm_motor; /* pitch轴电机参数 */
MOTOR_DM_Param_t yaw_dm_motor; /* yaw轴电机参数 */
MOTOR_RM_Param_t pit_rm_motor; /* pitch轴电机参数 */
MOTOR_RM_Param_t yaw_rm_motor; /* yaw轴电机参数 */
} Gimbal_MOTOR_Param_t;
/* 云台参数的结构体，包含所有初始化用的参数，通常是const，存好几组。*/
typedef struct {
uint16_t yaw_num;
MOTOR_RM_Param_t pit_rm_motor; /* pitch轴电机参数 */
MOTOR_RM_Param_t yaw_rm_motor; /* yaw轴电机参数 */
MOTOR_DM_Param_t pit_dm_motor;
MOTOR_DM_Param_t yaw_dm_motor;
Gimbal_MOTOR_Param_t motor;
struct {
KPID_Params_t yaw_omega; /* yaw轴角速度环PID参数 */
@ -62,9 +75,7 @@ typedef struct {
KPID_Params_t yaw_ecd_angle; /* yaw轴电机位置环PID参数 */
KPID_Params_t pit_velocity; /* pitch轴电机速度环PID参数 */
KPID_Params_t pit_ecd_angle; /* pitch轴电机位置环PID参数 */
} pid;
/* 低通滤波器截止频率 */
@ -131,13 +142,13 @@ typedef struct {
typedef struct {
uint64_t lask_wakeup;
float dt;
uint16_t yaw_num;
Gimbal_Params_t *param; /* 云台的参数，用Gimbal_Init设定 */
/* 模块通用 */
Gimbal_Mode_t mode; /* 云台模式 */
Gimbal_MOTOR MOTOR;
/* PID计算的目标值 */
struct {
AHRS_Eulr_t eulr; /* 表示云台姿态的欧拉角 */