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

MDK-ARM/gimbal/gimbal.__i

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

MDK-ARM/gimbal/gimbal.build_log.htm

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

MDK-ARM/gimbal/gimbal_gimbal.dep

@@ -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)
+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)(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)
@@ -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)

User/module/config.c

@@ -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},	
 						/*达妙电机参数自己配*/
-	.dm_Params_t={
+					.pit_dm_motor={},
-	.yaw_dm={.kd=0.3,},
+					.yaw_dm_motor={},
-	.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,
+                .k = 1.0f,
-                .p = 0.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,
+                .k = 1.0f,
-                .p =	0.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,

User/module/gimbal.c

@@ -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)
+	if(g->param->motor.yaw==RM)
-			MOTOR_RM_Register(&(g->param->yaw_rm_motor));
+			MOTOR_RM_Register(&(g->param->motor.yaw_rm_motor));
-	if(g->param->pit_rm_motor.module==MOTOR_GM6020)
+	if(g->param->motor.pit==RM)
-			MOTOR_RM_Register(&(g->param->pit_rm_motor));
+			MOTOR_RM_Register(&(g->param->motor.pit_rm_motor));
 	
 	/*达妙电机注册*/
-	if(g->param->yaw_dm_motor.module==MOTOR_DM_J4310){
+	if(g->param->motor.yaw==DM){
-			MOTOR_DM_Register(&(g->param->yaw_dm_motor));
+			MOTOR_DM_Register(&(g->param->motor.yaw_dm_motor));
-			MOTOR_DM_Enable(&(g->param->yaw_dm_motor));
+			MOTOR_DM_Enable(&(g->param->motor.yaw_dm_motor));
 	}
 	
-	if(g->param->pit_dm_motor.module==MOTOR_DM_J4310){
+	if(g->param->motor.pit==DM){
-			MOTOR_DM_Register(&(g->param->pit_dm_motor));
+			MOTOR_DM_Register(&(g->param->motor.pit_dm_motor));
-			MOTOR_DM_Enable(&(g->param->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){
+	if(gimbal->param->motor.yaw==RM){
-		MOTOR_RM_Update(&(gimbal->param->yaw_rm_motor));
+		MOTOR_RM_Update(&(gimbal->param->motor.yaw_rm_motor));
-		MOTOR_RM_t *rm_motor_yaw = MOTOR_RM_GetMotor(&(gimbal->param->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){
+	if(gimbal->param->motor.yaw==DM){
-		MOTOR_DM_Update(&(gimbal->param->yaw_dm_motor));
+		MOTOR_DM_Update(&(gimbal->param->motor.yaw_dm_motor));
-		MOTOR_DM_t *dm_motor = MOTOR_DM_GetMotor(&(gimbal->param->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){
+	if(gimbal->param->motor.pit==DM){
-		MOTOR_DM_Update(&(gimbal->param->pit_dm_motor));
+		MOTOR_DM_Update(&(gimbal->param->motor.pit_dm_motor));
-		MOTOR_DM_t *dm_motor = MOTOR_DM_GetMotor(&(gimbal->param->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,10 +240,17 @@ 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:/*放松模式*/
@@ -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){
+	if(g->param->motor.yaw==RM){
-			MOTOR_RM_Ctrl(&g->param->yaw_rm_motor);
+			MOTOR_RM_Ctrl(&g->param->motor.yaw_rm_motor);
-			MOTOR_RM_SetOutput(&g->param->yaw_rm_motor, g->out.yaw);
+			MOTOR_RM_SetOutput(&g->param->motor.yaw_rm_motor, g->out.yaw);
 	}
-	if(g->param->pit_rm_motor.module==MOTOR_GM6020){
+	if(g->param->motor.pit==RM){
-			MOTOR_RM_Ctrl(&g->param->pit_rm_motor);
+			MOTOR_RM_Ctrl(&g->param->motor.pit_rm_motor);
-			MOTOR_RM_SetOutput(&g->param->pit_rm_motor, g->out.pit);//
+			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));	
 		}
 	
-
-		
 }

User/module/gimbal.h

@@ -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;
 
-/* 云台参数的结构体，包含所有初始化用的参数，通常是const，存好几组。*/
+
 typedef struct {
-	uint16_t yaw_num;
+	
+	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 {
+	
+	Gimbal_MOTOR_Param_t motor;
 
-	MOTOR_DM_Param_t pit_dm_motor;
-	MOTOR_DM_Param_t yaw_dm_motor;
 	
   struct {
     KPID_Params_t yaw_omega; /* yaw轴角速度环PID参数 */
@@ -63,8 +76,6 @@ typedef struct {
     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; /* 表示云台姿态的欧拉角 */