111

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,15 +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 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=84384 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>

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)(0x6900D064)
+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)(0x6900D064)
+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)(0x6900D064)
+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)(0x6900D064)
+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)(0x6900D064)
+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)(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)
+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)(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)(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)
+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)(0x6900D064)
+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,44 +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不启动
-		小yaw默认限位
-		一般pit统一限位*/
-				.travel = {
-            .yaw = -1.0f,
-            .pit = 1.0f,
-						.ecd_yaw=-1.0f,
-						.ecd_pit=1.0f,
-        },
-	
+
+			.feedforward={
+				.imu = {
+					.yaw=false,
+					.pit=false,
+					.coefficient_yaw=0,
+					.coefficient_pit=0,
+					},
+
+			},
+			
+			/*零点参数*/
+				.zero={
+					.pit_encoder=0,
+					.yaw_encoder=0,
+				},
 				
 				.motor={
 					/*按自己需求选择电机*/
-					.pit=RM,
+					.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,
+		},
+			
     .low_pass_cutoff_freq = {
       .out = -1.0f,
       .gyro = 1000.0f,
@@ -104,48 +110,7 @@ Config_RobotParam_t robot_config = {
                 .out_limit = 10.0f,
                 .d_cutoff_freq = -1.0f,
                 .range = M_2PI,
-            },
-						/*电机控制参数*/
-             .yaw_velocity = {
-                .k = 1.0f,
-                .p = 1.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 = 1.0f,
-                .p = 1.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 = 1.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,28 +125,16 @@ 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->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();
 	/*大疆电机注册*/
@@ -231,8 +237,8 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
   float yaw_omega_set_point, pit_omega_set_point;
 	/* 电机角度控制相关逻辑 */
 	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;
@@ -242,15 +248,14 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
 	
 
 	
-	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);		
+
+		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:/*放松模式*/
@@ -260,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->travel.yaw > 0)
+		if (g->param->motor.limit_yaw==true)
 		Clip(&(g->setpoint.eulr.yaw),delta_min,delta_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;
-		
-  if (g->param->travel.pit > 0)
-  /* 限制pit控制命令 */
-		Clip(&(g->setpoint.eulr.pit),g->limit.set_pit.min,g->limit.set_pit.max);		
+	  /* 限制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);	
 
     yaw_omega_set_point = PID_Calc(&(g->pid.yaw_angle), g->setpoint.eulr.yaw,
                                    g->feedback.imu.eulr.yaw, 0.0f, g->dt);
@@ -285,44 +287,67 @@ int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
                                    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);
-
-
+	
+		/*前馈添加*/
+		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;
-		/*yaw的限位*/	
-	if(g->param->travel.yaw > 0)
-		Clip(&(g->setpoint.ecd.yaw),g->limit.set_ecd_yaw.min,g->limit.set_ecd_yaw.max);
-	else{
-	/*限制yaw在0~6.28*/
-	CircleAdd(&(g->setpoint.ecd.yaw), delta_ecd_yaw, M_2PI);
-	}
+		/*计算零点*/
+		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);	
 	
-  /* 处理pitch控制命令，软件限位 - 使用电机绝对角度 */
-	g->setpoint.ecd.pit+=delta_ecd_pit;
-	/* 限制控制命令 */
-  if (g->param->travel.pit > 0) 
-	Clip(&(g->setpoint.ecd.pit),g->limit.set_ecd_pit.min,g->limit.set_ecd_pit.max);		
-  
-	 
-    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);
-    g->out.yaw = PID_Calc(&(g->pid.yaw_velocity), yaw_velocity_set_point,
-                          g->feedback.motor.yaw.rotor_speed, 0.f, g->dt);
+		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;
 
-		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);	
-	
+		/*yaw的限位*/	
+		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;
+		}
+		/* 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);		
+  
+    yaw_omega_set_point = PID_Calc(&(g->pid.yaw_angle), g->setpoint.eulr.yaw,
+                                   g->feedback.imu.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);
+
+		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);
+		
+		/*前馈添加*/
+		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;
-    /* 输出滤波 */
-    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;
 }
 
@@ -345,12 +370,26 @@ void Gimbal_Output(Gimbal_t *g){
 	
 	/*达妙电机输出*/
 	if(g->param->motor.pit==DM){
-			g->out.pit_dm.torque= g->out.pit * 5.0f; // 乘以减速比
+		
+			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->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;
+			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

@@ -47,9 +47,19 @@ typedef struct {
   float min;
 } Gimbal_Limit_t;
 
+typedef	struct{
+	struct{
+    float yaw; /*  零点行程 */
+    float pit;
+  }travel;
+  float pit;		/*零点*/
+  float yaw;
+
+} 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轴电机参数 */
@@ -57,7 +67,8 @@ typedef struct {
 	
 	MOTOR_RM_Param_t pit_rm_motor; /* pitch轴电机参数 */
   MOTOR_RM_Param_t yaw_rm_motor; /* yaw轴电机参数 */
-} Gimbal_MOTOR_Param_t;
+	
+}Gimbal_MOTOR_Param_t;
 
 /* 云台参数的结构体，包含所有初始化用的参数，通常是const，存好几组。*/
 typedef struct {
@@ -77,39 +88,42 @@ typedef struct {
     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的限位*/
-  float pit_ecd_min;
-	float yaw_ecd_max;		/*yaw的限位*/
-  float yaw_ecd_min;		
-	}Set_Limit_t;
+	}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;
 
@@ -130,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;	
 
@@ -149,11 +162,11 @@ typedef struct {
   /* 模块通用 */
   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 {
@@ -170,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)
-      Gimbal_UpdateIMU(&gimbal, &gimbal_imu);
-    
-		
-		osMessageQueueGet(task_runtime.msgq.gimbal.cmd, &gimbal_cmd, NULL, 0);
+		if(osMessageQueueGet(task_runtime.msgq.gimbal.imu, &gimbal_imu, NULL, 0)==osOK){
+      Gimbal_UpdateIMU(&gimbal, &gimbal_imu);}
+		if(osMessageQueueGet(task_runtime.msgq.gimbal.cmd, &gimbal_cmd, NULL, 0)==osOK)
 		Gimbal_UpdateFeedback(&gimbal);
     Gimbal_Control(&gimbal,&gimbal_cmd);
     Gimbal_Output(&gimbal);