新增了电机角度控制

MDK-ARM/.vscode/keil-assistant.log

@@ -6,3 +6,5 @@
 
 [info] Log at : 2025/10/10|18:54:48|GMT+0800
 
+[info] Log at : 2025/10/12|20:17:36|GMT+0800
+

MDK-ARM/gimbal.uvoptx

@@ -157,7 +157,7 @@
         <Bp>
           <Number>0</Number>
           <Type>0</Type>
-          <LineNumber>38</LineNumber>
+          <LineNumber>39</LineNumber>
           <EnabledFlag>1</EnabledFlag>
           <Address>0</Address>
           <ByteObject>0</ByteObject>
@@ -175,6 +175,22 @@
           <Type>0</Type>
           <LineNumber>38</LineNumber>
           <EnabledFlag>1</EnabledFlag>
+          <Address>0</Address>
+          <ByteObject>0</ByteObject>
+          <HtxType>0</HtxType>
+          <ManyObjects>0</ManyObjects>
+          <SizeOfObject>0</SizeOfObject>
+          <BreakByAccess>0</BreakByAccess>
+          <BreakIfRCount>0</BreakIfRCount>
+          <Filename>..\User\module\gimbal.c</Filename>
+          <ExecCommand></ExecCommand>
+          <Expression></Expression>
+        </Bp>
+        <Bp>
+          <Number>2</Number>
+          <Type>0</Type>
+          <LineNumber>38</LineNumber>
+          <EnabledFlag>1</EnabledFlag>
           <Address>134229624</Address>
           <ByteObject>0</ByteObject>
           <HtxType>0</HtxType>

MDK-ARM/gimbal/gimbal.build_log.htm

@@ -17,7 +17,7 @@ Library Manager: ArmAr.exe V5.06 update 7 (build 960)
 Hex Converter:   FromElf.exe V5.06 update 7 (build 960)
 CPU DLL:         SARMCM3.DLL V5.34.0.0
 Dialog DLL:      DCM.DLL V1.17.3.0
-Target DLL:      STLink\ST-LINKIII-KEIL_SWO.dll V3.0.8.0
+Target DLL:      CMSIS_AGDI.dll V1.32.13.0
 Dialog DLL:      TCM.DLL V1.48.0.0
  
 <h2>Project:</h2>
@@ -31,10 +31,6 @@ 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.c...
-linking...
-Program Size: Code=59892 RO-data=980 RW-data=404 ZI-data=33788  
-FromELF: creating hex file...
 "gimbal\gimbal.axf" - 0 Error(s), 0 Warning(s).
 
 <h2>Software Packages used:</h2>
@@ -58,7 +54,7 @@ Package Vendor: Keil
 
    * Component: ARM::CMSIS:CORE:5.4.0
       Include file:  CMSIS\Core\Include\tz_context.h
-Build Time Elapsed:  00:00:03
+Build Time Elapsed:  00:00:01
 </pre>
 </body>
 </html>

MDK-ARM/gimbal/gimbal_gimbal.dep

@@ -2126,7 +2126,7 @@ I (D:\KEIL_MDK\ARM\ARMCC\include\math.h)(0x5E8E3CC2)
 I (D:\KEIL_MDK\ARM\ARMCC\include\stdbool.h)(0x5E8E3CC2)
 I (D:\KEIL_MDK\ARM\ARMCC\include\stdint.h)(0x5E8E3CC2)
 I (D:\KEIL_MDK\ARM\ARMCC\include\stddef.h)(0x5E8E3CC2)
-I (../User/module/gimbal.h)(0x68EB79B9)
+I (../User/module/gimbal.h)(0x68EF9D8A)
 I (../User/component/ahrs.h)(0x68E90EBF)
 I (../User/component/filter.h)(0x68E90EBF)
 I (../User/component/pid.h)(0x68E90EBF)
@@ -2203,7 +2203,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)(0x68EB79B9)
+I (../User/module/gimbal.h)(0x68EF9D8A)
 I (../User/component/ahrs.h)(0x68E90EBF)
 I (../User/component/user_math.h)(0x68E90EBF)
 I (D:\KEIL_MDK\ARM\ARMCC\include\float.h)(0x5E8E3CC2)
@@ -2314,7 +2314,7 @@ I (../User/component/pid.h)(0x68E90EBF)
 I (../User/component/filter.h)(0x68E90EBF)
 I (../User/device/bmi088.h)(0x68E90E66)
 I (../User/device/device.h)(0x68E90EBF)
-I (../User/module/gimbal.h)(0x68EB79B9)
+I (../User/module/gimbal.h)(0x68EF9D8A)
 I (../User/device/motor.h)(0x68E90E66)
 I (../User/device/motor_rm.h)(0x68E90E66)
 I (../User/bsp/can.h)(0x68E90EBF)
@@ -2344,7 +2344,7 @@ I (../User/component\user_math.h)(0x68E90EBF)
 I (D:\KEIL_MDK\ARM\ARMCC\include\float.h)(0x5E8E3CC2)
 I (D:\KEIL_MDK\ARM\ARMCC\include\math.h)(0x5E8E3CC2)
 I (D:\KEIL_MDK\ARM\ARMCC\include\stdbool.h)(0x5E8E3CC2)
-I (../User/module/gimbal.h)(0x68EB79B9)
+I (../User/module/gimbal.h)(0x68EF9D8A)
 I (../User/component/ahrs.h)(0x68E90EBF)
 I (../User/component/filter.h)(0x68E90EBF)
 I (../User/component/pid.h)(0x68E90EBF)
@@ -2389,7 +2389,7 @@ I (../Drivers/STM32F4xx_HAL_Driver/Inc/stm32f4xx_hal_uart.h)(0x68B05645)
 I (../User/bsp/bsp.h)(0x68E90EBF)
 I (../User/bsp/mm.h)(0x68E90EBF)
 I (../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os.h)(0x68B055DB)
-F (..\User\task\gimbal_ctrl.c)(0x68E6578C)(--c99 -c --cpu Cortex-M4.fp.sp -g -W -O0 --apcs=interwork --split_sections -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_MDK\ARM\CMSIS\5.7.0\CMSIS\Core\Include

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

-o gimbal\gimbal_ctrl.o --omf_browse gimbal\gimbal_ctrl.crf --depend gimbal\gimbal_ctrl.d)
+F (..\User\task\gimbal_ctrl.c)(0x68EF9F4E)(--c99 -c --cpu Cortex-M4.fp.sp -g -W -O0 --apcs=interwork --split_sections -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_MDK\ARM\CMSIS\5.7.0\CMSIS\Core\Include

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

-o gimbal\gimbal_ctrl.o --omf_browse gimbal\gimbal_ctrl.crf --depend gimbal\gimbal_ctrl.d)
 I (../User/task/user_task.h)(0x68DCD971)
 I (../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2/cmsis_os2.h)(0x68B055DB)
 I (D:\KEIL_MDK\ARM\ARMCC\include\stdint.h)(0x5E8E3CC2)
@@ -2403,7 +2403,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)(0x68EB79B9)
+I (../User/module/gimbal.h)(0x68EF9D8A)
 I (../User/component/ahrs.h)(0x68E90EBF)
 I (../User/component/user_math.h)(0x68E90EBF)
 I (D:\KEIL_MDK\ARM\ARMCC\include\float.h)(0x5E8E3CC2)
@@ -2469,8 +2469,8 @@ 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\module\gimbal.c)(0x68EB7A32)(--c99 -c --cpu Cortex-M4.fp.sp -g -W -O0 --apcs=interwork --split_sections -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_MDK\ARM\CMSIS\5.7.0\CMSIS\Core\Include

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

-o gimbal\gimbal.o --omf_browse gimbal\gimbal.crf --depend gimbal\gimbal.d)
-I (..\User\module\gimbal.h)(0x68EB79B9)
+F (..\User\module\gimbal.c)(0x68EFA454)(--c99 -c --cpu Cortex-M4.fp.sp -g -W -O0 --apcs=interwork --split_sections -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_MDK\ARM\CMSIS\5.7.0\CMSIS\Core\Include

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

-o gimbal\gimbal.o --omf_browse gimbal\gimbal.crf --depend gimbal\gimbal.d)
+I (..\User\module\gimbal.h)(0x68EF9D8A)
 I (../User/component/ahrs.h)(0x68E90EBF)
 I (../User/component/user_math.h)(0x68E90EBF)
 I (D:\KEIL_MDK\ARM\ARMCC\include\float.h)(0x5E8E3CC2)
@@ -2532,7 +2532,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/bsp/time.h)(0x68E90EBF)
-F (..\User\module\config.c)(0x68EB75BC)(--c99 -c --cpu Cortex-M4.fp.sp -g -W -O0 --apcs=interwork --split_sections -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_MDK\ARM\CMSIS\5.7.0\CMSIS\Core\Include

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

-o gimbal\config.o --omf_browse gimbal\config.crf --depend gimbal\config.d)
+F (..\User\module\config.c)(0x68EF9D67)(--c99 -c --cpu Cortex-M4.fp.sp -g -W -O0 --apcs=interwork --split_sections -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_MDK\ARM\CMSIS\5.7.0\CMSIS\Core\Include

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

-o gimbal\config.o --omf_browse gimbal\config.crf --depend gimbal\config.d)
 I (../User/component/user_math.h)(0x68E90EBF)
 I (D:\KEIL_MDK\ARM\ARMCC\include\float.h)(0x5E8E3CC2)
 I (D:\KEIL_MDK\ARM\ARMCC\include\math.h)(0x5E8E3CC2)
@@ -2592,7 +2592,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)(0x68E90E66)
-I (../User/module/gimbal.h)(0x68EB79B9)
+I (../User/module/gimbal.h)(0x68EF9D8A)
 I (../User/component/ahrs.h)(0x68E90EBF)
 I (../User/component/filter.h)(0x68E90EBF)
 I (../User/component/pid.h)(0x68E90EBF)

User/module/config.c

@@ -18,17 +18,29 @@
 Config_RobotParam_t robot_config = {
 
 .gimbal_param = {
+	/*欧拉角限位和电机角度限位*/
 	 .Set_Limit_t= {
 						.pit_max=0.69,
 						.pit_min=-0.47,
+						.yaw_max=0,
+						.yaw_min=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,
-        },
-   .travel = {
-            .yaw = 1.0f,
-            .pit = 1.16f,
+	 
+//	.mech_zero = {
+//            .yaw = 0.0f,
+//            .pit = -0.69f,
+//        },
+	/*是否启动限位,1启动，-1不启动
+		一般pit统一限位*/
+				.travel = {
+            .yaw = -1.0f,
+            .pit = 1.0f,
+						.ecd_yaw=-1.0f,
+						.ecd_pit=1.0f,
         },
 	
   .pit_motor={BSP_CAN_2,0x20A,MOTOR_GM6020,false,false},
@@ -39,6 +51,7 @@ Config_RobotParam_t robot_config = {
     },
 
         .pid = {
+					/*欧拉角控制参数*/
              .yaw_omega = {
                 .k = 0.45f,
                 .p = 1.0f,
@@ -78,6 +91,47 @@ 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

@@ -1,5 +1,23 @@
 /*
  * 云台模组
+ 模式自行选择：遥控器改变或自行初始化
+ delta获取从遥控器自行获取
+ example:
+	Gimbal_t gimbal;
+	Gimbal_IMU_t gimbal_imu;
+	Gimbal_CMD_t gimbal_cmd;
+
+	Gimbal_Init(&gimbal,&Config_GetRobotParam()->gimbal_param,GIMBAL_CTRL_FREQ);
+ 
+	while(1){
+	//这里写遥控器获取gimbal_cmd数据
+		if(osMessageQueueGet(task_runtime.msgq.gimbal.imu, &gimbal_imu, NULL, 0))
+    Gimbal_UpdateIMU(&gimbal, &gimbal_imu);
+		
+ 		Gimbal_UpdateFeedback(&gimbal);
+    Gimbal_Control(&gimbal,&gimbal_cmd);
+    Gimbal_Output(&gimbal);
+		}
  */
 
 /* Includes ----------------------------------------------------------------- */
@@ -43,7 +61,8 @@ 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.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;
 }
@@ -77,16 +96,31 @@ int8_t Gimbal_Init(Gimbal_t *g,Gimbal_Params_t *param,
   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->param->pid.yaw_ecd_angle));
+  PID_Init(&(g->pid.yaw_velocity), KPID_MODE_CALC_D, target_freq,
+           &(g->param->pid.yaw_velocity));
+  PID_Init(&(g->pid.pit_ecd_angle), KPID_MODE_CALC_D, target_freq,
+           &(g->param->pid.pit_ecd_angle));
+  PID_Init(&(g->pid.pit_velocity), KPID_MODE_CALC_D, target_freq,
+           &(g->param->pid.pit_velocity));
   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.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();
   MOTOR_RM_Register(&(g->param->yaw_motor));
   MOTOR_RM_Register(&(g->param->pit_motor));
@@ -143,7 +177,6 @@ int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu){
  *
  * \return 函数运行结果
  */
-float pit_set1,pit_set2,yaw_set1,yaw_set2,y;
 int8_t Gimbal_Control(Gimbal_t *g, Gimbal_CMD_t *g_cmd) {
   if (g == NULL || g_cmd == NULL) {
     return -1;
@@ -151,73 +184,91 @@ 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控制命令，软件限位 - 使用电机绝对角度 */
-  float delta_yaw = g_cmd->delta_yaw*g->dt;
-//  if (g->param->travel.yaw > 0) {
-//    /* 计算当前电机角度与IMU角度的偏差 */
-//    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;
-//    /* 计算到限位边界的距离 */
-//    const float delta_max = CircleError(g->limit.yaw.max,
-//        (g->setpoint.eulr.yaw + motor_imu_offset + delta_yaw),M_2PI);
-//    const float delta_min = CircleError(g->limit.yaw.min,
-//        (g->setpoint.eulr.yaw + motor_imu_offset + delta_yaw), M_2PI);
-//    
-//    /* 限制控制命令 */
-//    if (delta_yaw > delta_max) delta_yaw = delta_max;
-//    if (delta_yaw < delta_min) delta_yaw = delta_min;
-//  }
+  /* 欧拉角控制相关逻辑 */
+  float yaw_omega_set_point, pit_omega_set_point,delta_yaw;
+	/* 电机角度控制相关逻辑 */
+	float yaw_velocity_set_point, pit_velocity_set_point,delta_ecd_yaw;
 	
-		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控制命令，软件限位 - 使用电机绝对角度 */
-  float delta_pit = g_cmd->delta_pit*g->dt;
-  if (g->param->travel.pit > 0) {
-    /* 计算当前电机角度与IMU角度的偏差 */
-    float motor_imu_offset = g->feedback.motor.pit.rotor_abs_angle - g->feedback.imu.eulr.pit;
-    /* 处理跨越±π的情况 */
-    if (motor_imu_offset > M_PI) motor_imu_offset -= M_2PI;
-    if (motor_imu_offset < -M_PI) motor_imu_offset += M_2PI;
-    
-    /* 计算到限位边界的距离 */
-		g->limit.pit.max=g->limit.set_pit.max;
-		g->limit.pit.min=g->limit.set_pit.min;
-    const float delta_max = CircleError(g->limit.pit.max,
-        (g->setpoint.eulr.pit + motor_imu_offset + delta_pit), M_2PI);
-    const float delta_min = CircleError(g->limit.pit.min,
-        (g->setpoint.eulr.pit + motor_imu_offset + delta_pit), M_2PI);
-  }
-	 /* 限制控制命令 */
-	g->setpoint.eulr.pit+=delta_pit;
-	if(g->setpoint.eulr.pit>g->limit.set_pit.max)g->setpoint.eulr.pit=g->limit.set_pit.max;
-	if(g->setpoint.eulr.pit<g->limit.set_pit.min)g->setpoint.eulr.pit=g->limit.set_pit.min;		
-	
-  /* 控制相关逻辑 */
-  float yaw_omega_set_point, pit_omega_set_point;
   switch (g->mode) {
   case GIMBAL_MODE_RELAX:
     g->out.yaw = 0.0f;
     g->out.pit = 0.0f;
     break;
   case GIMBAL_MODE_ABSOLUTE:
-    yaw_set1=yaw_omega_set_point = PID_Calc(&(g->pid.yaw_angle), g->setpoint.eulr.yaw,
+		delta_yaw = g_cmd->delta_yaw*g->dt;
+		g->setpoint.eulr.yaw+=delta_yaw;	
+	
+		if (g->param->travel.yaw > 0){
+	if(g->setpoint.eulr.yaw>g->limit.set_yaw.max)g->setpoint.eulr.yaw=g->limit.set_yaw.max;
+	if(g->setpoint.eulr.yaw<g->limit.set_yaw.min)g->setpoint.eulr.yaw=g->limit.set_yaw.min;			
+}
+		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控制命令，软件限位 - 使用电机绝对角度 */
+  float delta_pit = g_cmd->delta_pit*g->dt;
+	g->setpoint.eulr.pit+=delta_pit;
+		
+  if (g->param->travel.pit > 0){
+  /* 限制控制命令 */
+	if(g->setpoint.eulr.pit>g->limit.set_pit.max)g->setpoint.eulr.pit=g->limit.set_pit.max;
+	if(g->setpoint.eulr.pit<g->limit.set_pit.min)g->setpoint.eulr.pit=g->limit.set_pit.min;		
+	}
+    yaw_omega_set_point = PID_Calc(&(g->pid.yaw_angle), g->setpoint.eulr.yaw,
                                    g->feedback.imu.eulr.yaw, 0.0f, g->dt);
-    yaw_set2=g->out.yaw = PID_Calc(&(g->pid.yaw_omega), yaw_omega_set_point,
+    g->out.yaw = PID_Calc(&(g->pid.yaw_omega), yaw_omega_set_point,
                           g->feedback.imu.gyro.z, 0.f, g->dt);
 
-		pit_set1= pit_omega_set_point = PID_Calc(&(g->pid.pit_angle), g->setpoint.eulr.pit,
+		pit_omega_set_point = PID_Calc(&(g->pid.pit_angle), g->setpoint.eulr.pit,
                                    g->feedback.imu.eulr.pit, 0.0f, g->dt);
-    pit_set2=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);
     break;
+		case GIMBAL_MODE_RELATIVE:
 		
+		
+		
+		break;
+	case GIMBAL_MODE_MOTOR:
+		
+  delta_ecd_yaw = g_cmd->delta_yaw*g->dt;
+	g->setpoint.ecd.yaw+=delta_ecd_yaw;
+	
+	if(g->param->travel.yaw > 0){
+
+	if(g->setpoint.ecd.yaw>g->limit.set_ecd_yaw.max)g->setpoint.ecd.pit=g->limit.set_ecd_yaw.max;
+	if(g->setpoint.ecd.yaw<g->limit.set_ecd_yaw.min)g->setpoint.ecd.pit=g->limit.set_ecd_yaw.min;	
+	}
+	else{
+	/*限制yaw在0~6.28*/
+	CircleAdd(&(g->setpoint.ecd.yaw), delta_ecd_yaw, M_2PI);
+	}
+	
+  /* 处理pitch控制命令，软件限位 - 使用电机绝对角度 */
+  float delta_ecd_pit = g_cmd->delta_pit*g->dt;
+	g->setpoint.ecd.pit+=delta_pit;
+	
+  if (g->param->travel.pit > 0) {
+		/* 限制控制命令 */
+	if(g->setpoint.ecd.pit>g->limit.set_ecd_pit.max)g->setpoint.ecd.pit=g->limit.set_ecd_pit.max;
+	if(g->setpoint.ecd.pit<g->limit.set_ecd_pit.min)g->setpoint.ecd.pit=g->limit.set_ecd_pit.min;	
+  }
+	 
+    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);
+
+		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);
@@ -237,5 +288,5 @@ void Gimbal_Output(Gimbal_t *g){
     MOTOR_RM_Ctrl(&g->param->pit_motor);
 		MOTOR_RM_Ctrl(&g->param->yaw_motor);
 		MOTOR_RM_SetOutput(&g->param->yaw_motor, g->out.yaw);
-	  MOTOR_RM_SetOutput(&g->param->pit_motor, -(g->out.pit));//极性反了
+	  MOTOR_RM_SetOutput(&g->param->pit_motor, g->out.pit);//
   }

User/module/gimbal.h

@@ -28,6 +28,7 @@ typedef enum {
   GIMBAL_MODE_RELAX,    /* 放松模式，电机不输出。一般情况云台初始化之后的模式 */
   GIMBAL_MODE_ABSOLUTE, /* 绝对坐标系控制，控制在空间内的绝对姿态 */
   GIMBAL_MODE_RELATIVE, /* 相对坐标系控制，控制相对于底盘的姿态 */
+	GIMBAL_MODE_MOTOR, /* 相对坐标系控制，控制相对于电机角度 */
 } Gimbal_Mode_t;
 
 typedef struct {
@@ -51,6 +52,11 @@ typedef struct {
     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;
 
   /* 低通滤波器截止频率 */
@@ -67,11 +73,19 @@ typedef struct {
   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;
 	
 } Gimbal_Params_t;
@@ -111,6 +125,7 @@ typedef struct {
   /* PID计算的目标值 */
   struct {
     AHRS_Eulr_t eulr; /* 表示云台姿态的欧拉角 */
+		AHRS_Eulr_t ecd;
   } setpoint;
 
   struct {
@@ -118,6 +133,11 @@ typedef struct {
     KPID_t yaw_omega; /* yaw轴角速度环PID */
     KPID_t pit_angle; /* 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 {
@@ -125,6 +145,8 @@ typedef struct {
     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;
 
 

User/task/gimbal_ctrl.c

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