revert 2f199a374d

revert 适配双yaw，pit，yaw的方向能直接选择

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

@@ -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\motor\gimbal\MDK-ARM\gimbal.uvprojx
+D:\yunha\gimbal\MDK-ARM\gimbal.uvprojx
 Project File Date:  10/25/2025
 
 <h2>Output:</h2>
@@ -31,9 +31,9 @@ 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 config.c...
+compiling gimbal.c...
 linking...
-Program Size: Code=84320 RO-data=996 RW-data=416 ZI-data=34280  
+Program Size: Code=84848 RO-data=996 RW-data=404 ZI-data=34268  
 "gimbal\gimbal.axf" - 0 Error(s), 0 Warning(s).
 
 <h2>Software Packages used:</h2>
@@ -58,7 +58,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)(0x6917284C)
 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)(0x6917284C)
 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)(0x6917284C)
 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)(0x6917284C)
 I (..\User\component\ahrs.h)(0x68F3410E)
 I (..\User\component\filter.h)(0x68F22FA9)
 I (..\User\component\pid.h)(0x68F22FA9)
@@ -2467,7 +2467,7 @@ I (..\Middlewares\Third_Party\FreeRTOS\Source\portable\RVDS\ARM_CM4F\portmacro.h
 I (..\Middlewares\Third_Party\FreeRTOS\Source\include\mpu_wrappers.h)(0x68B055DB)
 I (..\Middlewares\Third_Party\FreeRTOS\Source\include\task.h)(0x68B055DB)
 I (..\Middlewares\Third_Party\FreeRTOS\Source\include\list.h)(0x68B055DB)
-F (..\User\task\gimbal_ctrl.c)(0x68F23895)(-xc -std=c99 --target=arm-arm-none-eabi -mcpu=cortex-m4 -mfpu=fpv4-sp-d16 -mfloat-abi=hard -c

-fno-rtti -funsigned-char -fshort-enums -fshort-wchar

-gdwarf-3 -O0 -ffunction-sections -w -I ../Core/Inc -I ../Drivers/STM32F4xx_HAL_Driver/Inc -I ../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy -I ../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I ../Drivers/CMSIS/Include -I ../User/bsp -I ../User/component -I ../User/device -I ../Middlewares/Third_Party/FreeRTOS/Source/include -I ../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 -I ../Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F -I ../User/task -I ../User -I ../User/module

-I./RTE/_gimbal

-ID:/Keil_v5/Arm/Packs/ARM/CMSIS/5.7.0/CMSIS/Core/Include

-ID:/Keil_v5/Arm/Packs/Keil/STM32F4xx_DFP/2.17.1/Drivers/CMSIS/Device/ST/STM32F4xx/Include

-D__UVISION_VERSION="534" -D_RTE_ -DSTM32F407xx -D_RTE_ -DUSE_HAL_DRIVER -DSTM32F407xx

-o gimbal/gimbal_ctrl.o -MD)
+F (..\User\task\gimbal_ctrl.c)(0x6908B058)(-xc -std=c99 --target=arm-arm-none-eabi -mcpu=cortex-m4 -mfpu=fpv4-sp-d16 -mfloat-abi=hard -c

-fno-rtti -funsigned-char -fshort-enums -fshort-wchar

-gdwarf-3 -O0 -ffunction-sections -w -I ../Core/Inc -I ../Drivers/STM32F4xx_HAL_Driver/Inc -I ../Drivers/STM32F4xx_HAL_Driver/Inc/Legacy -I ../Drivers/CMSIS/Device/ST/STM32F4xx/Include -I ../Drivers/CMSIS/Include -I ../User/bsp -I ../User/component -I ../User/device -I ../Middlewares/Third_Party/FreeRTOS/Source/include -I ../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2 -I ../Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM4F -I ../User/task -I ../User -I ../User/module

-I./RTE/_gimbal

-ID:/Keil_v5/Arm/Packs/ARM/CMSIS/5.7.0/CMSIS/Core/Include

-ID:/Keil_v5/Arm/Packs/Keil/STM32F4xx_DFP/2.17.1/Drivers/CMSIS/Device/ST/STM32F4xx/Include

-D__UVISION_VERSION="534" -D_RTE_ -DSTM32F407xx -D_RTE_ -DUSE_HAL_DRIVER -DSTM32F407xx

-o gimbal/gimbal_ctrl.o -MD)
 I (..\User\task\user_task.h)(0x68DCD971)
 I (..\Middlewares\Third_Party\FreeRTOS\Source\CMSIS_RTOS_V2\cmsis_os2.h)(0x68B055DB)
 I (D:\Keil_v5\ARM\ARMCLANG\include\stdint.h)(0x6035A4A8)
@@ -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)(0x6917284C)
 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)(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)
-I (..\User\module\gimbal.h)(0x68FA3585)
+I (..\User\module\gimbal.h)(0x6917284C)
 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)(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)
 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)(0x6917284C)
 I (..\User\component\ahrs.h)(0x68F3410E)
 I (..\User\component\filter.h)(0x68F22FA9)
 I (..\User\component\pid.h)(0x68F22FA9)

User/module/config.c

@@ -20,43 +20,50 @@ Config_RobotParam_t robot_config = {
 .gimbal_param = {
 
 	/*欧拉角限位和电机角度限位*/
-	 .Set_Limit_t= {
+	 .Limit_t= {
 						.pit_max=0.69,
 						.pit_min=-0.47,
-						.yaw_max= 1.0,//yaw的限位作用于小yaw
+						.yaw_max= 1.0,//yaw的
 						.yaw_min=-1.0,
-						.pit_ecd_max=0,
-						.pit_ecd_min=0,
-						.yaw_ecd_max=0,
-						.yaw_ecd_min=0,
 	},
 	 
-//	.mech_zero = {
-//            .yaw = 0.0f,
-//            .pit = -0.69f,
-//        },
 
-	/*是否启动限位,1启动，-1不启动
+			.feedforward={
-		小yaw默认限位
+				.imu = {
-		一般pit统一限位*/
+					.yaw=false,
-				.travel = {
+					.pit=false,
-            .yaw = -1.0f,
+					.coefficient_yaw=0,
-            .pit = 1.0f,
+					.coefficient_pit=0,
-						.ecd_yaw=-1.0f,
+					},
-						.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},
 			
-	/*达妙电机参数自己配*/
+			/*零点参数*/
-	.dm_Params_t={
+				.zero={
-	.yaw_dm={.kd=0.3,},
+					.pit_encoder=0,
-	.pit_dm={.kd=0.3,},
+					.yaw_encoder=0,
-	},
+				},
+				
+				.motor={
+					/*按自己需求选择电机*/
+					.pit=DM,
+					.yaw=RM,
+					/*是否开启限位*/
+					.limit_yaw=false,
+					.limit_pit=true,
+					.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,},
+			.yaw_dm_Reduction_ratio=8.0f,//减速比
+			.pit_dm={.kd=1.0,},
+			.pit_dm_Reduction_ratio=5.0f,
+		},
 			
-//	.yaw_dm_motor={}
-//	.pit_dm_motor={}
     .low_pass_cutoff_freq = {
       .out = -1.0f,
       .gyro = 1000.0f,
@@ -103,47 +110,6 @@ Config_RobotParam_t robot_config = {
                 .out_limit = 10.0f,
                 .d_cutoff_freq = -1.0f,
                 .range = M_2PI,
-            },
-						/*电机控制参数*/
-             .yaw_velocity = {
-                .k = 0.0f,
-                .p = 0.0f,
-                .i = 0.0f,
-                .d = 0.000f,//0
-                .i_limit = 1.0f,
-                .out_limit = 1.0f,
-                .d_cutoff_freq = -1.0f,
-                .range = -1.0f,
-            },
-            .yaw_ecd_angle = {
-                .k = 0.0f,
-                .p =	0.0f ,
-                .i =	0.0f,
-                .d = 0.0f,
-                .i_limit = 0.0f,
-                .out_limit = 10.0f,
-                .d_cutoff_freq = -1.0f,
-                .range = M_2PI,
-            },
-            .pit_velocity = {
-                .k = 0.0f,
-                .p = 0.0f,
-                .i = 0.0f,
-                .d = 0.0f,
-                .i_limit = 1.0f,
-                .out_limit = 1.0f,
-                .d_cutoff_freq = -1.0f,
-                .range = -1.0f,
-            },
-            .pit_ecd_angle = {
-                .k = 0.0f,
-                .p = 0.0f,
-                .i = 0.0f,
-                .d = 0.0f,
-                .i_limit = 0.0f,
-                .out_limit = 10.0f,
-                .d_cutoff_freq = -1.0f,
-                .range = M_2PI,	
             },		
 					}       
 		},

User/module/gimbal.c

@@ -33,6 +33,26 @@
 /* Private macro ------------------------------------------------------------ */
 /* Private variables -------------------------------------------------------- */
 /* Private function  -------------------------------------------------------- */
+/**
+ * \brief 计算角度偏差
+ *
+ * \param 
+ * \param  
+ *
+ * \return 函数运行结果
+ */
+static float motor_imu_offset(float* motor, float* imu){
+
+	float motor_imu_offset = motor - imu;
+    /* 处理跨越±π的情况 */
+   if (motor_imu_offset > M_PI) motor_imu_offset -= M_2PI;
+   if (motor_imu_offset < -M_PI) motor_imu_offset += M_2PI;
+
+	return motor_imu_offset;
+}
+
+
+
 
 /**
  * \brief 设置云台模式
@@ -61,9 +81,7 @@ static int8_t Gimbal_SetMode(Gimbal_t *g, Gimbal_Mode_t mode) {
 	
   g->setpoint.eulr.pit = g->feedback.imu.eulr.pit;
   g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw;
-  g->setpoint.small_yaw = g->feedback.imu.eulr.yaw;
+
-	g->setpoint.ecd.pit=g->feedback.motor.pit.rotor_abs_angle;
-	g->setpoint.ecd.yaw=g->feedback.motor.yaw.rotor_abs_angle;
 
   g->mode = mode;
   return 0;
@@ -107,46 +125,33 @@ int8_t Gimbal_Init(Gimbal_t *g,Gimbal_Params_t *param,
   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);
 
-//  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->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;
-	g->limit.set_yaw.min=g->param->Set_Limit_t.yaw_min;	
-	g->limit.set_ecd_pit.max=g->param->Set_Limit_t.pit_ecd_max;
-	g->limit.set_ecd_pit.min=g->param->Set_Limit_t.pit_ecd_min;
-	g->limit.set_ecd_yaw.max=g->param->Set_Limit_t.yaw_ecd_max;
-	g->limit.set_ecd_yaw.min=g->param->Set_Limit_t.yaw_ecd_min;
 	
 
 	
   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 +172,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;
   }
@@ -231,7 +238,7 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
 	/* 电机角度控制相关逻辑 */
 	float yaw_velocity_set_point, pit_velocity_set_point;
 
-	float small_yaw_omega_setpoint;
+
 	
 	float delta_ecd_yaw = g_cmd->delta_yaw*g->dt;
 	float delta_ecd_pit = g_cmd->delta_pit*g->dt;
@@ -239,10 +246,16 @@ 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;
+		float yaw_motor_imu_offset=motor_imu_offset(&g->feedback.motor.yaw.rotor_abs_angle,
+			&g->feedback.imu.eulr.yaw);
+
+    float delta_max = CircleError(g->limit.yaw.max,
+        (g->setpoint.eulr.yaw + yaw_motor_imu_offset + delta_yaw), M_2PI);
+    float delta_min = CircleError(g->limit.yaw.min,
+        (g->setpoint.eulr.yaw + yaw_motor_imu_offset + delta_yaw), M_2PI);		
 			
   switch (g->mode) {
   case GIMBAL_MODE_RELAX:/*放松模式*/
@@ -252,21 +265,18 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
   case GIMBAL_MODE_ABSOLUTE:/*绝对模式*/
 	
 		g->setpoint.eulr.yaw+=delta_yaw;	
-		g->setpoint.small_yaw+=delta_yaw;		
+		if (g->param->motor.limit_yaw==true)
-		if (g->param->travel.yaw > 0)
+		Clip(&(g->setpoint.eulr.yaw),delta_min,delta_max);		
-		Clip(&(g->setpoint.eulr.yaw),g->limit.set_yaw.min,g->limit.set_yaw.max);		
 		else{
-		CircleAdd(&(g->setpoint.eulr.yaw), delta_yaw, M_2PI);
 		/*限制在-3.14~3.14*/
 		if (g->setpoint.eulr.yaw > M_PI) g->setpoint.eulr.yaw -= M_2PI;
 		}
 
   /* 处理pitch控制命令，软件限位 - 使用电机绝对角度 */
 	g->setpoint.eulr.pit+=delta_pit;
-		
+	  /* 限制pit控制命令 */	
-  if (g->param->travel.pit > 0)
+		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,
                                    g->feedback.imu.eulr.yaw, 0.0f, g->dt);
@@ -278,43 +288,66 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
     g->out.pit = PID_Calc(&(g->pid.pit_omega), pit_omega_set_point,
                           g->feedback.imu.gyro.x, 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);
+		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);
 		
     break;
 	case GIMBAL_MODE_RELATIVE:
-		/*跟据电机角度控制,遥控器不给值不会动*/
+		/*计算零点*/
-	g->setpoint.ecd.yaw+=delta_ecd_yaw;
+		g->zero.travel.yaw=motor_imu_offset(&g->feedback.motor.yaw.rotor_abs_angle,&g->feedback.imu.eulr.yaw);
+		g->zero.travel.pit=motor_imu_offset(&g->feedback.motor.pit.rotor_abs_angle,&g->feedback.imu.eulr.pit);	
+	
+		g->zero.yaw = g->param->zero.yaw_encoder - g->zero.travel.yaw;
+		g->zero.pit = g->param->zero.pit_encoder - g->zero.travel.pit;	
+		/*基于零点的设定角度*/
+		/*加的相对角度应该限制在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->travel.yaw > 0)
+		if (g->param->motor.limit_yaw==true)
-		Clip(&(g->setpoint.ecd.yaw),g->limit.set_ecd_yaw.min,g->limit.set_ecd_yaw.max);
+		Clip(&(g->setpoint.eulr.yaw),delta_min,delta_max);
-	else{
+		else{
-	/*限制yaw在0~6.28*/
+		/*限制在-3.14~3.14*/
-	CircleAdd(&(g->setpoint.ecd.yaw), delta_ecd_yaw, M_2PI);
+		if(g->setpoint.eulr.yaw > M_PI) g->setpoint.eulr.yaw -= 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);		
   
-  /* 处理pitch控制命令，软件限位 - 使用电机绝对角度 */
+    yaw_omega_set_point = PID_Calc(&(g->pid.yaw_angle), g->setpoint.eulr.yaw,
-	g->setpoint.ecd.pit+=delta_ecd_pit;
+                                   g->feedback.imu.eulr.yaw, 0.0f, g->dt);
-	/* 限制控制命令 */
+    g->out.yaw = PID_Calc(&(g->pid.yaw_omega), yaw_omega_set_point,
-  if (g->param->travel.pit > 0) 
+                          g->feedback.imu.gyro.z, 0.f, g->dt);
-	Clip(&(g->setpoint.ecd.pit),g->limit.set_ecd_pit.min,g->limit.set_ecd_pit.max);		
 
+		pit_omega_set_point = PID_Calc(&(g->pid.pit_angle), g->setpoint.eulr.pit,
+                                   g->feedback.imu.eulr.pit, 0.0f, g->dt);
+    g->out.pit = PID_Calc(&(g->pid.pit_omega), pit_omega_set_point,
+                          g->feedback.imu.gyro.x, 0.f, g->dt);
 		
-    yaw_velocity_set_point = PID_Calc(&(g->pid.yaw_ecd_angle), g->setpoint.ecd.yaw,
+		/*前馈添加*/
-                                   g->feedback.motor.yaw.rotor_abs_angle, 0.0f, g->dt);
+		if(g->param->feedforward.imu.yaw==true)
-    g->out.yaw = PID_Calc(&(g->pid.yaw_velocity), yaw_velocity_set_point,
+		g->out.yaw+=g->param->feedforward.imu.coefficient_yaw*(yaw_omega_set_point - g->feedback.imu.gyro.z);
-                          g->feedback.motor.yaw.rotor_speed, 0.f, g->dt);
+		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);
-		pit_velocity_set_point = PID_Calc(&(g->pid.pit_ecd_angle), g->setpoint.ecd.pit,
-                                   g->feedback.motor.pit.rotor_abs_angle, 0.0f, g->dt);
-    g->out.pit = PID_Calc(&(g->pid.pit_velocity), pit_velocity_set_point,
-                          g->feedback.motor.pit.rotor_speed, 0.f, g->dt);	
-	
 	
 		break;
-    /* 输出滤波 */
+
-    g->out.yaw = LowPassFilter2p_Apply(&g->filter_out.yaw, g->out.yaw);
-    g->out.pit = LowPassFilter2p_Apply(&g->filter_out.pit, g->out.pit);
 		
   }
+	    /* 输出滤波 */
+    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;
 }
 
@@ -326,25 +359,37 @@ 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));
+			g->out.pit_dm.angle=g->param->dm_Params_t.pit_dm.angle;
+			g->out.pit_dm.kd=g->param->dm_Params_t.pit_dm.kd;
+			g->out.pit_dm.kp=g->param->dm_Params_t.pit_dm.kp;
+			g->out.pit_dm.velocity=g->param->dm_Params_t.pit_dm.velocity;
+		
+			g->out.pit_dm.torque= g->out.pit * g->param->dm_Params_t.pit_dm_Reduction_ratio; // 乘以减速比
+		
+			 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; // 乘以减速比
+			g->out.yaw_dm.angle=g->param->dm_Params_t.yaw_dm.angle;
-			 MOTOR_DM_MITCtrl(&g->param->yaw_dm_motor,&(g->out.yaw_dm));	
+			g->out.yaw_dm.kd=g->param->dm_Params_t.yaw_dm.kd;
+			g->out.yaw_dm.kp=g->param->dm_Params_t.yaw_dm.kp;
+			g->out.yaw_dm.velocity=g->param->dm_Params_t.yaw_dm.velocity;
+
+			g->out.yaw_dm.torque= g->out.yaw * g->param->dm_Params_t.yaw_dm_Reduction_ratio; // 乘以减速比
+		
+			 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,34 @@ typedef struct {
   float min;
 } Gimbal_Limit_t;
 
-/* 云台参数的结构体，包含所有初始化用的参数，通常是const，存好几组。*/
+typedef	struct{
-typedef struct {
+	struct{
-	uint16_t yaw_num;
+    float yaw; /*  零点行程 */
+    float pit;
+  }travel;
+  float pit;		/*零点*/
+  float yaw;
 
-  MOTOR_RM_Param_t pit_rm_motor; /* pitch轴电机参数 */
+} Gimbal_zero_t;	
+
+typedef struct {
+	bool limit_yaw;/*是否开启限位*/
+	bool limit_pit;
+	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轴电机参数 */
 	
-	MOTOR_DM_Param_t pit_dm_motor;
+}Gimbal_MOTOR_Param_t;
-	MOTOR_DM_Param_t yaw_dm_motor;
+
+/* 云台参数的结构体，包含所有初始化用的参数，通常是const，存好几组。*/
+typedef struct {
+	
+	Gimbal_MOTOR_Param_t motor;
+
 	
   struct {
     KPID_Params_t yaw_omega; /* yaw轴角速度环PID参数 */
@@ -63,42 +87,43 @@ typedef struct {
     KPID_Params_t pit_velocity; /* pitch轴电机速度环PID参数 */
     KPID_Params_t pit_ecd_angle; /* pitch轴电机位置环PID参数 */
 			
-
-		
   } pid;
 	
+  struct {
+	float pit_encoder;		/*零点*/
+  float yaw_encoder;
+		
+  }zero;
+	  /* 前馈系数 */
+  struct {
+		struct{
+		bool yaw;
+		bool pit;
+		float coefficient_yaw;  
+    float coefficient_pit;			
+		}imu;
+		
+  }feedforward;
+	
   /* 低通滤波器截止频率 */
   struct {
     float out;  /* 电机输出 */
     float gyro; /* 陀螺仪数据 */
   } low_pass_cutoff_freq;
 
-  struct {
-    float yaw; /* yaw轴机械限位 */
-    float pit; /* pitch轴机械限位 */
-  } mech_zero;
-
-  struct {
-    float yaw; /* yaw轴机械限位行程 -1表示无限位 */
-    float pit; /* pitch轴机械限位行程 -1表示无限位*/
-		float ecd_yaw; /* yaw轴机械限位行程 -1表示无限位 */
-    float ecd_pit; /* pitch轴机械限位行程 -1表示无限位*/
-  } travel;
 
 	struct {
   float pit_max;		/*pit的限位*/
   float pit_min;
 	float yaw_max;		/*yaw的限位*/
   float yaw_min;
-  float pit_ecd_max;		/*pit的限位*/
+	}Limit_t;
-  float pit_ecd_min;
-	float yaw_ecd_max;		/*yaw的限位*/
-  float yaw_ecd_min;		
-	}Set_Limit_t;
 	
 	struct {
 	MOTOR_MIT_Output_t yaw_dm;		/*达妙的参数*/
 	MOTOR_MIT_Output_t pit_dm;	
+	float yaw_dm_Reduction_ratio;
+	float pit_dm_Reduction_ratio;	
 	}dm_Params_t;
 } Gimbal_Params_t;
 
@@ -119,7 +144,6 @@ typedef struct {
 typedef struct {
   float yaw; /* yaw轴电机输出 */
   float pit; /* pitch轴电机输出 */
-  float small_yaw; /* 小yaw轴电机输出 */	
 	MOTOR_MIT_Output_t yaw_dm;		/*达妙的参数*/
 	MOTOR_MIT_Output_t pit_dm;	
 
@@ -131,18 +155,18 @@ 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;
+	Gimbal_zero_t zero;
   /* PID计算的目标值 */
   struct {
     AHRS_Eulr_t eulr; /* 表示云台姿态的欧拉角 */
 		AHRS_Eulr_t ecd;
-		float small_yaw
   } setpoint;
 
   struct {
@@ -159,16 +183,14 @@ typedef struct {
   } pid;
 
   struct {
-    Gimbal_Limit_t yaw;
+		Gimbal_Limit_t yaw;
     Gimbal_Limit_t pit;
-		Gimbal_Limit_t set_yaw;
-    Gimbal_Limit_t set_pit;
-		Gimbal_Limit_t set_ecd_yaw;
-    Gimbal_Limit_t set_ecd_pit;
   } limit;
 
-
+  struct {
-	
+		float yaw;
+		float pit;
+  } relative_angle;
 	
   struct {
     LowPassFilter2p_t yaw;

User/task/gimbal_ctrl.c

@@ -39,11 +39,9 @@ void Task_gimbal_ctrl(void *argument) {
   while (1) {
     tick += delay_tick; /* 计算下一个唤醒时刻 */
     /* USER CODE BEGIN */
-		if(osMessageQueueGet(task_runtime.msgq.gimbal.imu, &gimbal_imu, NULL, 0)==osOK)
+		if(osMessageQueueGet(task_runtime.msgq.gimbal.imu, &gimbal_imu, NULL, 0)==osOK){
-      Gimbal_UpdateIMU(&gimbal, &gimbal_imu);
+      Gimbal_UpdateIMU(&gimbal, &gimbal_imu);}
-    
+		if(osMessageQueueGet(task_runtime.msgq.gimbal.cmd, &gimbal_cmd, NULL, 0)==osOK)
-		
-		osMessageQueueGet(task_runtime.msgq.gimbal.cmd, &gimbal_cmd, NULL, 0);
 		Gimbal_UpdateFeedback(&gimbal);
     Gimbal_Control(&gimbal,&gimbal_cmd);
     Gimbal_Output(&gimbal);