新版云台，适配哨兵，自瞄

MDK-ARM/gimbal/gimbal.build_log.htm

@@ -21,7 +21,7 @@ Target DLL:      CMSIS_AGDI.dll V1.32.13.0
 Dialog DLL:      TCM.DLL V1.48.0.0
  
 <h2>Project:</h2>
-D:\yunha\gimbal\MDK-ARM\gimbal.uvprojx
+D:\yunha\1\git_gimbal\gimbal\MDK-ARM\gimbal.uvprojx
 Project File Date:  10/25/2025
 
 <h2>Output:</h2>
@@ -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 ai.c...
+compiling config.c...
+compiling init.c...
+compiling atti_esti.c...
+compiling remote.c...
+compiling gimbal_ctrl.c...
 compiling gimbal.c...
 linking...
-Program Size: Code=84848 RO-data=996 RW-data=404 ZI-data=34268  
+Program Size: Code=85896 RO-data=996 RW-data=404 ZI-data=34388  
 "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:02
+Build Time Elapsed:  00:00:01
 </pre>
 </body>
 </html>

MDK-ARM/gimbal/gimbal_gimbal.dep

@@ -2118,7 +2118,7 @@ I (..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_tim.h)(0x68B05645)
 I (..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_tim_ex.h)(0x68B05645)
 I (..\Drivers\STM32F4xx_HAL_Driver\Inc\stm32f4xx_hal_uart.h)(0x68B05645)
 I (..\User\component\calc_lib.h)(0x62054DF2)
-F (..\User\device\ai.c)(0x68F37707)(-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/ai.o -MD)
+F (..\User\device\ai.c)(0x6941C432)(-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/ai.o -MD)
 I (..\User\device\ai.h)(0x68DD46B5)
 I (..\User\component\user_math.h)(0x68F22FA9)
 I (D:\Keil_v5\ARM\ARMCLANG\include\float.h)(0x6035A4A0)
@@ -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)(0x6917284C)
+I (..\User\module\gimbal.h)(0x69425C1B)
 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)(0x6917284C)
+I (..\User\module\gimbal.h)(0x69425C1B)
 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)(0x6917284C)
+I (..\User\module\gimbal.h)(0x69425C1B)
 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)(0x6917284C)
+I (..\User\module\gimbal.h)(0x69425C1B)
 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)(0x6917284C)
+I (..\User\module\gimbal.h)(0x69425C1B)
 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)(0x692C2F69)(-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)(0x69425C1B)(-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)(0x6917284C)
+I (..\User\module\gimbal.h)(0x69425C1B)
 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)(0x691727B8)(-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)(0x69425C1B)(-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)(0x6917284C)
+I (..\User\module\gimbal.h)(0x69425C1B)
 I (..\User\component\ahrs.h)(0x68F3410E)
 I (..\User\component\filter.h)(0x68F22FA9)
 I (..\User\component\pid.h)(0x68F22FA9)

User/module/config.c

@@ -18,31 +18,43 @@
 Config_RobotParam_t robot_config = {
 
 .gimbal_param = {
-
+	/* 云台欧拉角与角速度自由选择 */
+	.Direction={
+			.Eulr={
+					.pit=Pit,
+					.yaw=Yaw,			
+				},
+			.Gyro={
+					.pit=Gyro_x,
+					.yaw=Gyro_z,
+			},	
+	},
 	/*欧拉角限位和电机角度限位*/
 	 .Limit_t= {
 						.pit_max=0.69,
 						.pit_min=-0.47,
-						.yaw_max= 1.0,//yaw的
+		 			/*零点参数*/
-						.yaw_min=-1.0,
+				.zero={
-	},
+					.yaw_encoder=0,
+					
+					.travel={
+						.yaw=1.06,				
+					},
+				},
+			},
 	 
 
 			.feedforward={
 				.imu = {
 					.yaw=false,
 					.pit=false,
-					.coefficient_yaw=0,
+					.K_yaw=0,
-					.coefficient_pit=0,
+					.K_pit=0,
 					},
 
 			},
 			
-			/*零点参数*/
+
-				.zero={
-					.pit_encoder=0,
-					.yaw_encoder=0,
-				},
 				
 				.motor={
 					/*按自己需求选择电机*/
@@ -122,5 +134,4 @@ Config_RobotParam_t robot_config = {
  */
 Config_RobotParam_t* Config_GetRobotParam(void) {
     return &robot_config;
-}
+}
-

User/module/gimbal.c

@@ -51,9 +51,6 @@ static float motor_imu_offset(float* motor, float* imu){
 	return motor_imu_offset;
 }
 
-
-
-
 /**
  * \brief 设置云台模式
  *
@@ -68,14 +65,19 @@ static int8_t Gimbal_SetMode(Gimbal_t *g, Gimbal_Mode_t mode) {
   if (mode == g->mode)
     return GIMBAL_OK;
 
+	PID_Reset(&g->pid.major_yaw_angle);
+  PID_Reset(&g->pid.major_yaw_omega);
+	
   PID_Reset(&g->pid.yaw_angle);
   PID_Reset(&g->pid.yaw_omega);
   PID_Reset(&g->pid.pit_angle);
   PID_Reset(&g->pid.pit_omega);
+  LowPassFilter2p_Reset(&g->filter_out.major_yaw, 0.0f);	
+	
   LowPassFilter2p_Reset(&g->filter_out.yaw, 0.0f);
   LowPassFilter2p_Reset(&g->filter_out.pit, 0.0f);
 
-//  MOTOR_DM_Enable(&(g->param->yaw_motor));
+
   
   AHRS_ResetEulr(&(g->setpoint.eulr)); /* 切换模式后重置设定值 */
 	
@@ -107,6 +109,11 @@ int8_t Gimbal_Init(Gimbal_t *g,Gimbal_Params_t *param,
   g->mode = GIMBAL_MODE_RELAX; /* 设置默认模式 */
 
   /* 初始化云台电机控制PID和LPF */
+	PID_Init(&(g->pid.major_yaw_angle), KPID_MODE_CALC_D, target_freq,
+           &(g->param->pid.major_yaw_angle));
+  PID_Init(&(g->pid.major_yaw_omega), KPID_MODE_CALC_D, target_freq,
+           &(g->param->pid.major_yaw_omega));
+	
   PID_Init(&(g->pid.yaw_angle), KPID_MODE_CALC_D, target_freq,
            &(g->param->pid.yaw_angle));
   PID_Init(&(g->pid.yaw_omega), KPID_MODE_CALC_D, target_freq,
@@ -115,28 +122,30 @@ int8_t Gimbal_Init(Gimbal_t *g,Gimbal_Params_t *param,
            &(g->param->pid.pit_angle));
   PID_Init(&(g->pid.pit_omega), KPID_MODE_CALC_D, target_freq,
            &(g->param->pid.pit_omega));
+
+
 	
-  PID_Init(&(g->pid.yaw_ecd_angle), KPID_MODE_CALC_D, target_freq,
+	g->limit.yaw.max = g->param->Limit_t.zero.yaw_encoder+g->param->Limit_t.zero.travel.yaw;			 
-           &(g->param->pid.yaw_ecd_angle));
+	g->limit.yaw.min = g->param->Limit_t.zero.yaw_encoder;
-  PID_Init(&(g->pid.yaw_velocity), KPID_MODE_CALC_D, target_freq,
+
-           &(g->param->pid.yaw_velocity));
+  LowPassFilter2p_Init(&g->filter_out.major_yaw, target_freq,
-  PID_Init(&(g->pid.pit_ecd_angle), KPID_MODE_CALC_D, target_freq,
+                       g->param->low_pass_cutoff_freq.out);	
-           &(g->param->pid.pit_ecd_angle));
-  PID_Init(&(g->pid.pit_velocity), KPID_MODE_CALC_D, target_freq,
-           &(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,
                        g->param->low_pass_cutoff_freq.out);
   LowPassFilter2p_Init(&g->filter_out.pit, target_freq,
                        g->param->low_pass_cutoff_freq.out);
 
-	
-
-	
   BSP_CAN_Init();
+	/* 大yaw电机注册 */
+	if(g->param->motor.major_yaw==RM)
+			MOTOR_RM_Register(&(g->param->motor.major_yaw_rm_motor));	
+	
+	if(g->param->motor.major_yaw==DM){
+			MOTOR_DM_Register(&(g->param->motor.major_yaw_dm_motor));
+			MOTOR_DM_Enable(&(g->param->motor.major_yaw_dm_motor));
+	}	
+	
 	/*大疆电机注册*/
 	if(g->param->motor.yaw==RM)
 			MOTOR_RM_Register(&(g->param->motor.yaw_rm_motor));
@@ -154,11 +163,68 @@ int8_t Gimbal_Init(Gimbal_t *g,Gimbal_Params_t *param,
 			MOTOR_DM_Enable(&(g->param->motor.pit_dm_motor));
 	}
 	
-
-	
   return 0;
 }
+static int8_t Gimbal_Direction(Gimbal_t *g){
 
+  if (g == NULL) {
+    return -1;
+  }
+	/* 欧拉角方向选择 */
+	switch(g->param->Direction.Eulr.pit){
+		case Pit:
+			g->direction.Eulr.pit=g->feedback.imu.eulr.pit;
+			break;
+		case Yaw:
+			g->direction.Eulr.pit=g->feedback.imu.eulr.yaw;
+			break;	
+		case Rol:
+			g->direction.Eulr.pit=-g->feedback.imu.eulr.rol;
+			break;	
+	}
+	
+	switch(g->param->Direction.Eulr.yaw){
+		case Pit:
+			g->direction.Eulr.yaw=g->feedback.imu.eulr.pit;
+			break;
+		case Yaw:
+			g->direction.Eulr.yaw=g->feedback.imu.eulr.yaw;
+			break;	
+		case Rol:
+			g->direction.Eulr.yaw=g->feedback.imu.eulr.rol;
+			break;	
+	}	
+	/* 角速度方向选择 */
+	switch(g->param->Direction.Gyro.pit){
+		case Gyro_x:
+			g->direction.Gyro.pit=g->feedback.imu.gyro.x;
+			break;
+		case Gyro_y:
+			g->direction.Gyro.pit=g->feedback.imu.gyro.y;
+			break;	
+		case Gyro_z:
+			g->direction.Gyro.pit=g->feedback.imu.gyro.z;
+			break;	
+	}
+	
+	switch(g->param->Direction.Gyro.yaw){
+		case Gyro_x:
+			g->direction.Gyro.yaw=g->feedback.imu.gyro.x;
+			break;
+		case Gyro_y:
+			g->direction.Gyro.yaw=g->feedback.imu.gyro.y;
+			break;	
+		case Gyro_z:
+			g->direction.Gyro.yaw=g->feedback.imu.gyro.z;
+			break;	
+	}
+			g->direction.Quaternion_t.q0=g->feedback.imu.quat.q0;
+			g->direction.Quaternion_t.q1=g->feedback.imu.quat.q1;	
+			g->direction.Quaternion_t.q2=g->feedback.imu.quat.q2;
+			g->direction.Quaternion_t.q3=g->feedback.imu.quat.q3;		
+	
+    return 0;
+}
 /**
  * \brief 通过CAN设备更新云台反馈信息
  *
@@ -171,6 +237,23 @@ int8_t Gimbal_UpdateFeedback(Gimbal_t *gimbal) {
   if (gimbal == NULL)
     return -1;
 
+	
+		  /* 更新大yaw电机反馈数据（RM和DM） */	
+	if(gimbal->param->motor.major_yaw==RM){
+		MOTOR_RM_Update(&(gimbal->param->motor.major_yaw_rm_motor));
+		MOTOR_RM_t *rm_motor_yaw = MOTOR_RM_GetMotor(&(gimbal->param->motor.major_yaw_rm_motor));
+		if(rm_motor_yaw != NULL)
+		 gimbal->feedback.motor.major_yaw = rm_motor_yaw->feedback;
+	}	
+	
+	if(gimbal->param->motor.major_yaw==DM){
+		MOTOR_DM_Update(&(gimbal->param->motor.major_yaw_dm_motor));
+		MOTOR_DM_t *dm_motor = MOTOR_DM_GetMotor(&(gimbal->param->motor.major_yaw_dm_motor));
+			if (dm_motor != NULL) {
+				gimbal->feedback.motor.major_yaw = dm_motor->motor.feedback;
+		}
+	}	
+	
   /* 更新RM电机反馈数据 */
 	if(gimbal->param->motor.yaw==RM){
 		MOTOR_RM_Update(&(gimbal->param->motor.yaw_rm_motor));
@@ -193,14 +276,14 @@ int8_t Gimbal_UpdateFeedback(Gimbal_t *gimbal) {
 		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->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;
-  }
+		}
 	}
 
   return 0;
@@ -213,7 +296,66 @@ int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu){
   }
   gimbal->feedback.imu.gyro = imu->gyro;
   gimbal->feedback.imu.eulr = imu->eulr;
+  gimbal->feedback.imu.accl = imu->accl;
+  gimbal->feedback.imu.quat = imu->quat;	
+	Gimbal_Direction(gimbal);
 }
+/**
+ * \brief 运行大yaw控制逻辑
+ *
+ * \param g 包含云台数据的结构体
+ * \param 
+ *
+ * \return 函数运行结果
+ */
+ int8_t major_yaw_Control(Gimbal_t *g){
+
+	 /*处理大yaw控制命令，软件限位 - 使用电机绝对角度*/
+ /*获得小YAW的中点位置，如果小YAW大于中点的一定范围，大YAW开始运动，
+ 跟随小YAW向相同方向运动，将遥控数据进行分配*/
+
+ /*小YAW中点*/
+ float small_yaw_center_offset = g->param->Limit_t.zero.yaw_encoder  + 
+																 g->param->Limit_t.zero.travel.yaw* 0.5f;
+	g->setpoint.major_yaw=small_yaw_center_offset;
+ 
+// /* 小yaw是否开启 */
+//	float err = g->feedback.motor.yaw.rotor_abs_angle - small_yaw_center_offset; 
+
+//	float threshold = (g->param->Limit_t.zero.travel.yaw * 0.35f);
+//    /* 超出阈值范围，让大yaw跟随 */
+//	if (fabsf(err) > threshold) {
+//		/* 此时让大yaw动起来，跟随err的符号 */
+////		g->setpoint.major_yaw = g->feedback.motor.major_yaw.rotor_abs_angle - err;
+//    g->setpoint.major_yaw = small_yaw_center_offset;;
+//	} else {
+//    /* 阈值以内，大yaw不动*/
+//    g->setpoint.major_yaw = g->feedback.motor.major_yaw.rotor_abs_angle; //当前值，不动
+//	}
+	return 0;
+}
+
+
+int8_t Gimbal_Control_mode(Gimbal_t *g,Gimbal_CMD_t *g_cmd){
+	  if (g == NULL || g_cmd == NULL) {
+    return -1;
+  }
+		switch(g_cmd->ctrl_mode){
+			case GIMBAL_MODE_REMOTE:
+				g->setpoint.eulr.yaw+=(g_cmd->delta_yaw*g->dt);	
+				g->setpoint.eulr.pit+=(g_cmd->delta_pit*g->dt);
+				break;
+			case GIMBAL_MODE_AI:				
+			g->setpoint.eulr.yaw=g_cmd->set_yaw;	
+			g->setpoint.eulr.pit=g_cmd->set_pit;			
+				break;
+			case GIMBAL_MODE_NUC:
+				/* 可能用不上，还在想 */
+				break;			
+		}
+		return GIMBAL_OK;
+};
+
 
 /**
  * \brief 运行云台控制逻辑
@@ -223,7 +365,6 @@ int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu){
  *
  * \return 函数运行结果
  */
-
 int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
   if (g == NULL || g_cmd == NULL) {
     return -1;
@@ -232,23 +373,14 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
   g->dt = (BSP_TIME_Get_us() - g->lask_wakeup) / 1000000.0f;
   g->lask_wakeup = BSP_TIME_Get_us();
   Gimbal_SetMode(g, g_cmd->mode);
-
+	/* 双yaw的大yaw,setpoint从这设立 */
+	major_yaw_Control(g);
   /* 欧拉角控制相关逻辑 */
-  float yaw_omega_set_point, pit_omega_set_point;
+  float yaw_omega_set_point, pit_omega_set_point,major_yaw_omega_set_point;
-	/* 电机角度控制相关逻辑 */
-	float yaw_velocity_set_point, pit_velocity_set_point;
-
-
-	
-	float delta_ecd_yaw = g_cmd->delta_yaw*g->dt;
-	float delta_ecd_pit = g_cmd->delta_pit*g->dt;
 	
 	float delta_yaw = g_cmd->delta_yaw*g->dt;
 	float delta_pit = g_cmd->delta_pit*g->dt;
 	
-
-	
-
 		float yaw_motor_imu_offset=motor_imu_offset(&g->feedback.motor.yaw.rotor_abs_angle,
 			&g->feedback.imu.eulr.yaw);
 
@@ -259,98 +391,127 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
 			
   switch (g->mode) {
   case GIMBAL_MODE_RELAX:/*放松模式*/
-    g->out.yaw = 0.0f;
+ 
+    g->out.major_yaw = 0.0f;
+		g->out.yaw = 0.0f;
     g->out.pit = 0.0f;
     break;
   case GIMBAL_MODE_ABSOLUTE:/*绝对模式*/
-	
+		/* 手动和自瞄 */
-		g->setpoint.eulr.yaw+=delta_yaw;	
+		Gimbal_Control_mode(g, g_cmd);
 		if (g->param->motor.limit_yaw==true)
 		Clip(&(g->setpoint.eulr.yaw),delta_min,delta_max);		
 		else{
 		/*限制在-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;			
 		}
 
+
   /* 处理pitch控制命令，软件限位 - 使用电机绝对角度 */
-	g->setpoint.eulr.pit+=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);	
-
+ /*4310大YAW控制
+    这里是单环控制的，有需要加双环
+    */
+		
+		
+    major_yaw_omega_set_point =PID_Calc(&g->pid.major_yaw_angle,
+																				 g->setpoint.major_yaw,
+																				 g->feedback.motor.yaw.rotor_abs_angle,
+																				 g->feedback.motor.yaw.rotor_speed,g->dt);
+//	     g->out.yaw_4310 = PID_Calc(&g->pid.yaw_4310_omega,yaw_omega_set_point,
+//                               g->feedback.imu.gyro.z,0.0f,g->dt);
+    g->out.major_yaw = major_yaw_omega_set_point;  // 直接输出速度环目标值作为电机输出		
+		
     yaw_omega_set_point = PID_Calc(&(g->pid.yaw_angle), g->setpoint.eulr.yaw,
-                                   g->feedback.imu.eulr.yaw, 0.0f, g->dt);
+                                   g->direction.Eulr.yaw, 0.0f, g->dt);
     g->out.yaw = PID_Calc(&(g->pid.yaw_omega), yaw_omega_set_point,
-                          g->feedback.imu.gyro.z, 0.f, g->dt);
+                          g->direction.Gyro.yaw, 0.f, g->dt);
 
 		pit_omega_set_point = PID_Calc(&(g->pid.pit_angle), g->setpoint.eulr.pit,
-                                   g->feedback.imu.eulr.pit, 0.0f, g->dt);
+                                   g->direction.Eulr.pit, 0.0f, g->dt);
-    g->out.pit = PID_Calc(&(g->pid.pit_omega), pit_omega_set_point,
+		g->out.pit = PID_Calc(&(g->pid.pit_omega), pit_omega_set_point,
-                          g->feedback.imu.gyro.x, 0.f, g->dt);
+													g->feedback.imu.gyro.y, 0.f, g->dt);
-	
+
 		/*前馈添加*/
 		if(g->param->feedforward.imu.yaw==true)
-		g->out.yaw+=g->param->feedforward.imu.coefficient_yaw*(yaw_omega_set_point - g->feedback.imu.gyro.z);
+		g->out.yaw+=g->param->feedforward.imu.K_yaw*(yaw_omega_set_point - g->feedback.imu.gyro.z);
 		if(g->param->feedforward.imu.pit==true)
-		g->out.pit+=g->param->feedforward.imu.coefficient_pit*(pit_omega_set_point - g->feedback.imu.gyro.x);
+		g->out.pit+=g->param->feedforward.imu.K_pit*(pit_omega_set_point - g->feedback.imu.gyro.x);
 		
     break;
 	case GIMBAL_MODE_RELATIVE:
 		/*计算零点*/
-		g->zero.travel.yaw=motor_imu_offset(&g->feedback.motor.yaw.rotor_abs_angle,&g->feedback.imu.eulr.yaw);
+		CircleAdd(&(g->relative_angle.yaw), delta_yaw, M_2PI);
-		g->zero.travel.pit=motor_imu_offset(&g->feedback.motor.pit.rotor_abs_angle,&g->feedback.imu.eulr.pit);	
+		
-	
+		/*  -PI~PI 归一化 */
-		g->zero.yaw = g->param->zero.yaw_encoder - g->zero.travel.yaw;
+		if(g->relative_angle.yaw > M_PI) g->relative_angle.yaw -= M_2PI;
-		g->zero.pit = g->param->zero.pit_encoder - g->zero.travel.pit;	
+		if(g->relative_angle.yaw < -M_PI) g->relative_angle.yaw += M_2PI;
-		/*基于零点的设定角度*/
-		/*加的相对角度应该限制在3.14~-3.14*/
-		CircleAdd(&(g->relative_angle.yaw),delta_yaw,M_2PI);
-		if((g->relative_angle.yaw)>=M_2PI)g->relative_angle.yaw-=M_2PI;
-		/*限制pit的累加角度*/
-		g->relative_angle.pit=+delta_pit;
-		Clip(&(g->relative_angle.pit),(g->param->Limit_t.pit_min+g->zero.pit),
-		(g->param->Limit_t.pit_max-g->zero.pit));
-	
-		g->setpoint.eulr.yaw=g->zero.yaw + g->relative_angle.yaw;
-		g->setpoint.eulr.pit=g->zero.pit + g->relative_angle.pit;
 
-		/*yaw的限位*/	
+		/*  计算底盘当前在世界坐标系的方向 (坐标系转换) */
-		if (g->param->motor.limit_yaw==true)
+		/* 底盘Yaw(世界) = 云台Yaw(世界) - 云台电机Yaw(机械) */
-		Clip(&(g->setpoint.eulr.yaw),delta_min,delta_max);
+		
-		else{
+		float current_chassis_yaw_world=-motor_imu_offset(&g->feedback.motor.yaw.rotor_abs_angle,&g->feedback.imu.eulr.yaw);
-		/*限制在-3.14~3.14*/
+		/* 归一化底盘角度 */
-		if(g->setpoint.eulr.yaw > M_PI) g->setpoint.eulr.yaw -= M_2PI;
+		if(current_chassis_yaw_world > M_PI) current_chassis_yaw_world -= M_2PI;
-		}
+		if(current_chassis_yaw_world < -M_PI) current_chassis_yaw_world += M_2PI;
-		/* 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);		
+		/*  最终的欧拉角控制目标 (Setpoint) */
-  
+		/* 目标(世界) = 底盘(世界) + 期望相对角(相对) */
+		g->setpoint.eulr.yaw = current_chassis_yaw_world + g->relative_angle.yaw;
+		
+		/* 再次归一化 Setpoint，防止数值溢出 */
+		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;
+
+		/*  Pitch 轴处理 ( Pitch 保持绝对水平，不随底盘颠簸) */
+		g->setpoint.eulr.pit += delta_pit; 
+		/* Pitch 限位 */
+		if (g->param->motor.limit_pit == true)
+		Clip(&(g->setpoint.eulr.pit), g->param->Limit_t.pit_min, g->param->Limit_t.pit_max);
+
+ /*4310大YAW控制
+    这里是单环控制的，有需要加双环
+    */
+    major_yaw_omega_set_point =PID_Calc(&g->pid.major_yaw_angle,
+																				 g->setpoint.major_yaw,
+																				 g->feedback.motor.major_yaw.rotor_abs_angle,
+																				 g->feedback.motor.major_yaw.rotor_speed,g->dt);
+//	     g->out.yaw_4310 = PID_Calc(&g->pid.yaw_4310_omega,yaw_omega_set_point,
+//                               g->feedback.imu.gyro.z,0.0f,g->dt);
+    g->out.major_yaw = major_yaw_omega_set_point;  // 直接输出速度环目标值作为电机输出				
+		
     yaw_omega_set_point = PID_Calc(&(g->pid.yaw_angle), g->setpoint.eulr.yaw,
-                                   g->feedback.imu.eulr.yaw, 0.0f, g->dt);
+                                   g->direction.Eulr.yaw, 0.0f, g->dt);
     g->out.yaw = PID_Calc(&(g->pid.yaw_omega), yaw_omega_set_point,
-                          g->feedback.imu.gyro.z, 0.f, g->dt);
+                          g->direction.Gyro.yaw, 0.f, g->dt);
 
 		pit_omega_set_point = PID_Calc(&(g->pid.pit_angle), g->setpoint.eulr.pit,
-                                   g->feedback.imu.eulr.pit, 0.0f, g->dt);
+                                   g->direction.Eulr.pit, 0.0f, g->dt);
-    g->out.pit = PID_Calc(&(g->pid.pit_omega), pit_omega_set_point,
+		g->out.pit = PID_Calc(&(g->pid.pit_omega), pit_omega_set_point,
-                          g->feedback.imu.gyro.x, 0.f, g->dt);
+													g->direction.Gyro.pit, 0.f, g->dt);
-		
 		/*前馈添加*/
 		if(g->param->feedforward.imu.yaw==true)
-		g->out.yaw+=g->param->feedforward.imu.coefficient_yaw*(yaw_omega_set_point - g->feedback.imu.gyro.z);
+		g->out.yaw+=g->param->feedforward.imu.K_yaw*(yaw_omega_set_point - g->feedback.imu.gyro.z);
 		if(g->param->feedforward.imu.pit==true)
-		g->out.pit+=g->param->feedforward.imu.coefficient_pit*(pit_omega_set_point - g->feedback.imu.gyro.x);
+		g->out.pit+=g->param->feedforward.imu.K_pit*(pit_omega_set_point - g->feedback.imu.gyro.x);
-	
 		break;
-
-		
   }
 	    /* 输出滤波 */
+    g->out.major_yaw = LowPassFilter2p_Apply(&g->filter_out.major_yaw, g->out.major_yaw);	
     g->out.yaw = LowPassFilter2p_Apply(&g->filter_out.yaw, g->out.yaw);
     g->out.pit = LowPassFilter2p_Apply(&g->filter_out.pit, g->out.pit);
 	 return 0;
 }
 
+
+
+
+
   /**
    * \brief 云台输出
    *
@@ -358,6 +519,25 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
    * \param out CAN设备云台输出结构体
    */
 void Gimbal_Output(Gimbal_t *g){
+	
+	/*大yaw电机输出*/
+	if(g->param->motor.major_yaw==RM){
+			MOTOR_RM_Ctrl(&g->param->motor.major_yaw_rm_motor);
+			MOTOR_RM_SetOutput(&g->param->motor.major_yaw_rm_motor, g->out.major_yaw);
+	}	
+
+	if(g->param->motor.major_yaw==DM){
+		
+			g->out.major_yaw_dm.angle=g->param->dm_Params_t.major_yaw_dm.angle;
+			g->out.major_yaw_dm.kd=g->param->dm_Params_t.major_yaw_dm.kd;
+			g->out.major_yaw_dm.kp=g->param->dm_Params_t.major_yaw_dm.kp;
+			g->out.major_yaw_dm.velocity=g->param->dm_Params_t.major_yaw_dm.velocity;
+		
+			g->out.major_yaw_dm.torque= g->out.major_yaw * g->param->dm_Params_t.major_yaw_dm_Reduction_ratio; // 乘以减速比
+		
+			 MOTOR_DM_MITCtrl(&g->param->motor.major_yaw_dm_motor,&(g->out.major_yaw_dm));
+	}	
+	
 	/*大疆电机输出*/
 	if(g->param->motor.yaw==RM){
 			MOTOR_RM_Ctrl(&g->param->motor.yaw_rm_motor);

User/module/gimbal.h

@@ -30,15 +30,50 @@ typedef enum {
   GIMBAL_MODE_RELATIVE, /* 相对坐标系控制，控制相对于底盘的姿态 */
 } Gimbal_Mode_t;
 
+typedef enum {
+  GIMBAL_MODE_REMOTE,    /* 遥控器模式 */
+  GIMBAL_MODE_NUC, 			 /* 操作手模式 */	
+  GIMBAL_MODE_AI, 			 /* 自瞄模式 */
+	
+} Gimbal_Control_Mode_t;
+
 typedef enum {
   DM,    /*大妙*/
   RM, 		/*大疆 */
-} Gimbal_MOTOR;
+} Gimbal_MOTOR_t;
+
+typedef enum {
+  Gyro_x,   
+  Gyro_y, 		
+	Gyro_z,
+} Gimbal_Gyro_t;
+
+typedef enum {
+  Pit,   
+  Yaw, 		
+	Rol,
+} Gimbal_Eulr_t;
+
+typedef enum {
+  Accl_x,   
+  Accl_y, 		
+	Accl_z,
+} Gimbal_Accl_t;
+
+typedef enum {
+  Q0,   
+  Q1, 		
+	Q2,
+	Q3,
+} Gimbal_Quat_t;
 
 typedef struct {
-  Gimbal_Mode_t mode;
+  Gimbal_Mode_t mode; 
+  Gimbal_Control_Mode_t ctrl_mode; 	
   float delta_yaw;
   float delta_pit;
+  float set_yaw;
+  float set_pit;	
 } Gimbal_CMD_t;
 
 /* 软件限位 */
@@ -54,53 +89,93 @@ typedef	struct{
   }travel;
   float pit;		/*零点*/
   float yaw;
-
 } Gimbal_zero_t;	
 
+
 typedef struct {
+		struct{
+		float pit;
+		float yaw;
+		float rol;
+		}Gyro;
+		
+		struct{
+		float pit;
+		float yaw;
+		float rol;
+		}Eulr;
+		
+		struct {
+		float q0;
+		float q1;
+		float q2;
+		float q3;
+		} Quaternion_t;
+		
+  } Gimbal_Direction_t;
+
+typedef struct {
+
 	bool limit_yaw;/*是否开启限位*/
 	bool limit_pit;
-	Gimbal_MOTOR pit;
+	
-	Gimbal_MOTOR yaw;
+	Gimbal_MOTOR_t major_yaw;	
+
+	Gimbal_MOTOR_t pit;
+	Gimbal_MOTOR_t yaw;
+	
+	MOTOR_DM_Param_t major_yaw_dm_motor; /* 大yaw轴电机参数 */
+  MOTOR_RM_Param_t major_yaw_rm_motor; /* 大yaw轴电机参数 */	
+	
   MOTOR_DM_Param_t pit_dm_motor; /* pitch轴电机参数 */
   MOTOR_DM_Param_t yaw_dm_motor; /* yaw轴电机参数 */
 	
 	MOTOR_RM_Param_t pit_rm_motor; /* pitch轴电机参数 */
   MOTOR_RM_Param_t yaw_rm_motor; /* yaw轴电机参数 */
 	
-}Gimbal_MOTOR_Param_t;
+}Gimbal_MOTOR_t_Param_t;
 
 /* 云台参数的结构体，包含所有初始化用的参数，通常是const，存好几组。*/
 typedef struct {
-	
-	Gimbal_MOTOR_Param_t motor;
 
-	
+	Gimbal_MOTOR_t_Param_t motor;
   struct {
+		KPID_Params_t major_yaw_omega; /* yaw轴角速度环PID参数 */
+    KPID_Params_t major_yaw_angle; /* yaw轴角位置环PID参数 */
+		
     KPID_Params_t yaw_omega; /* yaw轴角速度环PID参数 */
     KPID_Params_t yaw_angle; /* yaw轴角位置环PID参数 */
     KPID_Params_t pit_omega; /* pitch轴角速度环PID参数 */
     KPID_Params_t pit_angle; /* pitch轴角位置环PID参数 */
 		
-		KPID_Params_t yaw_velocity; /* yaw轴电机速度环PID参数 */
-    KPID_Params_t yaw_ecd_angle; 		/* yaw轴电机位置环PID参数 */
-    KPID_Params_t pit_velocity; /* pitch轴电机速度环PID参数 */
-    KPID_Params_t pit_ecd_angle; /* pitch轴电机位置环PID参数 */
-			
   } pid;
-	
+
   struct {
-	float pit_encoder;		/*零点*/
+		struct{
-  float yaw_encoder;
+		Gimbal_Gyro_t pit;
-		
+		Gimbal_Gyro_t yaw;
-  }zero;
+		Gimbal_Gyro_t rol;
+		}Gyro;
+		struct{
+		Gimbal_Eulr_t pit;
+		Gimbal_Eulr_t yaw;
+		Gimbal_Eulr_t rol;
+		}Eulr;
+		struct{
+		Gimbal_Accl_t pit;
+		Gimbal_Accl_t yaw;
+		Gimbal_Accl_t rol;
+		}Accl;
+  } Direction;
+
+	
 	  /* 前馈系数 */
   struct {
 		struct{
 		bool yaw;
 		bool pit;
-		float coefficient_yaw;  
+		float K_yaw;  
-    float coefficient_pit;			
+    float K_pit;			
 		}imu;
 		
   }feedforward;
@@ -116,12 +191,26 @@ typedef struct {
   float pit_max;		/*pit的限位*/
   float pit_min;
 	float yaw_max;		/*yaw的限位*/
-  float yaw_min;
+  float yaw_min;		
+		 struct {
+			float pit_encoder;		/*零点*/
+			float yaw_encoder;
+				struct{	
+						float yaw;
+						float pit;
+			}travel;
+  }zero;
+		
+		
 	}Limit_t;
 	
 	struct {
+
+	/*大yaw的参数*/
+	MOTOR_MIT_Output_t major_yaw_dm;			
 	MOTOR_MIT_Output_t yaw_dm;		/*达妙的参数*/
 	MOTOR_MIT_Output_t pit_dm;	
+	float major_yaw_dm_Reduction_ratio;						
 	float yaw_dm_Reduction_ratio;
 	float pit_dm_Reduction_ratio;	
 	}dm_Params_t;
@@ -130,11 +219,14 @@ typedef struct {
 typedef struct {
   AHRS_Gyro_t gyro;
   AHRS_Eulr_t eulr;
+  AHRS_Quaternion_t quat;		
+	AHRS_Accl_t accl;	
 } Gimbal_IMU_t;
 /* 云台反馈数据的结构体，包含反馈控制用的反馈数据 */
 typedef struct {
   Gimbal_IMU_t imu;
   struct {
+    MOTOR_Feedback_t major_yaw; /* 大yaw轴电机反馈 */
     MOTOR_Feedback_t yaw; /* yaw轴电机反馈 */
     MOTOR_Feedback_t pit; /* pitch轴电机反馈 */
   } motor;
@@ -142,8 +234,13 @@ typedef struct {
 
 /* 云台输出数据的结构体*/
 typedef struct {
+
+
+	float major_yaw;	
   float yaw; /* yaw轴电机输出 */
   float pit; /* pitch轴电机输出 */
+
+	MOTOR_MIT_Output_t major_yaw_dm;			
 	MOTOR_MIT_Output_t yaw_dm;		/*达妙的参数*/
 	MOTOR_MIT_Output_t pit_dm;	
 
@@ -156,30 +253,29 @@ typedef struct {
   uint64_t lask_wakeup;
   float dt;
 
-	
   Gimbal_Params_t *param; /* 云台的参数，用Gimbal_Init设定 */
 	
   /* 模块通用 */
+	Gimbal_Control_Mode_t ctrl_mode;
   Gimbal_Mode_t mode; /* 云台模式 */
-	Gimbal_MOTOR MOTOR;
+	Gimbal_MOTOR_t MOTOR;
 	Gimbal_zero_t zero;
+	Gimbal_Direction_t direction;
   /* PID计算的目标值 */
   struct {
+		float major_yaw;
     AHRS_Eulr_t eulr; /* 表示云台姿态的欧拉角 */
 		AHRS_Eulr_t ecd;
   } setpoint;
 
-  struct {
+  struct {	
+    KPID_t major_yaw_angle; /* 大yaw轴角位置环PID */
+    KPID_t major_yaw_omega; /* 大yaw轴角速度环PID */		
+		
     KPID_t yaw_angle; /* yaw轴角位置环PID */
     KPID_t yaw_omega; /* yaw轴角速度环PID */
     KPID_t pit_angle; /* pitch轴角位置环PID */
-    KPID_t pit_omega; /* pitch轴角速度环PID */
+    KPID_t pit_omega; /* pitch轴角速度环PID */	
-		
-		KPID_t yaw_velocity; /* yaw轴电机速度环PID参数 */
-    KPID_t yaw_ecd_angle; 		/* yaw轴电机位置环PID参数 */
-    KPID_t pit_velocity; /* pitch轴电机速度环PID参数 */
-    KPID_t pit_ecd_angle; /* pitch轴电机位置环PID参数 */
-		
   } pid;
 
   struct {
@@ -187,12 +283,15 @@ typedef struct {
     Gimbal_Limit_t pit;
   } limit;
 
-  struct {
+  struct {		
+		float major_yaw;
 		float yaw;
 		float pit;
   } relative_angle;
 	
   struct {
+    LowPassFilter2p_t major_yaw;		
+		
     LowPassFilter2p_t yaw;
     LowPassFilter2p_t pit;
   } filter_out;