修复电机通信bug

# Conflicts:
#	APP/Device/Device_speed/speed_to_servoMotor.c
#	BSP/Driver/servoMotor/motorCommu.c
第一次合并

APP/Appcfg/app.c

@@ -287,10 +287,10 @@ static void task_print()
 
 static void task_print_task()
 {
-  while(1)
+    while(1)
-  {
+    {
     task_print();
-  }
+    }
 }
 
 /*!

APP/Appcfg/app_cfg.h

@@ -55,88 +55,208 @@
 
 /// CE
 #define TASK_TESTQUEUE_PRIO                         55u
+///*****************************************************************/
+//
+//#define TASK_GET_ANGLE_PRIO                            21u
+//#define TASK_GET_ANGLE_STK_SIZE                        120u
+//
+//#define POWER_OFF_PRIO                                 17u//原来为7，后导致ping包有延时，造成网络阻塞，UCOSII系统卡死，故优先级低于TCPIP
+//#define POWER_OFF_STK_SIZE                             200u
+//
+//#define TASK_PS_PRIO                                   22u
+//#define TASK_PS_STK_SIZE                               80u
+//
+//#define TASK_HORI_ROATE_MONITOR_PRIO                   23u
+//#define TASK_HORI_ROATE_MONITOR_STK_SIZE               120u
+//
+//#define TASK_VERT_ROATE_MONITOR_PRIO                   24u
+//#define TASK_VERT_ROATE_MONITOR_STK_SIZE               120u
+//
+//#define TASK_ELECTRIC_STABLE_PRIO                      25u
+//#define TASK_ELECTRIC_STABLE_STK_SIZE                  150u
+//
+//#define TASK_VERT_ELECTRIC_STABLE_PWM_PRIO             26u
+//#define TASK_VERT_ELECTRIC_STABLE_PWM_SIZE             150u
+//
+//#define TASK_PTZ_UPDATE_RECE_DATA_PRIO                 27u//接收升级数据
+//#define TASK_PTZ_UPDATE_RECE_DATA_STK_SIZE             170u
+//
+//#define TASK_VERT_SELF_CHECK_PRIO                      28u
+//#define TASK_VERT_SELF_CHECK_STK_SIZE                  180u
+//
+//#define TASK_HORI_SELF_CHECK_PRIO                      29u
+//#define TASK_HORI_SELF_CHECK_STK_SIZE                  180u
+//
+//#define TASK_HORI_PID_PRIO                             30u
+//#define TASK_HORI_PID_STK_SIZE                         150u
+//
+//#define TASK_VERT_DIRECTOR_SPEED_PWM_PRIO              31u
+//#define TASK_VERT_DIRECTOR_SPEED_PWM_STK_SIZE          150u
+//
+//#define TASK_VERT_PID_PRIO                             32u
+//#define TASK_VERT_PID_STK_SIZE                         150u
+//
+//#define TASK_FAULT_DETECT_PRIO                         34u
+//#define TASK_FAULT_DETECT_STK_SIZE                     180u
+///**/
+//#define TASK_AREA_SCAN_PRIO                            35u
+//#define TASK_AREA_SCAN_STK_SIZE                        120u
+//
+//#define TASK_PRESET_BIT_SCAN_PRIO                      36u
+//#define TASK_PRESET_BIT_SCAN_STK_SIZE                  100u
+//
+//#define TASK_LISTEN_PTZ_SERVER_PRIO                    37u//云台接收指令分析
+//#define TASK_LISTEN_PTZ_SERVER_STK_SIZE                600
+//
+//#define TASK_PTZ_UPDATE_DATA_PROCESS_PRIO              39u//处理升级数据
+//#define TASK_PTZ_UPDATE_DATA_PROCESS_STK_SIZE          200u
+//
+//#define TASK_PTZ_UART_485_LASER_PROCESS_PRIO           40u//处理串口数据
+//#define TASK_PTZ_UART_485_LASER_PROCESS_STK_SIZE       150u
+//
+//#define TASK_PTZ_UART_485_DATA_PROCESS_PRIO            41u//处理串口数据
+//#define TASK_PTZ_UART_485_DATA_PROCESS_STK_SIZE        300u
+//
+//#define TASK_PTZ_UART_422_DATA_PROCESS_PRIO            42u//处理串口数据
+//#define TASK_PTZ_UART_422_DATA_PROCESS_STK_SIZE        300u
+//
+//#define TASK_PTZ_AUTO_RETURN_PRIO                      43u//角度回传任务
+//#define TASK_PTZ_AUTO_RETURN_STK_SIZE                  180u
+//
+//#define TASK_PTZ_SPEED_RETURN_PRIO                     44u//角度回传任务
+//#define TASK_PTZ_SPEED_RETURN_STK_SIZE                 180u
+//
+//#define TASK_PTZ_DATA_COLLECT_PRIO                     45u
+//#define TASK_PTZ_DATA_COLLECT_STK_SIZE                 200u
+//
+//#define TASK_PTZ_HEAT_RESISTOR_PRIO                    46u
+//#define TASK_PTZ_HEAT_RESISTOR_STK_SIZE                60u
+//
+//#define TASK_PTZ_RESTORE_PRIO                          47u
+//#define TASK_PTZ_RESTORE_STK_SIZE                      60u
+//
+//
+//#define TASK_PTZ_ERROR_COUNT_PRIO                          48u
+//#define TASK_PTZ_ERROR_COUNT_STK_SIZE                      200u
+//
+//
+////打印任务堆栈等信息
+//#define TASK_PTZ_TASK_PRINTF_PRIO                          55u
+//#define TASK_PTZ_TASK_PRINTF_STK_SIZE                      200u
+//
+//
+////解析水平电机返回的数据
+//#define TASK_RECV_HORI_SERVO_PRIO                          56u
+//#define TASK_RECV_HORI_SERVO_STK_SIZE                      200u
+//
+////解析俯仰电机返回的数据
+//#define TASK_RECV_VERT_SERVO_PRIO                          57u
+//#define TASK_RECV_VERT_SERVO_STK_SIZE                      200u
+//
+//
+//
+////#define ????????????????????_PRIO                    55u
+////#define ????????????????????_STK_SIZE                150u
+///*******************************************************************************/
+
 /*****************************************************************/
 
-#define TASK_GET_ANGLE_PRIO                            21u
+#define TASK_GET_ANGLE_PRIO                            22u
 #define TASK_GET_ANGLE_STK_SIZE                        120u
 
 #define POWER_OFF_PRIO                                 17u//原来为7，后导致ping包有延时，造成网络阻塞，UCOSII系统卡死，故优先级低于TCPIP
 #define POWER_OFF_STK_SIZE                             200u
 
-#define TASK_PS_PRIO                                   22u
-#define TASK_PS_STK_SIZE                               80u
 
-#define TASK_HORI_ROATE_MONITOR_PRIO                   23u
-#define TASK_HORI_ROATE_MONITOR_STK_SIZE               120u
 
-#define TASK_VERT_ROATE_MONITOR_PRIO                   24u
+/*********电机通讯协议任务优先级在此**********/
-#define TASK_VERT_ROATE_MONITOR_STK_SIZE               120u
+//解析水平电机返回的数据
+#define TASK_RECV_HORI_SERVO_PRIO                      18u
+#define TASK_RECV_HORI_SERVO_STK_SIZE                  200u
 
-#define TASK_ELECTRIC_STABLE_PRIO                      25u
+//解析俯仰电机返回的数据
-#define TASK_ELECTRIC_STABLE_STK_SIZE                  150u
+#define TASK_RECV_VERT_SERVO_PRIO                      19u
+#define TASK_RECV_VERT_SERVO_STK_SIZE                  200u
 
-#define TASK_VERT_ELECTRIC_STABLE_PWM_PRIO             26u
+#define TASK_HORI_PID_PRIO                             20u
-#define TASK_VERT_ELECTRIC_STABLE_PWM_SIZE             150u
-
-#define TASK_PTZ_UPDATE_RECE_DATA_PRIO                 27u//接收升级数据
-#define TASK_PTZ_UPDATE_RECE_DATA_STK_SIZE             170u
-
-#define TASK_VERT_SELF_CHECK_PRIO                      28u
-#define TASK_VERT_SELF_CHECK_STK_SIZE                  180u
-
-#define TASK_HORI_SELF_CHECK_PRIO                      29u
-#define TASK_HORI_SELF_CHECK_STK_SIZE                  180u
-
-#define TASK_HORI_PID_PRIO                             30u
 #define TASK_HORI_PID_STK_SIZE                         150u
 
-#define TASK_VERT_DIRECTOR_SPEED_PWM_PRIO              31u
+#define TASK_VERT_PID_PRIO                             21u
-#define TASK_VERT_DIRECTOR_SPEED_PWM_STK_SIZE          150u
-
-#define TASK_VERT_PID_PRIO                             32u
 #define TASK_VERT_PID_STK_SIZE                         150u
 
-#define TASK_FAULT_DETECT_PRIO                         34u
+#define TASK_VERT_DIRECTOR_SPEED_PWM_PRIO              21u
+#define TASK_VERT_DIRECTOR_SPEED_PWM_STK_SIZE          150u
+/***************/
+
+
+#define TASK_PS_PRIO                                   23u
+#define TASK_PS_STK_SIZE                               80u
+
+#define TASK_HORI_ROATE_MONITOR_PRIO                   24u
+#define TASK_HORI_ROATE_MONITOR_STK_SIZE               120u
+
+#define TASK_VERT_ROATE_MONITOR_PRIO                   25u
+#define TASK_VERT_ROATE_MONITOR_STK_SIZE               120u
+
+#define TASK_ELECTRIC_STABLE_PRIO                      26u
+#define TASK_ELECTRIC_STABLE_STK_SIZE                  150u
+
+#define TASK_VERT_ELECTRIC_STABLE_PWM_PRIO             27u
+#define TASK_VERT_ELECTRIC_STABLE_PWM_SIZE             150u
+
+#define TASK_PTZ_UPDATE_RECE_DATA_PRIO                 28u//接收升级数据
+#define TASK_PTZ_UPDATE_RECE_DATA_STK_SIZE             170u
+
+#define TASK_VERT_SELF_CHECK_PRIO                      29u
+#define TASK_VERT_SELF_CHECK_STK_SIZE                  180u
+
+#define TASK_HORI_SELF_CHECK_PRIO                      32u
+#define TASK_HORI_SELF_CHECK_STK_SIZE                  180u
+
+// #define TASK_VERT_PID_PRIO                             34u
+// #define TASK_VERT_PID_STK_SIZE                         150u
+
+#define TASK_FAULT_DETECT_PRIO                         35u
 #define TASK_FAULT_DETECT_STK_SIZE                     180u
 /**/
-#define TASK_AREA_SCAN_PRIO                            35u
+#define TASK_AREA_SCAN_PRIO                            36u
 #define TASK_AREA_SCAN_STK_SIZE                        120u
 
-#define TASK_PRESET_BIT_SCAN_PRIO                      36u
+#define TASK_PRESET_BIT_SCAN_PRIO                      37u
 #define TASK_PRESET_BIT_SCAN_STK_SIZE                  100u
 
-#define TASK_LISTEN_PTZ_SERVER_PRIO                    37u//云台接收指令分析
+#define TASK_LISTEN_PTZ_SERVER_PRIO                    39u//云台接收指令分析
 #define TASK_LISTEN_PTZ_SERVER_STK_SIZE                600
 
-#define TASK_PTZ_UPDATE_DATA_PROCESS_PRIO              39u//处理升级数据
+#define TASK_PTZ_UPDATE_DATA_PROCESS_PRIO              40u//处理升级数据
 #define TASK_PTZ_UPDATE_DATA_PROCESS_STK_SIZE          200u
 
-#define TASK_PTZ_UART_485_LASER_PROCESS_PRIO           40u//处理串口数据
+#define TASK_PTZ_UART_485_LASER_PROCESS_PRIO           41u//处理串口数据
 #define TASK_PTZ_UART_485_LASER_PROCESS_STK_SIZE       150u
 
-#define TASK_PTZ_UART_485_DATA_PROCESS_PRIO            41u//处理串口数据
+#define TASK_PTZ_UART_485_DATA_PROCESS_PRIO            42u//处理串口数据
 #define TASK_PTZ_UART_485_DATA_PROCESS_STK_SIZE        300u
 
-#define TASK_PTZ_UART_422_DATA_PROCESS_PRIO            42u//处理串口数据
+#define TASK_PTZ_UART_422_DATA_PROCESS_PRIO            43u//处理串口数据
 #define TASK_PTZ_UART_422_DATA_PROCESS_STK_SIZE        300u
 
-#define TASK_PTZ_AUTO_RETURN_PRIO                      43u//角度回传任务
+#define TASK_PTZ_AUTO_RETURN_PRIO                      44u//角度回传任务
 #define TASK_PTZ_AUTO_RETURN_STK_SIZE                  180u
 
-#define TASK_PTZ_SPEED_RETURN_PRIO                     44u//角度回传任务
+#define TASK_PTZ_SPEED_RETURN_PRIO                     45u//角度回传任务
 #define TASK_PTZ_SPEED_RETURN_STK_SIZE                 180u
 
-#define TASK_PTZ_DATA_COLLECT_PRIO                     45u
+#define TASK_PTZ_DATA_COLLECT_PRIO                     46u
 #define TASK_PTZ_DATA_COLLECT_STK_SIZE                 200u
 
-#define TASK_PTZ_HEAT_RESISTOR_PRIO                    46u
+#define TASK_PTZ_HEAT_RESISTOR_PRIO                    47u
 #define TASK_PTZ_HEAT_RESISTOR_STK_SIZE                60u
 
-#define TASK_PTZ_RESTORE_PRIO                          47u
+#define TASK_PTZ_RESTORE_PRIO                          48u
 #define TASK_PTZ_RESTORE_STK_SIZE                      60u
 
 
-#define TASK_PTZ_ERROR_COUNT_PRIO                          48u
+#define TASK_PTZ_ERROR_COUNT_PRIO                          49u
 #define TASK_PTZ_ERROR_COUNT_STK_SIZE                      200u
 
 
@@ -145,13 +265,7 @@
 #define TASK_PTZ_TASK_PRINTF_STK_SIZE                      200u
 
 
-//解析水平电机返回的数据
-#define TASK_RECV_HORI_SERVO_PRIO                          56u
-#define TASK_RECV_HORI_SERVO_STK_SIZE                      200u
 
-//解析俯仰电机返回的数据
-#define TASK_RECV_VERT_SERVO_PRIO                          57u
-#define TASK_RECV_VERT_SERVO_STK_SIZE                      200u
 
 
 

APP/Appcfg/os_cfg.h

@@ -135,6 +135,6 @@
 #define OS_TMR_CFG_MAX            12u  /*     Maximum number of timers                                 */
 #define OS_TMR_CFG_NAME_EN        1u   /*     Determine timer names                                    */
 #define OS_TMR_CFG_WHEEL_SIZE     8u   /*     Size of timer wheel (#Spokes)                            */
-#define OS_TMR_CFG_TICKS_PER_SEC  1u   /* 1s    Rate at which timer management task runs (Hz)            */
+#define OS_TMR_CFG_TICKS_PER_SEC  5u   /* 1s    Rate at which timer management task runs (Hz)            */
                                       
 #endif

APP/Common/ptz_default_value.h

@@ -537,7 +537,7 @@
     #define PTZ_HORI_MOTOR_DIRECTION    1
     ///水平电机加速度时间常数  单位ms (0-->>1000rpm)
     #define PTZ_HORI_MOTOR_AccelerationTimeConstant    3000
-    ///水平电机加速度时间常数  单位ms (1000-->>0rpm)
+    ///水平电机减速度时间常数  单位ms (1000-->>0rpm)
     #define PTZ_HORI_MOTOR_DecelerationTimeConstant    2000
 
 

APP/Device/Device_rotate/rotate_servo.c

@@ -1,12 +1,14 @@
 
 #include "rotate_servo.h"
-
+#include "speed_to_servoMotor.h"
 
 #ifdef PTZ_MEDIUM_WORM_SERVO_MOTOR_24V
 
 static BSP_OS_SEM ptz_hori_stop_mutex;//共享资源锁
 static BSP_OS_SEM ptz_vert_stop_mutex;
 
+static char ptz_hori_stop_count;//水平停止计数，防止多次停止多次延时
+static char ptz_vert_stop_count;//垂直停止计数，防止多次停止多次延时
 
 float ptz_vert_break_angle()
 {
@@ -49,10 +51,83 @@ void ptz_sem_post_stop_mutex()
     BSP_OS_SemPost(&ptz_vert_stop_mutex);
 }
 
-void ptz_hori_start(char direction, float speed)
+/*!
+    \brief      设置伺服电机速度
+    \param[in]  data：储存发送数据的链表
+    \param[out] none 
+    \retval     none
+*/
+void set_speed_to_servoMotor(uint8_t motorType, float speed)
+{
+    //转换为电机端的r/min
+    speed = speed * PTZ_HORI_RATIO + 0.5;
+
+    if (motorType == horiMotorType) {
+        if (speed > PTZ_HORI_MOTOR_MAX_SPEED) {
+            speed = PTZ_HORI_MOTOR_MAX_SPEED;
+        }
+        else if (speed < PTZ_HORI_MIN_SPEED) {
+            speed = PTZ_HORI_MOTOR_MIN_SPEED;
+        }
+        //设置速度，电机的r/min范围-6000~6000
+        servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, (uint16_t)speed)
+                                , WRITE_ONE_REG_FRAME_NUM, lowPriority);
+    }
+    else if (motorType == vertMotorType) {
+        if (speed > PTZ_VERT_MOTOR_MAX_SPEED) {
+            speed = PTZ_VERT_MOTOR_MAX_SPEED;
+        }
+        else if (speed < PTZ_VERT_MOTOR_MIN_SPEED) {
+            speed = PTZ_VERT_MOTOR_MIN_SPEED;
+        }
+   
+        //设置速度，电机的r/min范围-6000~6000
+        servoSendData(vertMotorType, WriteMotorOneReg(V_MOTOR, H06_SPEED_COMMU_SET_VALUE, (uint16_t)speed)
+                                , WRITE_ONE_REG_FRAME_NUM, lowPriority);
+    }
+    
+}
+
+/*
+      ///云台水平右转
+      #define PTZ_HORI_DIR_RIGHT     1
+      ///云台水平左转
+      #define PTZ_HORI_DIR_LEFT      3//0
+      ///云台处于停止状态
+      #define PTZ_HORI_DIR_STOP      2
+*/
+void ptz_hori_start(char direction, float speed)//输入参数的speed是云台末端的r/min
 {
     BSP_OS_SemWait(&ptz_hori_stop_mutex, 0u);
-
+    switch ( direction )
+    {
+        case PTZ_HORI_DIR_RIGHT://伺服电机默认速度为正使云台右转
+        case PTZ_HORI_DIR_STOP:
+            break;    
+        case PTZ_HORI_DIR_LEFT:
+            speed = -speed;
+        default:
+            break;
+    }
+    /*
+        -------------------------------------add speed change to here--------------------------------------
+    */
+    //设定转动速度
+    // servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, speed)//设置速度，电机的r/min范围-6000~6000
+    //                           , WRITE_ONE_REG_FRAME_NUM, lowPriority);
+    set_speed_to_servoMotor(horiMotorType, speed);
+    servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 1)
+                              , WRITE_ONE_REG_FRAME_NUM, lowPriority); //使能电机
+    g_ptz.hori_speed_set = speed;
+    //设定转动方向
+    g_ptz.hori_direction_last = g_ptz.hori_direction_set;
+    g_ptz.hori_direction_set = direction;
+    g_ptz.hori_direction_actual = g_ptz.hori_direction_set;
+    //启动电机
+    g_ptz.hori_start_stop_set = PTZ_HORI_START;
+    g_ptz.hori_start_stop_actual = g_ptz.hori_start_stop_set;
+        
+    ptz_hori_stop_count = 0;
     BSP_OS_SemPost(&ptz_hori_stop_mutex);
 }
 
@@ -60,13 +135,68 @@ void ptz_hori_stop(unsigned short int time)
 {
     BSP_OS_SemWait(&ptz_hori_stop_mutex, 0u);
 
+    //停止电机
+    g_ptz.hori_start_stop_set = PTZ_HORI_STOP;
+    g_ptz.hori_start_stop_actual = g_ptz.hori_start_stop_set;
+
+    g_ptz.hori_direction_actual = PTZ_HORI_DIR_STOP;
+  
+    
+    //设定转动速度
+    // servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, 0)//设置速度
+    //                           , WRITE_ONE_REG_FRAME_NUM, lowPriority);
+    set_speed_to_servoMotor(horiMotorType, 0);
+    g_ptz.hori_speed_set = 0;
+    g_ptz.hori_speed_actual = 0;
+  
+    if(ptz_hori_stop_count <= 0)
+    {
+        OSTimeDlyHMSM(0u, 0u, 0u, time);
+        ptz_hori_stop_count = 0;
+    }
+    ptz_hori_stop_count ++;
+    servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 0)
+                              , WRITE_ONE_REG_FRAME_NUM, highPriority); //失能电机 
+  //电子稳定
+#ifdef PTZ_ELECTRIC_STABLE_L6235D
+  ptz_hori_electric_stable_init();
+#endif
+    
     BSP_OS_SemPost(&ptz_hori_stop_mutex);
 }
 
-void ptz_vert_start(char direction, float speed)
+void ptz_vert_start(char direction, float speed)//输入参数的speed是云台末端的r/min
 {
     BSP_OS_SemWait(&ptz_vert_stop_mutex, 0u);
 
+    switch ( direction )
+    {
+        case PTZ_VERT_DIR_UP://伺服电机默认速度为正使云台向上
+        case PTZ_VERT_DIR_DOWN:
+            break;    
+        case PTZ_VERT_DIR_STOP:
+            speed = -speed;
+        default:
+            break;
+    }
+    /*
+        -------------------------------------add speed change to here--------------------------------------
+    */
+    //设定转动速度
+    set_speed_to_servoMotor(vertMotorType, speed);
+    servoSendData(vertMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 1)
+                              , WRITE_ONE_REG_FRAME_NUM, lowPriority); //使能电机
+    g_ptz.hori_speed_set = speed;
+    //设定转动方向
+    g_ptz.hori_direction_last = g_ptz.hori_direction_set;
+    g_ptz.hori_direction_set = direction;
+    g_ptz.hori_direction_actual = g_ptz.hori_direction_set;
+    //启动电机
+    g_ptz.hori_start_stop_set = PTZ_HORI_START;
+    g_ptz.hori_start_stop_actual = g_ptz.hori_start_stop_set;
+        
+    ptz_vert_stop_count = 0;
+    
     BSP_OS_SemPost(&ptz_vert_stop_mutex);
 }
 
@@ -74,6 +204,32 @@ void ptz_vert_stop(unsigned short int time)
 {
     BSP_OS_SemWait(&ptz_vert_stop_mutex, 0u);
 
+    //停止电机
+    g_ptz.vert_start_stop_set = PTZ_VERT_STOP;
+    g_ptz.vert_start_stop_actual = g_ptz.vert_start_stop_set;
+
+    g_ptz.vert_direction_actual = PTZ_VERT_DIR_STOP;
+  
+    
+    //设定转动速度
+    // servoSendData(vertMotorType, WriteMotorOneReg(V_MOTOR, H06_SPEED_COMMU_SET_VALUE, 0)//设置速度
+    //                           , WRITE_ONE_REG_FRAME_NUM, lowPriority);    
+    set_speed_to_servoMotor(vertMotorType, 0);
+    g_ptz.vert_speed_set = 0;
+    g_ptz.vert_speed_actual = 0;
+  
+    if(ptz_vert_stop_count <= 0)
+    {
+        OSTimeDlyHMSM(0u, 0u, 0u, time);
+        ptz_vert_stop_count = 0;
+    }
+    servoSendData(vertMotorType, WriteMotorOneReg(V_MOTOR, H03_DI1_LOGICAL_SELEC, 0)
+                              , WRITE_ONE_REG_FRAME_NUM, highPriority); //失能电机 
+    ptz_vert_stop_count ++;
+  //电子稳定
+#ifdef PTZ_ELECTRIC_STABLE_L6235D
+  ptz_hori_electric_stable_init();
+#endif
     BSP_OS_SemPost(&ptz_vert_stop_mutex);
 }
 
@@ -93,7 +249,7 @@ static void ptz_hori_rotate_task()
     while(1)
     {
         ptz_hori_rotate_monitor_task();
-        OSTimeDlyHMSM(0u, 0u, 0u, 1u);
+        OSTimeDlyHMSM(0u, 0u, 0u, 10u);
     }
 }
 
@@ -103,7 +259,7 @@ static void ptz_vert_rotate_task()
     while(1)
     {
         ptz_vert_rotate_monitor_task();
-        OSTimeDlyHMSM(0u, 0u, 0u, 1u);
+        OSTimeDlyHMSM(0u, 0u, 0u, 10u);
     }
 }
 
@@ -161,6 +317,8 @@ static  void  creat_task_vert_rotate(void)
 	}
 }
 
+
+
 void init_rotate_monitor_module(void)
 {
     BSP_OS_SemCreate(&ptz_hori_stop_mutex,1u,"ptz_hori_stop_mutex");

APP/Device/Device_rotate/rotate_servo.h

@@ -48,6 +48,8 @@ void ptz_hori_stop(unsigned short int time);
 void ptz_vert_start(char direction, float speed);
 void ptz_vert_stop(unsigned short int time);
 
+void set_speed_to_servoMotor(uint8_t motorType, float speed);
+
 void init_rotate_monitor_module(void);
 
 #endif

APP/Device/Device_speed/servoMotor_recv.c

@@ -9,6 +9,237 @@
 BSP_OS_SEM g_horiMotorMutex;
 BSP_OS_SEM g_vertMotorMutex;
 
+/**
+* @brief    写单个寄存器成功后，电机会返回一帧与写格式完全一致的数据
+* @param    motorNo：写垂直电机还是水平电机
+* @return   false:错误，读出的内容与写入的内容不一致
+*/
+static bool MotorReplyForWrite(uint8_t motorNo)
+{
+    static uint8_t motorReplybuff[WRITE_ONE_REG_FRAME_NUM];
+    if (motorNo == horiMotorType )
+    {
+        if ( highPriority == g_servoMotorLinkList.horiMotor.linkListPriority )
+        {
+
+            if ( g_servoMotorLinkList.horiMotor.LinkListHead_H->data[1] == WRITE_ONE_REG )//为写单个寄存器
+            {
+                /*提取数据失败*/
+                if ( CommuRsvData(motorNo, motorReplybuff, WRITE_ONE_REG_FRAME_NUM) == false )
+                {
+                    return false;
+                }
+                if ( 0 != memcmp( motorReplybuff, g_servoMotorLinkList.horiMotor.LinkListHead_H->data, WRITE_ONE_REG_FRAME_NUM ) )
+                {
+                    return false;
+                }
+                servoLinkListMemPut(horiMotorType);  
+            }
+            else
+            {
+              return false;
+            }
+        }
+        else if ( lowPriority == g_servoMotorLinkList.horiMotor.linkListPriority )
+        {
+
+            if ( g_servoMotorLinkList.horiMotor.LinkListHead_L->data[1] == WRITE_ONE_REG )//为写单个寄存器
+            {
+                /*提取数据失败*/
+                if ( CommuRsvData(motorNo, motorReplybuff, WRITE_ONE_REG_FRAME_NUM) == false )
+                {
+                    return false;
+                }
+                if ( 0 != memcmp( motorReplybuff, g_servoMotorLinkList.horiMotor.LinkListHead_L->data, WRITE_ONE_REG_FRAME_NUM ) )
+                {
+                    return false;
+                }
+                servoLinkListMemPut(horiMotorType);  
+            }
+            else
+            {
+              return false;
+            }
+        }
+//        servoLinkListMemPut(horiMotorType);    
+    }
+    else if ( motorNo == vertMotorType )
+    {
+        if ( highPriority == g_servoMotorLinkList.vertMotor.linkListPriority )
+        {
+
+            if ( g_servoMotorLinkList.vertMotor.LinkListHead_H->data[1] == WRITE_ONE_REG )//为写单个寄存器
+            {
+                /*提取数据失败*/
+                if ( CommuRsvData(motorNo, motorReplybuff, WRITE_ONE_REG_FRAME_NUM) == false )
+                {
+                    return false;
+                }
+                if ( 0 != memcmp( motorReplybuff, g_servoMotorLinkList.vertMotor.LinkListHead_H->data, WRITE_ONE_REG_FRAME_NUM ) )
+                {
+                    return false;
+                }
+                servoLinkListMemPut(vertMotorType);  
+            }
+            else
+            {
+              return false;
+            }
+        }
+        else if ( lowPriority == g_servoMotorLinkList.vertMotor.linkListPriority )
+        {
+
+            if ( g_servoMotorLinkList.vertMotor.LinkListHead_L->data[1] == WRITE_ONE_REG )//为写单个寄存器
+            {
+                /*提取数据失败*/
+                if ( CommuRsvData(motorNo, motorReplybuff, WRITE_ONE_REG_FRAME_NUM) == false )
+                {
+                    return false;
+                }
+                if ( 0 != memcmp( motorReplybuff, g_servoMotorLinkList.vertMotor.LinkListHead_L->data, WRITE_ONE_REG_FRAME_NUM ) )
+                {
+                    return false;
+                }
+                servoLinkListMemPut(vertMotorType);  
+            }
+            else
+            {
+              return false;
+            }
+        }
+//        servoLinkListMemPut(vertMotorType);   
+    }
+    
+    return true;
+}
+
+/**
+* @brief    读单个寄存器成功后，电机会返回一帧7字节数据
+* @param    motorNo：写垂直电机还是水平电机
+* @param    speed：获取读取命令后，电机返回的数据内容
+* @return   false:读取返回错误
+*/
+
+static bool MotorReplyForRead(uint8_t motorNo)
+{
+    int16_t speed = 0;
+    static uint8_t motorReplybuff[READ_ONE_REG_FRAME_NUM];
+    if (motorNo == horiMotorType )
+    {
+        if ( highPriority == g_servoMotorLinkList.horiMotor.linkListPriority )
+        {
+
+            if ( g_servoMotorLinkList.horiMotor.LinkListHead_H->data[1] == READ_ONE_REG )//为读单个寄存器
+            {
+                /*提取数据失败*/
+                if ( CommuRsvData(motorNo, motorReplybuff, READ_ONE_REG_FRAME_NUM-1) == false )
+                {
+                    return false;
+                }
+                if ( motorReplybuff[0] != H_MOTOR_ADDR )return false;//从机地址不对
+                if ( motorReplybuff[1] != READ_ONE_REG )return false;//功能码不对
+                if ( motorReplybuff[2] != 0x02 )return false;//返回数据的长度不对
+                speed = ((motorReplybuff[3]) << 8 ) | motorReplybuff[4];
+                if (speed < 0)
+                {
+                    speed = -speed;
+                }
+                g_ptz.hori_speed_actual = (float)speed / PTZ_HORI_RATIO;                
+                servoLinkListMemPut(horiMotorType); 
+            }
+            else
+            {
+              return false;
+            }
+        }
+        else if ( lowPriority == g_servoMotorLinkList.horiMotor.linkListPriority )
+        {
+
+            if ( g_servoMotorLinkList.horiMotor.LinkListHead_L->data[1] == READ_ONE_REG )//为读单个寄存器
+            {
+                /*提取数据失败*/
+                if ( CommuRsvData(motorNo, motorReplybuff, READ_ONE_REG_FRAME_NUM-1) == false )
+                {
+                    return false;
+                }
+                if ( motorReplybuff[0] != H_MOTOR_ADDR )return false;//从机地址不对
+                if ( motorReplybuff[1] != READ_ONE_REG )return false;//功能码不对
+                if ( motorReplybuff[2] != 0x02 )return false;//返回数据的长度不对
+                speed = ((motorReplybuff[3]) << 8 ) | motorReplybuff[4];
+                if (speed < 0)
+                {
+                    speed = -speed;
+                }
+                g_ptz.hori_speed_actual = (float)speed / PTZ_HORI_RATIO; 
+                servoLinkListMemPut(horiMotorType); 
+            }
+            else
+            {
+              return false;
+            }
+        }
+//        servoLinkListMemPut(horiMotorType);    
+    }
+    else if ( motorNo == vertMotorType )
+    {
+        if ( highPriority == g_servoMotorLinkList.vertMotor.linkListPriority )
+        {
+
+            if ( g_servoMotorLinkList.vertMotor.LinkListHead_H->data[1] == READ_ONE_REG )//为读单个寄存器
+            {
+                /*提取数据失败*/
+                if ( CommuRsvData(motorNo, motorReplybuff, READ_ONE_REG_FRAME_NUM-1) == false )
+                {
+                    return false;
+                }
+                if ( motorReplybuff[0] != V_MOTOR_ADDR )return false;//从机地址不对
+                if ( motorReplybuff[1] != READ_ONE_REG )return false;//功能码不对
+                if ( motorReplybuff[2] != 0x02 )return false;//返回数据的长度不对
+                speed = ((motorReplybuff[3]) << 8 ) | motorReplybuff[4];
+                if (speed < 0)
+                {
+                    speed = -speed;
+                }
+                g_ptz.vert_speed_actual = (float)speed / PTZ_VERT_RATIO;
+                servoLinkListMemPut(vertMotorType); 
+            }
+            else
+            {
+              return false;
+            }
+        }
+        else if ( lowPriority == g_servoMotorLinkList.vertMotor.linkListPriority )
+        {
+
+            if ( g_servoMotorLinkList.vertMotor.LinkListHead_L->data[1] == READ_ONE_REG )//为读单个寄存器
+            {
+                /*提取数据失败*/
+                if ( CommuRsvData(motorNo, motorReplybuff, READ_ONE_REG_FRAME_NUM-1) == false )
+                {
+                    return false;
+                }
+                if ( motorReplybuff[0] != V_MOTOR_ADDR )return false;//从机地址不对
+                if ( motorReplybuff[1] != READ_ONE_REG )return false;//功能码不对
+                if ( motorReplybuff[2] != 0x02 )return false;//返回数据的长度不对
+                speed = ((motorReplybuff[3]) << 8 ) | motorReplybuff[4];
+                if (speed < 0)
+                {
+                    speed = -speed;
+                }
+                g_ptz.vert_speed_actual = (float)speed / PTZ_VERT_RATIO;
+                servoLinkListMemPut(vertMotorType); 
+            }
+            else
+            {
+                return false;
+            }
+        }
+//        servoLinkListMemPut(vertMotorType);   
+    }
+    
+    return true;
+}
+
 /*!
     \brief      水平电机接收任务
     \param[in]  none
@@ -18,13 +249,17 @@ BSP_OS_SEM g_vertMotorMutex;
 static void ptz_recv_hori_servo_task()
 {
     CPU_INT08U  err;
-
     while(1) {
         OSSemPend(g_horiMotorMutex, 0, &err);
-
+        stopTimeOut(H_MOTOR);
-        
+        if ( ( MotorReplyForWrite(H_MOTOR) | MotorReplyForRead(H_MOTOR) ) == false)
-
+        {
-
+            // H_MOTOR_STOP;
+            continue;
+        }
+//        OSTimeDlyHMSM(0u, 0u, 0u, 5u);
+        //释放信号量，通知能发送一次
+        OSSemPost(g_horiSpeedMutex); 
     }
 }
 
@@ -37,12 +272,17 @@ static void ptz_recv_hori_servo_task()
 static void ptz_recv_vert_servo_task()
 {
     CPU_INT08U  err;
-
     while(1) {
         OSSemPend(g_vertMotorMutex, 0, &err);
-
+        stopTimeOut(V_MOTOR);
-
+        if ( ( MotorReplyForWrite(V_MOTOR) | MotorReplyForRead(V_MOTOR) ) == false)
-
+        {
+            // V_MOTOR_STOP;
+            continue;
+        }
+        
+        //释放信号量，通知能发送一次
+        OSSemPost(g_vertSpeedMutex);  
     }
 }
 
@@ -78,7 +318,7 @@ static  void  creat_task_servo_recv_task(void)
 		   (INT8U           ) TASK_RECV_VERT_SERVO_PRIO,                   
 		   (INT16U          ) TASK_RECV_VERT_SERVO_PRIO,
 		   (OS_STK         *)&task_recv_vert_servo_stk[0],
-		   (INT32U          ) task_recv_vert_servo_stk,
+		   (INT32U          ) TASK_RECV_HORI_SERVO_STK_SIZE,
 		   (void           *) 0,
 		   (INT16U          )(OS_TASK_OPT_STK_CHK | OS_TASK_OPT_STK_CLR));
     #if (OS_TASK_NAME_EN > 0)
@@ -95,8 +335,11 @@ static  void  creat_task_servo_recv_task(void)
 
 void Init_ServoMotorRecv(void)
 {
+    CPU_INT08U  err;
     g_horiMotorMutex = OSSemCreate(1);
     g_vertMotorMutex = OSSemCreate(1);
+    OSSemPend(g_horiMotorMutex, 1, &err);
+    OSSemPend(g_vertMotorMutex, 1, &err);
 
     creat_task_servo_recv_task();
 }

APP/Device/Device_speed/speed_to_servoMotor.c

@@ -9,14 +9,11 @@ BSP_OS_SEM g_vertSpeedSem;
 BSP_OS_SEM g_horiSpeedMutex;
 BSP_OS_SEM g_vertSpeedMutex;
 
-
-
 uint16_t memBuffer[linkListNodeLen * sendDataBufNumber * 2] = {0};
 OS_MEM *g_memPtr;
 
 ptzServoLinkList g_servoMotorLinkList = {0};
 
-
 //发送云台实际转速
 void ptz_send_speed(char dev, char speed)
 {
@@ -62,16 +59,81 @@ void ptz_send_speed(char dev, char speed)
     \param[out] none 
     \retval     none
 */
-void servoLinkListMemPut(linkList *data)
+void servoLinkListMemPut(uint8_t motorType)
 {
-    if (data == NULL) {
+    //水平电机
-        return;
+    if (motorType == horiMotorType) {
+        //当前发送完成的数据为高优先级链表中的数据
+        if (g_servoMotorLinkList.horiMotor.linkListPriority == highPriority) {
+            if (g_servoMotorLinkList.horiMotor.LinkListHead_H != NULL) {
+                linkList *ptr;
+                ptr = g_servoMotorLinkList.horiMotor.LinkListHead_H;
+                if (ptr->next != NULL) {
+                    g_servoMotorLinkList.horiMotor.LinkListHead_H = ptr->next;
+                }
+                else {
+                    g_servoMotorLinkList.horiMotor.LinkListHead_H = NULL;
+                    g_servoMotorLinkList.horiMotor.LinkListTail_H = NULL;
+                }  
+                g_servoMotorLinkList.horiMotor.linkListNum--;
+                OSMemPut(g_memPtr, (uint8_t *)ptr);
+            }            
+        }
+        //当前发送完成的数据为低优先级链表中的数据
+        else if (g_servoMotorLinkList.horiMotor.linkListPriority == lowPriority) {
+            if (g_servoMotorLinkList.horiMotor.LinkListHead_L != NULL) {
+                linkList *ptr;
+                ptr = g_servoMotorLinkList.horiMotor.LinkListHead_L;
+                if (ptr->next != NULL) {
+                    g_servoMotorLinkList.horiMotor.LinkListHead_L = ptr->next;
+                }
+                else {
+                    g_servoMotorLinkList.horiMotor.LinkListHead_L = NULL;
+                    g_servoMotorLinkList.horiMotor.LinkListTail_L = NULL;
+                }     
+                g_servoMotorLinkList.horiMotor.linkListNum--;
+                OSMemPut(g_memPtr, (uint8_t *)ptr);
+            }            
+        }
+//        g_servoMotorLinkList.horiMotor.linkListNum--;
+    }
+    else if (motorType == vertMotorType) {
+        if (g_servoMotorLinkList.vertMotor.linkListPriority == highPriority) {
+            if (g_servoMotorLinkList.vertMotor.LinkListHead_H != NULL) {
+                linkList *ptr;
+                ptr = g_servoMotorLinkList.vertMotor.LinkListHead_H;
+                if (ptr->next != NULL) {
+                    g_servoMotorLinkList.vertMotor.LinkListHead_H = ptr->next;
+                }
+                else {
+                    g_servoMotorLinkList.vertMotor.LinkListHead_H = NULL;
+                    g_servoMotorLinkList.vertMotor.LinkListTail_H = NULL;
+                }
+                g_servoMotorLinkList.vertMotor.linkListNum--;
+                OSMemPut(g_memPtr, (uint8_t *)ptr);
+                
+            }
+        }
+        else if (g_servoMotorLinkList.vertMotor.linkListPriority == lowPriority) {
+            if (g_servoMotorLinkList.vertMotor.LinkListHead_L != NULL) {
+                linkList *ptr;
+                ptr = g_servoMotorLinkList.vertMotor.LinkListHead_L;
+                if (ptr->next != NULL) {
+                    g_servoMotorLinkList.vertMotor.LinkListHead_L = ptr->next;
+                }
+                else {
+                    g_servoMotorLinkList.vertMotor.LinkListHead_L = NULL;
+                    g_servoMotorLinkList.vertMotor.LinkListTail_L = NULL;
+                }
+                g_servoMotorLinkList.vertMotor.linkListNum--;
+                OSMemPut(g_memPtr, (uint8_t *)ptr);
+                
+            }
+        }
+//        g_servoMotorLinkList.vertMotor.linkListNum--;
     }
     
-    linkList *ptr;
+
-    ptr = data;
-    data = data->next;
-    OSMemPut(g_memPtr, (uint8_t *)ptr);
 }
 
 /*!
@@ -85,19 +147,19 @@ void servoLinkListMemPut(linkList *data)
     \param[out] none 
     \retval     none
 */
-void servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priority)
+bool servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priority)
 {
     if ((motor != horiMotorType) && (motor!= vertMotorType)) {
-        return;
+        return false;
     }
     if (data == NULL) {
-        return;
+        return false;
     }
     if (dataLen > sendDataBufLen) {
-        return;
+        return false;
     }
     if ((priority != highPriority) && (priority != lowPriority)) {
-        return;
+        return false;
     }
 
     linkList *ptr = NULL;
@@ -105,58 +167,110 @@ void servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priorit
 
     //保存数据到链表节点
     ptr = OSMemGet(g_memPtr, &err);
+    if (ptr == NULL) {
+        return false;
+    }
     ptr->length = dataLen;
     memcpy(ptr->data, data, dataLen);
 
     //将节点添加进入链表中
     if (motor == horiMotorType) {
-        if (g_servoMotorLinkList.horiMotor.linkListNum > sendDataBufNumber) {
+        if (priority == highPriority) {
-            OSMemPut(g_memPtr, ptr);
+            if (g_servoMotorLinkList.horiMotor.linkListNum >= sendDataBufNumber) {
-            return;
+                OSMemPut(g_memPtr, ptr);
+                return false;
+            }
         }
+        else {
+            if (g_servoMotorLinkList.horiMotor.linkListNum >= sendDataBufNumber - 1) {
+                OSMemPut(g_memPtr, ptr);
+                return false;
+            }
+        }        
 
         if (priority == highPriority) {
-            g_servoMotorLinkList.horiMotor.LinkListTail_H = ptr;
+            if (g_servoMotorLinkList.horiMotor.LinkListTail_H == NULL) {
+                g_servoMotorLinkList.horiMotor.LinkListTail_H = ptr;
+            }
+            else {
+                g_servoMotorLinkList.horiMotor.LinkListTail_H->next = ptr;
+                g_servoMotorLinkList.horiMotor.LinkListTail_H = ptr;
+            }
+
             if (g_servoMotorLinkList.horiMotor.LinkListHead_H == NULL) {
                 g_servoMotorLinkList.horiMotor.LinkListHead_H
                     = g_servoMotorLinkList.horiMotor.LinkListTail_H;
             }
+            g_servoMotorLinkList.horiMotor.linkListNum++;
         }
         else {
-            g_servoMotorLinkList.horiMotor.LinkListTail_L = ptr;
+            if (g_servoMotorLinkList.horiMotor.LinkListTail_L == NULL) {
+                g_servoMotorLinkList.horiMotor.LinkListTail_L = ptr;
+            }
+            else {
+                g_servoMotorLinkList.horiMotor.LinkListTail_L->next = ptr;
+                g_servoMotorLinkList.horiMotor.LinkListTail_L = ptr;
+            }
+
             if (g_servoMotorLinkList.horiMotor.LinkListHead_L == NULL) {
                 g_servoMotorLinkList.horiMotor.LinkListHead_L
                     = g_servoMotorLinkList.horiMotor.LinkListTail_L;
-            }            
+            }
+            g_servoMotorLinkList.horiMotor.linkListNum++;
         }
 
         //释放信号量，通知能发送一次
         OSSemPost(g_horiSpeedSem);
     }
     else {
-        if (g_servoMotorLinkList.vertMotor.linkListNum > sendDataBufNumber) {
+        if (priority == highPriority) {
-            OSMemPut(g_memPtr, ptr);
+            if (g_servoMotorLinkList.vertMotor.linkListNum >= sendDataBufNumber) {
-            return;
+                OSMemPut(g_memPtr, ptr);
+                return false;
+            }
+        }
+        else {
+            if (g_servoMotorLinkList.vertMotor.linkListNum >= sendDataBufNumber - 1) {
+                OSMemPut(g_memPtr, ptr);
+                return false;
+            }
         }
 
         if (priority == highPriority) {
-            g_servoMotorLinkList.vertMotor.LinkListTail_H = ptr;
+            if (g_servoMotorLinkList.vertMotor.LinkListTail_H == NULL) {
+                g_servoMotorLinkList.vertMotor.LinkListTail_H = ptr;
+            }
+            else {
+                g_servoMotorLinkList.vertMotor.LinkListTail_H->next = ptr;
+                g_servoMotorLinkList.vertMotor.LinkListTail_H = ptr;
+            }
+
             if (g_servoMotorLinkList.vertMotor.LinkListHead_H == NULL) {
                 g_servoMotorLinkList.vertMotor.LinkListHead_H
                     = g_servoMotorLinkList.vertMotor.LinkListTail_H;
             }
+            g_servoMotorLinkList.vertMotor.linkListNum++;
         }
         else {
-            g_servoMotorLinkList.vertMotor.LinkListTail_L = ptr;
+            if (g_servoMotorLinkList.vertMotor.LinkListTail_L == NULL) {
+                g_servoMotorLinkList.vertMotor.LinkListTail_L = ptr;
+            }
+            else {
+                g_servoMotorLinkList.vertMotor.LinkListTail_L->next = ptr;
+                g_servoMotorLinkList.vertMotor.LinkListTail_L = ptr;
+            }
+            
             if (g_servoMotorLinkList.vertMotor.LinkListHead_L == NULL) {
                 g_servoMotorLinkList.vertMotor.LinkListHead_L
                     = g_servoMotorLinkList.vertMotor.LinkListTail_L;
-            }            
+            }   
+            g_servoMotorLinkList.vertMotor.linkListNum++;
         }
 
         //释放信号量，通知能发送一次
         OSSemPost(g_vertSpeedSem);
     }
+    return true;
 }
 
 static void ptz_hori_servo_task()
@@ -176,20 +290,22 @@ static void ptz_hori_servo_task()
         // 高优先级链表中数据先发送
         if (g_servoMotorLinkList.horiMotor.LinkListHead_H != NULL) {
             //发送数据
-
+            CommuTransData(H_MOTOR, g_servoMotorLinkList.horiMotor.LinkListHead_H->data
-
+                                    , g_servoMotorLinkList.horiMotor.LinkListHead_H->length);
             
-            g_servoMotorLinkList.horiMotor.linkListNum = highPriority;
+            g_servoMotorLinkList.horiMotor.linkListPriority = highPriority;
+            startTimeOut(horiMotorType);
             continue;
         }
         
         // 高优先级链表中无数据时，发送低优先级中的数据
         if (g_servoMotorLinkList.horiMotor.LinkListHead_L != NULL) {
             //发送数据
+            CommuTransData(H_MOTOR, g_servoMotorLinkList.horiMotor.LinkListHead_L->data
+                                    , g_servoMotorLinkList.horiMotor.LinkListHead_L->length);
 
-
+            g_servoMotorLinkList.horiMotor.linkListPriority = lowPriority;
-
+            startTimeOut(horiMotorType);
-            g_servoMotorLinkList.horiMotor.linkListNum = lowPriority;
         }
         else {
             pdebug(DEBUG_LEVEL_FATAL,"ptz_hori_servo_task  no data...\n\r");
@@ -213,23 +329,24 @@ static void ptz_vert_servo_task()
 
         // 高优先级链表中数据先发送
         if (g_servoMotorLinkList.vertMotor.LinkListHead_H != NULL) {
-            g_servoMotorLinkList.vertMotor.linkListNum = highPriority;
+            g_servoMotorLinkList.vertMotor.linkListPriority = highPriority;
-
+            CommuTransData(V_MOTOR,  g_servoMotorLinkList.vertMotor.LinkListHead_H->data
-
+                                    , g_servoMotorLinkList.vertMotor.LinkListHead_H->length);
-            
+            startTimeOut(V_MOTOR);
             continue;
         }
-        
+
         // 高优先级链表中无数据时，发送低优先级中的数据
         if (g_servoMotorLinkList.vertMotor.LinkListHead_L != NULL) {
-            g_servoMotorLinkList.vertMotor.linkListNum = highPriority;
+            g_servoMotorLinkList.vertMotor.linkListPriority = lowPriority;
-
+            CommuTransData(V_MOTOR, g_servoMotorLinkList.vertMotor.LinkListHead_L->data
-
+                                    , g_servoMotorLinkList.vertMotor.LinkListHead_L->length);
-            
+          startTimeOut(V_MOTOR);  
         }
         else {
             pdebug(DEBUG_LEVEL_FATAL,"ptz_vert_servo_task  no data...\n\r");
         }
+        
     }
 }
 
@@ -286,7 +403,103 @@ static  void  creat_task_vert_servo_task(void)
 	}
 }
 
+void horiServoTimeOut()
+{
+    servoLinkListMemPut(horiMotorType);
+    OSSemPost(g_horiSpeedMutex);
+}
 
+void vertServoTimeOut()
+{
+    servoLinkListMemPut(vertMotorType);
+    OSSemPost(g_vertSpeedMutex);
+}
+
+BSP_OS_TMR horiServoSoftWareTim;
+BSP_OS_TMR vertServoSoftWareTim;
+void servoCommSoftWareTimInit()
+{
+    CPU_INT08U  ServoSoftWareTimErr;
+    horiServoSoftWareTim = OSTmrCreate(5
+                                        , 100//1*200ms
+                                        , OS_TMR_OPT_ONE_SHOT
+                                        , (OS_TMR_CALLBACK)horiServoTimeOut
+                                        , (void *)0
+                                        , "tmr1"
+                                        , &ServoSoftWareTimErr);
+      
+    if ((ServoSoftWareTimErr == OS_ERR_NONE)) {
+        pdebug(DEBUG_LEVEL_INFO,"create horiServoSoftWareTim  success...\n\r");
+	} else { 
+        pdebug(DEBUG_LEVEL_FATAL,"create horiServoSoftWareTim  failed...\n\r");
+	}
+
+    vertServoSoftWareTim = OSTmrCreate(5
+                                        , 100
+                                        , OS_TMR_OPT_ONE_SHOT
+                                        , (OS_TMR_CALLBACK)vertServoTimeOut
+                                        , (void *)0
+                                        , "tmr2"
+                                        , &ServoSoftWareTimErr);
+          
+    if ((ServoSoftWareTimErr == OS_ERR_NONE)) {
+        pdebug(DEBUG_LEVEL_INFO,"create vertServoSoftWareTim  success...\n\r");
+	} else { 
+        pdebug(DEBUG_LEVEL_FATAL,"create vertServoSoftWareTim  failed...\n\r");
+	}
+}
+
+/*!
+    \brief      停止软件定时器
+    \param[in]  motor：电机
+                            horiMotorType 水平电机
+                            vertMotorType 俯仰电机
+    \param[out] none 
+    \retval     none
+*/
+void stopTimeOut(uint8_t motorType)
+{
+    if (motorType == horiMotorType) {
+        CPU_INT08U  err;
+        OSTmrStop(horiServoSoftWareTim, OS_TMR_OPT_NONE, (void *)0, &err);
+        if (err != OS_ERR_NONE) {
+            pdebug(DEBUG_LEVEL_FATAL,"start horiServoSoftWareTim  failed...\n\r");
+        }
+    }
+    else if (motorType == vertMotorType) {
+        CPU_INT08U  err;
+        OSTmrStop(vertServoSoftWareTim, OS_TMR_OPT_NONE, (void *)0, &err);
+        if (err != OS_ERR_NONE) {
+            pdebug(DEBUG_LEVEL_FATAL,"start vertServoSoftWareTim  failed...\n\r");
+        }
+    }
+}
+
+/*!
+    \brief      启动软件定时器
+    \param[in]  motor：电机
+                            horiMotorType 水平电机
+                            vertMotorType 俯仰电机
+    \param[out] none 
+    \retval     none
+*/
+void startTimeOut(uint8_t motorType)
+{
+    if (motorType == horiMotorType) {
+        CPU_INT08U  err;
+        OSTmrStart(horiServoSoftWareTim, &err);
+        if (err != OS_ERR_NONE) {
+            pdebug(DEBUG_LEVEL_FATAL,"start horiServoSoftWareTim  failed...\n\r");
+        }
+    }
+    else if (motorType == vertMotorType) {
+        CPU_INT08U  err;
+        OSTmrStart(vertServoSoftWareTim, &err);
+        if (err != OS_ERR_NONE) {
+            pdebug(DEBUG_LEVEL_FATAL,"start vertServoSoftWareTim  failed...\n\r");
+        }
+    }
+}
 
 void init_speed_module(void)
 {
@@ -303,7 +516,7 @@ void init_speed_module(void)
     if (err != OS_ERR_NONE) {
         pdebug(DEBUG_LEVEL_FATAL, "Failed to create the motor memory pool\n\r");
     }
-
+    
     //初始化链表头尾
     g_servoMotorLinkList.horiMotor.LinkListHead_H = NULL;
     g_servoMotorLinkList.horiMotor.LinkListHead_L = NULL;
@@ -324,6 +537,47 @@ void init_speed_module(void)
 
     creat_task_hori_servo_task();
     creat_task_vert_servo_task();
+
+    servoCommSoftWareTimInit();
+
+    OSTimeDlyHMSM(0u, 0u, 0u, 500u);//等待硬件初始化成功
+
+
+
+     servoSendData(horiMotorType, WriteMotorOneReg(horiMotorType, H02_CONTR_MODE_SELEC, 0),
+                             WRITE_ONE_REG_FRAME_NUM, lowPriority);//切换为速度控制模式
+//      OSTimeDlyHMSM(0u, 0u, 0u, 5u);
+     servoSendData(vertMotorType, WriteMotorOneReg(vertMotorType, H02_CONTR_MODE_SELEC, 0),
+                             WRITE_ONE_REG_FRAME_NUM, lowPriority);//切换为速度控制模式
+     OSTimeDlyHMSM(0u, 0u, 0u, 5u);
+
+    // servoSendData(horiMotorType, WriteMotorOneReg(horiMotorType, H02_MOTOR_DIR_SELEC, PTZ_HORI_MOTOR_DIRECTION)
+    //                     , WRITE_ONE_REG_FRAME_NUM, lowPriority);//设置水平电机方向
+    // servoSendData(vertMotorType, WriteMotorOneReg(vertMotorType, H02_MOTOR_DIR_SELEC, PTZ_VERT_MOTOR_DIRECTION)
+    //                     , WRITE_ONE_REG_FRAME_NUM, lowPriority);//设置垂直电机方向
+    // OSTimeDlyHMSM(0u, 0u, 0u, 5u);
+    
+    // servoSendData(horiMotorType, WriteMotorOneReg(horiMotorType, H06_SPEED_UP_SLOPE_VALUE, PTZ_HORI_MOTOR_AccelerationTimeConstant)
+    //                         , WRITE_ONE_REG_FRAME_NUM, lowPriority);//设置加速时间常数
+    // servoSendData(vertMotorType, WriteMotorOneReg(vertMotorType, H06_SPEED_UP_SLOPE_VALUE, PTZ_VERT_MOTOR_AccelerationTimeConstant)
+    //                         , WRITE_ONE_REG_FRAME_NUM, lowPriority);//设置加速时间常数
+    // OSTimeDlyHMSM(0u, 0u, 0u, 5u);
+    
+    // servoSendData(horiMotorType, WriteMotorOneReg(horiMotorType, H06_SPEED_DOWN_SLOPE_VALUE, PTZ_HORI_MOTOR_DecelerationTimeConstant)
+    //                         , WRITE_ONE_REG_FRAME_NUM, lowPriority);//设置减速时间常数
+    // servoSendData(vertMotorType, WriteMotorOneReg(vertMotorType, H06_SPEED_DOWN_SLOPE_VALUE, PTZ_VERT_MOTOR_DecelerationTimeConstant)
+    //                         , WRITE_ONE_REG_FRAME_NUM, lowPriority);//设置减速时间常数
+    // OSTimeDlyHMSM(0u, 0u, 0u, 5u);
+    
+    // servoSendData(i, WriteMotorOneReg(i, H06_SPEED_COMMU_SET_VALUE, 60)
+    //                       , WRITE_ONE_REG_FRAME_NUM, lowPriority);//设置电机转速为60r/min
+    // OSTimeDlyHMSM(0u, 0u, 0u, 5u);
+    
+     servoSendData(horiMotorType, WriteMotorOneReg(horiMotorType, H03_DI1_LOGICAL_SELEC, 1)
+                             , WRITE_ONE_REG_FRAME_NUM, lowPriority);//电机运行使能
+     servoSendData(vertMotorType, WriteMotorOneReg(vertMotorType, H03_DI1_LOGICAL_SELEC, 1)
+                             , WRITE_ONE_REG_FRAME_NUM, lowPriority);//电机运行使能
+     OSTimeDlyHMSM(0u, 0u, 0u, 5u);
 }
 
 

APP/Device/Device_speed/speed_to_servoMotor.h

@@ -33,6 +33,8 @@ typedef struct _linkList {
     uint8_t data[sendDataBufLen];
     //发送的数据长度
     uint16_t length;
+//    uint8_t funcCode;
+//    uint16_t regAddr;
     //下一个节点
     struct _linkList *next;
 } linkList;
@@ -67,8 +69,10 @@ extern ptzServoLinkList g_servoMotorLinkList;
 void ptz_send_speed(char dev, char speed);
 void init_speed_module(void);
 
-void servoLinkListMemPut(linkList *data);
+bool servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priority);
-void servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priority);
+void servoLinkListMemPut(uint8_t motorType);
+void stopTimeOut(uint8_t motorType);
+void startTimeOut(uint8_t motorType);
 
 #endif
 

APP/Device/device_Other/device_adc_collect.c

@@ -1,5 +1,7 @@
 #include "device_adc_collect.h"
 
+#include "rotate_servo.h"
+#include "speed_to_servoMotor.h"
 
 // ADC_Phase_current H_ADC_Collect;
 // ADC_Phase_current V_ADC_Collect;
@@ -487,21 +489,26 @@ static char ptz_data_collect_task()
 {
   int i=0,j=0;
   while(1)
-  {
+  { 
-    
+    // if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
-    
+    // {//电机处于启动状态
-    
+    //   // H_ADC2_Phase_current();
-    if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
+    // }
-    {//电机处于启动状态
+    // if(g_ptz.vert_start_stop_set == PTZ_VERT_START)
-      // H_ADC2_Phase_current();
+    // {//电机处于启动状态
-    }
+    //   // V_ADC0_Phase_current();
-    if(g_ptz.vert_start_stop_set == PTZ_VERT_START)
+    // }
-    {//电机处于启动状态
-      // V_ADC0_Phase_current();
-    }
     
     if(j >= 100)
     {
+
+        
+        // //读取水平电机实时速度
+        // servoSendData(horiMotorType, ReadMotorOneReg(horiMotorType, READ_MOTOR_SPEED_NOW)
+        //                     , READ_ONE_REG_FRAME_NUM, lowPriority);
+        // //读取俯仰电机实时速度
+        // servoSendData(vertMotorType, ReadMotorOneReg(vertMotorType, READ_MOTOR_SPEED_NOW)
+        //                     , READ_ONE_REG_FRAME_NUM, lowPriority);
       j=0;
       //云台不自检关闭，打开采集任务
 #ifndef PTZ_NO_SELF_CHECK

APP/Device/device_Other/device_interrupt.c

@@ -17,101 +17,7 @@
 /// @note       修改日志
 ///             LH于2022-05-25
 void EXTI_IRQ_init()
-{
+{  
-#ifdef L6235D
-  /*HALL中断，水平——PD13-H1,PD14-H2,PD15-H3，，垂直PC6-H1,PC7-H2,PC8-H3*/
-  nvic_irq_enable(EXTI10_15_IRQn, 3U, 3U);//水平，一个中断函数，中断线10-15
-  nvic_irq_enable(EXTI5_9_IRQn, 3U, 2U);//垂直，一个中断函数，中断线5-9
-  
-  //对应引脚配置外部中断线
-  syscfg_exti_line_config(EXTI_SOURCE_GPIOD, EXTI_SOURCE_PIN13);
-  syscfg_exti_line_config(EXTI_SOURCE_GPIOD, EXTI_SOURCE_PIN14);
-  syscfg_exti_line_config(EXTI_SOURCE_GPIOD, EXTI_SOURCE_PIN15);
-  syscfg_exti_line_config(EXTI_SOURCE_GPIOC, EXTI_SOURCE_PIN6);
-  syscfg_exti_line_config(EXTI_SOURCE_GPIOC, EXTI_SOURCE_PIN7);
-  syscfg_exti_line_config(EXTI_SOURCE_GPIOC, EXTI_SOURCE_PIN8);
-  
-  //外部中断线初始化
-  exti_init(EXTI_6, EXTI_INTERRUPT, EXTI_TRIG_RISING);//上升沿中断
-  exti_init(EXTI_7, EXTI_INTERRUPT, EXTI_TRIG_RISING);
-  exti_init(EXTI_8, EXTI_INTERRUPT, EXTI_TRIG_RISING);
-  exti_init(EXTI_13, EXTI_INTERRUPT, EXTI_TRIG_RISING);
-  exti_init(EXTI_14, EXTI_INTERRUPT, EXTI_TRIG_RISING);
-  exti_init(EXTI_15, EXTI_INTERRUPT, EXTI_TRIG_RISING);
-  //清除标志位
-  exti_interrupt_flag_clear(EXTI_6);
-  exti_interrupt_flag_clear(EXTI_7);
-  exti_interrupt_flag_clear(EXTI_8);
-  exti_interrupt_flag_clear(EXTI_13);
-  exti_interrupt_flag_clear(EXTI_14);
-  exti_interrupt_flag_clear(EXTI_15);
-  
-  BSP_IntVectSet(39,EXTI5_9_IRQHandler);
-  BSP_IntEn(39);
-  
-  BSP_IntVectSet(56,EXTI10_15_IRQHandler);
-  BSP_IntEn(56);
-#endif
-  
-#ifdef Full_bridge  //MOS+栅极驱动
-  rcu_periph_clock_enable(RCU_SYSCFG);
-  
-    //配置引脚时钟
-  rcu_periph_clock_enable(RCU_GPIOE);
-  
-  //水平——PE10-H1,PE11-H2,PE12-H3，，垂直PE13-H1,PE14-H2,PE15-H3
-  //设置引脚为输入模式
-  gpio_mode_set(GPIOE, GPIO_MODE_INPUT, GPIO_PUPD_NONE, GPIO_PIN_10);
-  gpio_mode_set(GPIOE, GPIO_MODE_INPUT, GPIO_PUPD_NONE, GPIO_PIN_11);
-  gpio_mode_set(GPIOE, GPIO_MODE_INPUT, GPIO_PUPD_NONE, GPIO_PIN_12);
-  
-  gpio_mode_set(GPIOE, GPIO_MODE_INPUT, GPIO_PUPD_NONE, GPIO_PIN_13);
-  gpio_mode_set(GPIOE, GPIO_MODE_INPUT, GPIO_PUPD_NONE, GPIO_PIN_14);
-  gpio_mode_set(GPIOE, GPIO_MODE_INPUT, GPIO_PUPD_NONE, GPIO_PIN_15);
-  
-  /*HALL中断，水平——PE10-H1,PE11-H2,PE12-H3，，垂直PE13-H1,PE14-H2,PE15-H3*/
-  //对应引脚配置外部中断线
-  syscfg_exti_line_config(EXTI_SOURCE_GPIOE, EXTI_SOURCE_PIN10);
-  syscfg_exti_line_config(EXTI_SOURCE_GPIOE, EXTI_SOURCE_PIN11);
-  syscfg_exti_line_config(EXTI_SOURCE_GPIOE, EXTI_SOURCE_PIN12);
-  
-  syscfg_exti_line_config(EXTI_SOURCE_GPIOE, EXTI_SOURCE_PIN13);
-  syscfg_exti_line_config(EXTI_SOURCE_GPIOE, EXTI_SOURCE_PIN14);
-  syscfg_exti_line_config(EXTI_SOURCE_GPIOE, EXTI_SOURCE_PIN15);
-  
-  
-  //外部中断线初始化
-  exti_init(EXTI_10, EXTI_INTERRUPT, EXTI_TRIG_RISING);//上升沿中断
-  exti_init(EXTI_11, EXTI_INTERRUPT, EXTI_TRIG_RISING);
-  exti_init(EXTI_12, EXTI_INTERRUPT, EXTI_TRIG_RISING);
-  
-  exti_init(EXTI_13, EXTI_INTERRUPT, EXTI_TRIG_RISING);//上升沿中断
-  exti_init(EXTI_14, EXTI_INTERRUPT, EXTI_TRIG_RISING);
-  exti_init(EXTI_15, EXTI_INTERRUPT, EXTI_TRIG_RISING);
-  
-  exti_interrupt_enable(EXTI_10);
-  exti_interrupt_enable(EXTI_11);
-  exti_interrupt_enable(EXTI_12);
-  
-  exti_interrupt_enable(EXTI_13);
-  exti_interrupt_enable(EXTI_14);
-  exti_interrupt_enable(EXTI_15);
-  
-  
-  //清除标志位
-  exti_interrupt_flag_clear(EXTI_10);
-  exti_interrupt_flag_clear(EXTI_11);
-  exti_interrupt_flag_clear(EXTI_12);
-  
-  exti_interrupt_flag_clear(EXTI_13);
-  exti_interrupt_flag_clear(EXTI_14);
-  exti_interrupt_flag_clear(EXTI_15);
-  
-  nvic_irq_enable(EXTI10_15_IRQn, 2U, 2U);//，一个中断函数，中断线10-15
-  
-  BSP_IntVectSet(56,EXTI10_15_IRQHandler);
-  BSP_IntEn(56);
-  
   /*光电开关中断PE7-SW1，PE8-SW2，PE9-SW3*/
   syscfg_exti_line_config(EXTI_SOURCE_GPIOE, EXTI_SOURCE_PIN7);
   syscfg_exti_line_config(EXTI_SOURCE_GPIOE, EXTI_SOURCE_PIN8);
@@ -142,47 +48,6 @@ void EXTI_IRQ_init()
   
   BSP_IntVectSet(39,EXTI5_9_IRQHandler);
   BSP_IntEn(39);
-  
-#else
-  /*光电开关中断PB0-SW1，PB1-SW2，PB2-SW3*/
-  nvic_irq_enable(EXTI0_IRQn, 2U, 0U);//垂直光电开关SW1
-  nvic_irq_enable(EXTI1_IRQn, 2U, 1U);//垂直光电开关SW2
-  nvic_irq_enable(EXTI2_IRQn, 2U, 2U);//水平光电开关SW3 
-  
-  syscfg_exti_line_config(EXTI_SOURCE_GPIOB, EXTI_SOURCE_PIN0);
-  syscfg_exti_line_config(EXTI_SOURCE_GPIOB, EXTI_SOURCE_PIN1);
-  syscfg_exti_line_config(EXTI_SOURCE_GPIOB, EXTI_SOURCE_PIN2);
-  
-  //垂直光电开关SW1
-  #ifdef PTZ_SW1_DOWN_FALL_UPDATE
-    exti_init(EXTI_0, EXTI_INTERRUPT, EXTI_TRIG_FALLING);//下降沿中断
-  #endif
-    
-  #ifdef PTZ_SW1_UP_RISE_UPDATE
-    exti_init(EXTI_0, EXTI_INTERRUPT, EXTI_TRIG_BOTH);//边沿中断
-  #endif
-  //垂直光电开关SW2
-    exti_init(EXTI_1, EXTI_INTERRUPT, EXTI_TRIG_FALLING);//下降沿中断
-  //水平光电开关SW3
-  #ifdef PTZ_SW3_LEFT_RISE_UPDATE
-    exti_init(EXTI_2, EXTI_INTERRUPT, EXTI_TRIG_BOTH);//边沿中断
-  #else
-    exti_init(EXTI_2, EXTI_INTERRUPT, EXTI_TRIG_FALLING);//下降沿中断
-  #endif
-  //清除标志位
-  exti_interrupt_flag_clear(EXTI_0);
-  exti_interrupt_flag_clear(EXTI_1);
-  exti_interrupt_flag_clear(EXTI_2);
-  
-  BSP_IntVectSet(22,EXTI0_IRQHandler);
-  BSP_IntEn(22);
-  
-  BSP_IntVectSet(23,EXTI1_IRQHandler);
-  BSP_IntEn(23);
-  
-  BSP_IntVectSet(24,EXTI2_IRQHandler);
-  BSP_IntEn(24);
-#endif
 }
 
 
@@ -594,6 +459,194 @@ void ptz_SW_IRQHandler(exti_line_enum sw_linex)
 #endif
 #endif
   
+#ifdef PTZ_SERVO_MOTOR
+#ifdef PTZ_NO_SELF_CHECK
+  if(g_ptz.Voltage > SWITCH_IRQ_V)
+  {
+    switch(sw_linex)
+    {   
+        case EXTI_7://垂直光电开关SW1
+  #ifdef PTZ_PHOTOELECTRIC_SWITCH
+                 asm("nop");
+                 asm("nop");
+                 asm("nop");
+                 asm("nop");
+                 asm("nop");
+          if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_UP)
+          {
+            if(PS_VERT_SW1_READ == PS_HIGH)
+            {
+              g_ptz.vert_ps_sw1_up_rise ++;
+            }
+          }
+          if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_DOWN)
+          {
+            if(PS_VERT_SW1_READ == PS_LOW)
+            {
+              g_ptz.vert_ps_sw1_down_fall ++;
+            }
+          }
+  #endif
+          
+          break;
+          
+        case EXTI_8://垂直光电开关SW2
+  #ifdef PTZ_PHOTOELECTRIC_SWITCH
+                 asm("nop");
+                 asm("nop");
+                 asm("nop");
+                 asm("nop");
+                 asm("nop");
+          if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_UP)
+          {
+            if(PS_VERT_SW2_READ == PS_LOW)
+            {
+              g_ptz.vert_ps_sw2_up_fall ++;
+            }
+          }
+          if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_DOWN)
+          {
+            if(PS_VERT_SW2_READ == PS_HIGH)
+            {
+              g_ptz.vert_ps_sw2_down_rise ++;
+            }
+          }
+  #endif
+          
+  
+          break;
+          
+        case EXTI_9://水平光电开关SW3
+  #ifdef PTZ_PHOTOELECTRIC_SWITCH
+                 asm("nop");
+                 asm("nop");
+                 asm("nop");
+                 asm("nop");
+                 asm("nop");
+          if(PS_HORI_SW3_READ == PS_LOW)
+          {
+            if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_RIGHT)
+            {
+              g_ptz.hori_ps_sw3_right_fall ++;
+            }
+            if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_LEFT)
+            {
+              g_ptz.hori_ps_sw3_left_fall ++;
+            } 
+          }
+          if(PS_HORI_SW3_READ == PS_HIGH)//PS_HIGH
+          {
+            if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_RIGHT)
+            {
+              g_ptz.hori_ps_sw3_right_rise ++;
+            }
+            if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_LEFT)
+            {
+              g_ptz.hori_ps_sw3_left_rise ++;
+//              term_printf(" hori_ps_sw3_left_rise \r\n");//中断不能使用串口打印，会占用较长时间
+            }
+          }
+  #endif
+          
+          break;
+         
+        default:break;
+    }
+
+  }
+#else
+
+  switch(sw_linex)
+  {
+      case EXTI_7://垂直光电开关SW1
+  #ifdef PTZ_PHOTOELECTRIC_SWITCH
+                 asm("nop");
+                 asm("nop");
+                 asm("nop");
+                 asm("nop");
+                 asm("nop");
+          if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_UP)
+          {
+            if(PS_VERT_SW1_READ == PS_HIGH)
+            {
+              g_ptz.vert_ps_sw1_up_rise ++;
+            }
+          }
+          if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_DOWN)
+          {
+            if(PS_VERT_SW1_READ == PS_LOW)
+            {
+              g_ptz.vert_ps_sw1_down_fall ++;
+            }
+          }
+  #endif
+          
+          break;
+          
+        case EXTI_8://垂直光电开关SW2
+  #ifdef PTZ_PHOTOELECTRIC_SWITCH
+                 asm("nop");
+                 asm("nop");
+                 asm("nop");
+                 asm("nop");
+                 asm("nop");
+          if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_UP)
+          {
+            if(PS_VERT_SW2_READ == PS_LOW)
+            {
+              g_ptz.vert_ps_sw2_up_fall ++;
+            }
+          }
+          if(g_ptz.vert_direction_actual == PTZ_VERT_DIR_DOWN)
+          {
+            if(PS_VERT_SW2_READ == PS_HIGH)
+            {
+              g_ptz.vert_ps_sw2_down_rise ++;
+            }
+          }
+  #endif
+          
+  
+          break;
+          
+        case EXTI_9://水平光电开关SW3
+  #ifdef PTZ_PHOTOELECTRIC_SWITCH
+                 asm("nop");
+                 asm("nop");
+                 asm("nop");
+                 asm("nop");
+                 asm("nop");
+          if(PS_HORI_SW3_READ == PS_LOW)
+          {
+            if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_RIGHT)
+            {
+              g_ptz.hori_ps_sw3_right_fall ++;
+            }
+            if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_LEFT)
+            {
+              g_ptz.hori_ps_sw3_left_fall ++;
+            } 
+          }
+          if(PS_HORI_SW3_READ == PS_HIGH)
+          {
+            if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_RIGHT)
+            {
+              g_ptz.hori_ps_sw3_right_rise ++;
+            }
+            if(g_ptz.hori_direction_actual == PTZ_HORI_DIR_LEFT)
+            {
+              g_ptz.hori_ps_sw3_left_rise ++;
+//              term_printf(" hori_ps_sw3_left_rise \r\n");//中断不能使用串口打印，会占用较长时间
+            }
+          }
+  #endif
+          
+          break;
+         
+        default:break;
+  }
+#endif
+#endif
 }
 
 /// @brief      水平HALL中断处理函数 

APP/Device/device_Other/device_interrupt.h

@@ -51,6 +51,11 @@
 
     #endif
 
+    //
+    #ifdef PTZ_MEDIUM_WORM_SERVO_MOTOR_24V
+          #define SWITCH_IRQ_V    20
+    #endif
+
 void ptz_SW_IRQHandler(exti_line_enum sw_linex);
 void ptz_H_HALL_IRQHandler(exti_line_enum hall_linex);
 void ptz_V_HALL_IRQHandler(exti_line_enum hall_linex);

APP/Device/device_Other/device_wdog.c

@@ -16,6 +16,7 @@
 #include "gd32f4xx_gpio.h"
 #include  <includes.h>
 #include "device_wdog.h"
+
 /// 引脚初始化 
 ///
 /// @param  none   
@@ -46,8 +47,7 @@ static  void task_feeddog ()
   {
     OSTimeDlyHMSM(0u,0u,0u,20u);
     //翻转电平喂狗
-    gpio_bit_toggle(GPIOE, GPIO_PIN_5);
+    gpio_bit_toggle(GPIOE, GPIO_PIN_5);    
-    
   }              
 }
 

APP/Service/service_error_count.c

@@ -17,6 +17,7 @@
 #include "service_error_count.h"
 
 #include "rotate_servo.h"
+#include "speed_to_servoMotor.h"
 
 char error_conut_state;
 #define COUNT_STATE   1
@@ -55,8 +56,10 @@ static void ptz_hori_error_count_task()
     case COUNT_STATE:
       g_ptz.hori_as5047d.as5047d_state = 0;//数据不可正常使用
       OSTimeDlyHMSM(0u, 0u, 0u, 10u);
+        // set_speed_to_servoMotor(horiMotorType, ERROR_COUNT_SPEED);
 //      data = ptz_hori_choice_microstep(ERROR_COUNT_SPEED);
 //      g_ptz.hori_tmc2160 = data;
+
       if(g_ptz.hori_ps_sw3_state == PS_COVER)//如果水平光电开关被挡住
       {
         ptz_hori_start(PTZ_HORI_DIR_LEFT, ERROR_COUNT_SPEED);

APP/Service/service_selfcheck.c

@@ -21,6 +21,7 @@
 #include "service_error_count.h"
 
 #include "rotate_servo.h"
+#include "speed_to_servoMotor.h"
 
 #ifdef  PTZ_BLDC_MOTOR
 /// 云台全范围自检
@@ -809,6 +810,7 @@ static unsigned char ptz_hori_complete_self_check_task()
   {
     //首先让云台水平向右转动
     case PTZ_HORI_SELF_CHECK_COMPLETE_STEP:
+        set_speed_to_servoMotor(horiMotorType, PTZ_HORI_SELF_CHECK_SPEED);
       if(g_ptz.hori_ps_sw3_state == PS_COVER)//如果水平光电开关被挡住
       {
         ptz_hori_start(PTZ_HORI_DIR_LEFT, PTZ_HORI_SELF_CHECK_SPEED);
@@ -885,7 +887,8 @@ static unsigned char ptz_hori_simplify_self_check_task()
   switch(g_ptz.hori_self_check)
   {
     case PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP:
-      if(g_ptz.hori_ps_sw3_state == PS_COVER)//如果水平光电开关被挡住
+        set_speed_to_servoMotor(horiMotorType, PTZ_HORI_SELF_CHECK_SPEED);
+    if(g_ptz.hori_ps_sw3_state == PS_COVER)//如果水平光电开关被挡住
       {
         ptz_hori_start(PTZ_HORI_DIR_LEFT, PTZ_HORI_SELF_CHECK_SPEED);
         g_ptz.hori_self_check = PTZ_HORI_SELF_CHECK_SIMPLIFY_STEP + 1;
@@ -953,6 +956,7 @@ static unsigned char ptz_vert_complete_self_check_task()
   {
     ///首先读取光电开关的状态，初步判断垂直状态
     case PTZ_VERT_SELF_CHECK_COMPLETE_STEP:
+        set_speed_to_servoMotor(vertMotorType, PTZ_VERT_SELF_CHECK_SPEED);
       g_ptz.vert_ps_sw1_down_fall = 0;
       g_ptz.vert_ps_sw1_up_rise = 0;
       g_ptz.vert_ps_sw2_up_fall = 0;
@@ -1075,6 +1079,7 @@ static unsigned char ptz_vert_simplify_self_check_task()
   switch(g_ptz.vert_self_check)
   {
     case PTZ_VERT_SELF_CHECK_SIMPLIFY_STEP:
+        set_speed_to_servoMotor(vertMotorType, PTZ_VERT_SELF_CHECK_SPEED);
       g_ptz.vert_ps_sw1_down_fall = 0;
       g_ptz.vert_ps_sw1_up_rise = 0;
       g_ptz.vert_ps_sw2_up_fall = 0;

BSP/Driver/drv_adc.c

@@ -233,7 +233,7 @@ float ptz_Voltage_collect_adc1_task()
     #endif
 
     /* 间接测量，11倍分压/放大 */
-    adc1_v[adc1_v_num] = (float)value_V / 4096.0 * 3.381 * 12.55;
+    adc1_v[adc1_v_num] = (float)value_V / 4096.0 * 3.3 * 11;
 
     adc1_v_num++;
     if(adc1_v_num >= LB_V_TIMES)
@@ -283,7 +283,7 @@ float ptz_Current_collect_adc1_task()
     #endif
 
     /* 间接测量 */
-    adc1_i[adc1_i_num] = ((float)value_I / 4096.0  * 3.381) * 10;
+    adc1_i[adc1_i_num] = ((float)value_I / 4096.0  * 3.3) * 10;
 
     adc1_i_num++;
     if(adc1_i_num >= LB_I_TIMES)

BSP/Driver/servoMotor/modbus_crc.c

@@ -1,66 +1,70 @@
 #include "stdint.h"
+#include "ptz_type_select.h"
 
-static const uint8_t aucCRCHi[] = {
+#ifdef PTZ_MEDIUM_WORM_SERVO_MOTOR_24V
-    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
-    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
-    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
-    0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
-    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
-    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
-    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
-    0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
-    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
-    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
-    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
-    0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
-    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
-    0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
-    0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
-    0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
-    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
-    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
-    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
-    0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
-    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
-    0x00, 0xC1, 0x81, 0x40
-};
 
-static const uint8_t aucCRCLo[] = {
+    static const uint8_t aucCRCHi[] = {
-    0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 0x07, 0xC7,
+        0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
-    0x05, 0xC5, 0xC4, 0x04, 0xCC, 0x0C, 0x0D, 0xCD, 0x0F, 0xCF, 0xCE, 0x0E,
+        0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
-    0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09, 0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9,
+        0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
-    0x1B, 0xDB, 0xDA, 0x1A, 0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC,
+        0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
-    0x14, 0xD4, 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3,
+        0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
-    0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, 0xF2, 0x32,
+        0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
-    0x36, 0xF6, 0xF7, 0x37, 0xF5, 0x35, 0x34, 0xF4, 0x3C, 0xFC, 0xFD, 0x3D,
+        0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
-    0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A, 0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38,
+        0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
-    0x28, 0xE8, 0xE9, 0x29, 0xEB, 0x2B, 0x2A, 0xEA, 0xEE, 0x2E, 0x2F, 0xEF,
+        0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
-    0x2D, 0xED, 0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26,
+        0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
-    0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60, 0x61, 0xA1,
+        0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
-    0x63, 0xA3, 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67, 0xA5, 0x65, 0x64, 0xA4,
+        0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
-    0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F, 0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB,
+        0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
-    0x69, 0xA9, 0xA8, 0x68, 0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA,
+        0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
-    0xBE, 0x7E, 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5,
+        0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
-    0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71, 0x70, 0xB0,
+        0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
-    0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92, 0x96, 0x56, 0x57, 0x97,
+        0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
-    0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C, 0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E,
+        0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
-    0x5A, 0x9A, 0x9B, 0x5B, 0x99, 0x59, 0x58, 0x98, 0x88, 0x48, 0x49, 0x89,
+        0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
-    0x4B, 0x8B, 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C,
+        0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
-    0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42, 0x43, 0x83,
+        0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
-    0x41, 0x81, 0x80, 0x40
+        0x00, 0xC1, 0x81, 0x40
-};
+    };
 
-uint16_t ModbusCRC16(uint8_t *pucFrame, uint16_t usLen)
+    static const uint8_t aucCRCLo[] = {
-{
+        0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 0x07, 0xC7,
-    uint8_t           ucCRCHi = 0xFF;
+        0x05, 0xC5, 0xC4, 0x04, 0xCC, 0x0C, 0x0D, 0xCD, 0x0F, 0xCF, 0xCE, 0x0E,
-    uint8_t           ucCRCLo = 0xFF;
+        0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09, 0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9,
-    int             iIndex;
+        0x1B, 0xDB, 0xDA, 0x1A, 0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC,
+        0x14, 0xD4, 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3,
+        0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, 0xF2, 0x32,
+        0x36, 0xF6, 0xF7, 0x37, 0xF5, 0x35, 0x34, 0xF4, 0x3C, 0xFC, 0xFD, 0x3D,
+        0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A, 0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38,
+        0x28, 0xE8, 0xE9, 0x29, 0xEB, 0x2B, 0x2A, 0xEA, 0xEE, 0x2E, 0x2F, 0xEF,
+        0x2D, 0xED, 0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26,
+        0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60, 0x61, 0xA1,
+        0x63, 0xA3, 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67, 0xA5, 0x65, 0x64, 0xA4,
+        0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F, 0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB,
+        0x69, 0xA9, 0xA8, 0x68, 0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA,
+        0xBE, 0x7E, 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5,
+        0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71, 0x70, 0xB0,
+        0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92, 0x96, 0x56, 0x57, 0x97,
+        0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C, 0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E,
+        0x5A, 0x9A, 0x9B, 0x5B, 0x99, 0x59, 0x58, 0x98, 0x88, 0x48, 0x49, 0x89,
+        0x4B, 0x8B, 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C,
+        0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42, 0x43, 0x83,
+        0x41, 0x81, 0x80, 0x40
+    };
 
-    while(usLen--)
+    uint16_t ModbusCRC16(uint8_t *pucFrame, uint16_t usLen)
     {
-        iIndex = ucCRCLo ^ *(pucFrame++);
+        uint8_t           ucCRCHi = 0xFF;
-        ucCRCLo = (uint8_t)(ucCRCHi ^ aucCRCHi[iIndex]);
+        uint8_t           ucCRCLo = 0xFF;
-        ucCRCHi = aucCRCLo[iIndex];
+        int             iIndex;
+
+        while(usLen--)
+        {
+            iIndex = ucCRCLo ^ *(pucFrame++);
+            ucCRCLo = (uint8_t)(ucCRCHi ^ aucCRCHi[iIndex]);
+            ucCRCHi = aucCRCLo[iIndex];
+        }
+        return (uint16_t)(ucCRCHi << 8 | ucCRCLo);
     }
-    return (uint16_t)(ucCRCHi << 8 | ucCRCLo);
+#endif
-}

BSP/Driver/servoMotor/modbus_crc.h

@@ -1,7 +1,12 @@
 #ifndef _MODBUS_CRC_H_
 #define _MODBUS_CRC_H_
 
+#include "ptz_type_select.h"
 
-uint16_t ModbusCRC16(uint8_t *pucFrame, uint16_t usLen);
+#ifdef PTZ_MEDIUM_WORM_SERVO_MOTOR_24V
+
+    uint16_t ModbusCRC16(uint8_t *pucFrame, uint16_t usLen);
+
+#endif
 
 #endif

BSP/Driver/servoMotor/motorCommu.c

@@ -1,6 +1,8 @@
 #include "motorCommu.h"
 #include "servoMotor_recv.h"
+#include "speed_to_servoMotor.h"
 
+#ifdef PTZ_MEDIUM_WORM_SERVO_MOTOR_24V
 /*
 ********************************************************************************************************
 *                             电机串口通讯驱动初始化相关结构体
@@ -99,11 +101,9 @@ static MotorCommuDmaHwInfo_t  g_MotorDmaBuff[] =
 ********************************************************************************************************
 */
 /* dma接收缓冲区 */
-static uint8_t g_horiDmaRxBuff1[DMA_BUFF_SIZE] = {0};//水平电机DMA接受缓存区1
+static uint8_t g_horiDmaRxBuff[DMA_BUFF_SIZE] = {0};//水平电机DMA接受缓存区
-static uint8_t g_horiDmaRxBuff2[DMA_BUFF_SIZE] = {0};//水平电机DMA接受缓存区2
 
-static uint8_t g_vertDmaRxBuff1[DMA_BUFF_SIZE] = {0};//垂直电机DMA接受缓存区1
+static uint8_t g_vertDmaRxBuff[DMA_BUFF_SIZE] = {0};//垂直电机DMA接受缓存区
-static uint8_t g_vertDmaRxBuff2[DMA_BUFF_SIZE] = {0};//垂直电机DMA接受缓存区2
 /* 处理串口通讯与数据缓冲的数据结构 */
 static CommuInfo_t g_horiCommuDeal; //水平电机 
 static CommuInfo_t g_vertCommuDeal; //垂直电机
@@ -111,22 +111,19 @@ static CommuInfo_t g_vertCommuDeal; //
 typedef struct
 {
 	CommuInfo_t*            pCommuInfo;			//串口通讯与数据缓冲相关的数据结构
-	uint8_t*                dmaRxBuff1;          //dma接受缓存区1指针
+	uint8_t*                dmaRxBuff;          //dma接受缓存区指针
-    uint8_t*                dmaRxBuff2;          //dma接受缓存区2指针
 }CommuHwInfo_t;//方便缓冲区初始化的结构体
 static CommuHwInfo_t g_commuInfoBuff[] = 
 {
     //水平电机
     {
         .pCommuInfo     =       &g_horiCommuDeal,
-        .dmaRxBuff1     =       g_horiDmaRxBuff1,
+        .dmaRxBuff      =       g_horiDmaRxBuff,
-        .dmaRxBuff2     =       g_horiDmaRxBuff2,
     },
     //垂直电机串口5
     {
         .pCommuInfo     =       &g_vertCommuDeal,
-        .dmaRxBuff1     =       g_vertDmaRxBuff1,
+        .dmaRxBuff      =       g_vertDmaRxBuff,
-        .dmaRxBuff2     =       g_vertDmaRxBuff2,
     },
 };
 
@@ -211,7 +208,14 @@ static void DmaCofig(void)
         dma_circulation_disable(g_MotorDmaBuff[i].dmaNo, g_MotorDmaBuff[i].dmaTxch);
         dma_channel_subperipheral_select(g_MotorDmaBuff[i].dmaNo, g_MotorDmaBuff[i].dmaTxch, g_MotorDmaBuff[i].dmaPeriph);
         //dma中断配置
-        nvic_irq_enable(g_MotorDmaBuff[i].dmaTxIrq, 4, 3);
+        if( i == H_MOTOR )
+        {
+          nvic_irq_enable(g_MotorDmaBuff[H_MOTOR].dmaTxIrq, 1, 2);
+        }
+        else
+        {
+          nvic_irq_enable(g_MotorDmaBuff[V_MOTOR].dmaTxIrq, 1, 1);
+        }
         dma_interrupt_enable(g_MotorDmaBuff[i].dmaNo, g_MotorDmaBuff[i].dmaTxch, DMA_CHXCTL_FTFIE);//dma发送完成中断
 
         
@@ -219,7 +223,7 @@ static void DmaCofig(void)
         //dma配置
         dma_deinit(g_MotorDmaBuff[i].dmaNo, g_MotorDmaBuff[i].dmaRxch);
         dmaStruct.direction = DMA_PERIPH_TO_MEMORY;
-        dmaStruct.memory0_addr = (uint32_t)(g_commuInfoBuff[i].pCommuInfo->pDmaRsvBuff1);  
+        dmaStruct.memory0_addr = (uint32_t)(g_commuInfoBuff[i].pCommuInfo->pDmaRsvBuff);  
         dmaStruct.memory_inc = DMA_MEMORY_INCREASE_ENABLE;
         dmaStruct.number = DMA_BUFF_SIZE;    
         dmaStruct.periph_addr = g_MotorDmaBuff[i].periphAddr;
@@ -229,10 +233,17 @@ static void DmaCofig(void)
         dma_memory_width_config(g_MotorDmaBuff[i].dmaNo, g_MotorDmaBuff[i].dmaRxch, DMA_MEMORY_WIDTH_8BIT);
         dma_periph_width_config(g_MotorDmaBuff[i].dmaNo, g_MotorDmaBuff[i].dmaRxch, DMA_PERIPH_WIDTH_8BIT);
         dma_single_data_mode_init(g_MotorDmaBuff[i].dmaNo, g_MotorDmaBuff[i].dmaRxch, &dmaStruct);
-        dma_circulation_disable(g_MotorDmaBuff[i].dmaNo, g_MotorDmaBuff[i].dmaRxch);//非循环模式
+        dma_circulation_enable(g_MotorDmaBuff[i].dmaNo, g_MotorDmaBuff[i].dmaRxch);//循环模式
         dma_channel_subperipheral_select(g_MotorDmaBuff[i].dmaNo, g_MotorDmaBuff[i].dmaRxch, g_MotorDmaBuff[i].dmaPeriph);
         //中断配置
-        nvic_irq_enable(g_MotorDmaBuff[i].dmaRxIrq, 4, 2);
+        if ( i == H_MOTOR )
+        {
+          nvic_irq_enable(g_MotorDmaBuff[H_MOTOR].dmaRxIrq, 0, 2);
+        }
+        else
+        {
+          nvic_irq_enable(g_MotorDmaBuff[V_MOTOR].dmaRxIrq, 0, 1);
+        }
         dma_interrupt_enable(g_MotorDmaBuff[i].dmaNo, g_MotorDmaBuff[i].dmaRxch, DMA_CHXCTL_FTFIE);
         dma_channel_enable(g_MotorDmaBuff[i].dmaNo, g_MotorDmaBuff[i].dmaRxch); 
     }
@@ -257,9 +268,7 @@ static void CommuStructInit()
         CommuInfo_t *pCommuDeal = g_commuInfoBuff[i].pCommuInfo;
         pCommuDeal->dmaTranFlag = DMA_TRANS_IDLE;
         pCommuDeal->dmaSize = DMA_BUFF_SIZE;
-        pCommuDeal->pDmaRsvBuff1 = g_commuInfoBuff[i].dmaRxBuff1;
+        pCommuDeal->pDmaRsvBuff = g_commuInfoBuff[i].dmaRxBuff;
-        pCommuDeal->pDmaRsvBuff2 = g_commuInfoBuff[i].dmaRxBuff2;
-        pCommuDeal->pDmaRsvBuffSelect = DMA_RSVBUFF_SELECT1;//默认使用缓冲区1
     }
 }
 
@@ -300,7 +309,6 @@ static void CommuDmaTra(uint8_t devNo, uint8_t *buffer,uint16_t len)
 void DMA0_Channel3_IRQHandler(void)
 {
 /* 
-* 配合串口的发送。功能是继续发送缓冲区未发送的数据。
 * 发送完成配置的CNT次数后，会进入此中断函数
 */
     if(dma_interrupt_flag_get(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaTxch, DMA_INT_FLAG_FTF)) 
@@ -321,81 +329,37 @@ void DMA0_Channel3_IRQHandler(void)
 * @return 
 ***********************************************************
 */
+static uint16_t g_horiLastPos;
+static uint16_t g_horiNowPos;
 void USART2_IRQHandler(void)
 { 				
     /* 串口的接收空闲中断方式进行了数据缓存。*/
-    dma_single_data_parameter_struct dmaStruct;
     if(RESET != usart_interrupt_flag_get(g_motorCommuBuff[H_MOTOR].uartNo, USART_INT_FLAG_IDLE))
     {
         /* clear IDLE flag */
         usart_interrupt_flag_clear(g_motorCommuBuff[H_MOTOR].uartNo, USART_INT_FLAG_IDLE);  	//第一步，读取stat0寄存器，清除IDLE标志位
-        usart_data_receive(g_motorCommuBuff[H_MOTOR].uartNo);  								//第二步，读取数据寄存器，清除IDLE标志位
+        usart_data_receive(g_motorCommuBuff[H_MOTOR].uartNo);  								    //第二步，读取数据寄存器，清除IDLE标志位
-
+   
-        //释放信号量，通知接收到一包数据，任务可以处理了
+        g_horiLastPos = g_horiNowPos;
+        g_horiNowPos = DMA_BUFF_SIZE - dma_transfer_number_get(g_MotorDmaBuff[H_MOTOR].dmaNo, 
+                                                               g_MotorDmaBuff[H_MOTOR].dmaRxch);//函数获取的是还有多少个没传输，而不是已经传输了多少
+         //释放信号量，通知能处理数据
         OSSemPost(g_horiMotorMutex);
-        CommuInfo_t *pCommuDeal = g_commuInfoBuff[H_MOTOR].pCommuInfo;
-        /* 切换使用接收缓冲区，这样上层的解析永远从第一个字节起*/
-        if ( pCommuDeal->pDmaRsvBuffSelect == DMA_RSVBUFF_SELECT1 )
-        {
-            pCommuDeal->pDmaRsvBuffSelect = DMA_RSVBUFF_SELECT2;
-            dma_channel_disable(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch);//切换缓冲区，配置缓冲区长度需要先禁用DMA
-            dmaStruct.memory0_addr = (uint32_t)(pCommuDeal->pDmaRsvBuff2); 
-            dma_single_data_mode_init(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch, &dmaStruct);
-            dma_channel_enable(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch);
-        }
-        else
-        {
-            pCommuDeal->pDmaRsvBuffSelect = DMA_RSVBUFF_SELECT1;
-            dma_channel_disable(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch);//切换缓冲区，配置缓冲区长度需要先禁用DMA
-            dmaStruct.memory0_addr = (uint32_t)(pCommuDeal->pDmaRsvBuff1); 
-            dma_single_data_mode_init(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch, &dmaStruct);
-            dma_channel_enable(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch);   
-        }
-
-
-        /*计算在DMA缓冲区需要获取的数据长度*/
-        // DmaIdleNum = dma_transfer_number_get(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch);//获取的是还有多少个没传输，而不是已经传输了多少
     } 
 }
 
 
 void DMA0_Channel1_IRQHandler(void)
 {  	 
-    dma_single_data_parameter_struct dmaStruct;
 /* 
-* 配合串口1的接收空闲中断。功能是复位DMA的偏移量
+
-* 1、为了串口1的空闲中断在处理数据时防止越界，将 pUartAttr->DamOffset置为0; 
+* 注意：DMA为正常模式，那么当完成一次拷贝后,DMA会自动disable掉。
-* 2、DMA为循环方式进行数据搬运的，当搬运完配置的Cnt后，会进入此中断处理函数，
-* 3、当有数据过来时，数据将会拷贝到缓冲区的起始位置;
-* 4、注意：如果DMA为正常模式，那么当完成一次拷贝后,DMA会自动disable掉。
 */
     if(dma_interrupt_flag_get(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch, DMA_INT_FLAG_FTF)) 
     {
         dma_interrupt_flag_clear(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch, DMA_INT_FLAG_FTF); 
 
-        CommuInfo_t *pCommuDeal = g_commuInfoBuff[H_MOTOR].pCommuInfo; 
+        
-        //释放信号量，通知接收到一包数据，任务可以处理了
-        OSSemPost(g_horiMotorMutex);
-        /* 切换使用接收缓冲区，这样上层的解析永远从第一个字节起*/
-        if ( pCommuDeal->pDmaRsvBuffSelect == DMA_RSVBUFF_SELECT1 )
-        {
-            pCommuDeal->pDmaRsvBuffSelect = DMA_RSVBUFF_SELECT2;
-            dma_channel_disable(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch);//切换缓冲区，配置缓冲区长度需要先禁用DMA
-            dmaStruct.memory0_addr = (uint32_t)(pCommuDeal->pDmaRsvBuff2); 
-            dmaStruct.number = DMA_BUFF_SIZE;
-            dma_single_data_mode_init(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch, &dmaStruct);
-            dma_channel_enable(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch);
-        }
-        else
-        {
-            pCommuDeal->pDmaRsvBuffSelect = DMA_RSVBUFF_SELECT1;
-            dma_channel_disable(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch);//切换缓冲区，配置缓冲区长度需要先禁用DMA
-            dmaStruct.memory0_addr = (uint32_t)(pCommuDeal->pDmaRsvBuff1);  
-            dmaStruct.number = DMA_BUFF_SIZE;
-            dma_single_data_mode_init(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch, &dmaStruct);
-            dma_channel_enable(g_MotorDmaBuff[H_MOTOR].dmaNo, g_MotorDmaBuff[H_MOTOR].dmaRxch);   
-        }
- 
     }
 }
 
@@ -425,7 +389,6 @@ void DMA1_Channel7_IRQHandler(void)
         while(usart_flag_get(g_motorCommuBuff[V_MOTOR].uartNo, USART_FLAG_TC) == RESET); // 等待串口发送完成
         V_COMMU_RS485_RX; //485切换为接收
         pCommuDeal->dmaTranFlag = DMA_TRANS_IDLE;
-
     }
 }
 
@@ -436,9 +399,10 @@ void DMA1_Channel7_IRQHandler(void)
 * @return 
 ***********************************************************
 */
+static uint16_t g_vertLastPos;
+static uint16_t g_vertNowPos;
 void USART5_IRQHandler(void)
 { 			
-    dma_single_data_parameter_struct dmaStruct;	
     /* 串口的接收空闲中断方式进行了数据缓存。*/
     if(RESET != usart_interrupt_flag_get(g_motorCommuBuff[V_MOTOR].uartNo, USART_INT_FLAG_IDLE))
     {
@@ -446,69 +410,22 @@ void USART5_IRQHandler(void)
         usart_interrupt_flag_clear(g_motorCommuBuff[V_MOTOR].uartNo, USART_INT_FLAG_IDLE);  	//第一步，读取stat0寄存器，清除IDLE标志位
         usart_data_receive(g_motorCommuBuff[V_MOTOR].uartNo);  								//第二步，读取数据寄存器，清除IDLE标志位
 
-        //释放信号量，通知接收到一包数据，任务可以处理了
+        g_vertLastPos = g_vertNowPos;
-        OSSemPost(g_horiMotorMutex);
+        g_vertNowPos = DMA_BUFF_SIZE - dma_transfer_number_get(g_MotorDmaBuff[V_MOTOR].dmaNo, 
-        CommuInfo_t *pCommuDeal = g_commuInfoBuff[V_MOTOR].pCommuInfo;
+                                                               g_MotorDmaBuff[V_MOTOR].dmaRxch);
-        /* 切换使用接收缓冲区，这样上层的解析永远从第一个字节起*/
+        //释放信号量，通知接收任务处理
-        if ( pCommuDeal->pDmaRsvBuffSelect == DMA_RSVBUFF_SELECT1 )
+        OSSemPost(g_vertMotorMutex);
-        {
-            pCommuDeal->pDmaRsvBuffSelect = DMA_RSVBUFF_SELECT2;
-            dma_channel_disable(g_MotorDmaBuff[V_MOTOR].dmaNo, g_MotorDmaBuff[V_MOTOR].dmaRxch);//切换缓冲区，配置缓冲区长度需要先禁用DMA
-            dmaStruct.memory0_addr = (uint32_t)(pCommuDeal->pDmaRsvBuff2);  
-            dma_single_data_mode_init(g_MotorDmaBuff[V_MOTOR].dmaNo, g_MotorDmaBuff[V_MOTOR].dmaRxch, &dmaStruct);
-            dma_channel_enable(g_MotorDmaBuff[V_MOTOR].dmaNo, g_MotorDmaBuff[V_MOTOR].dmaRxch);
-        }
-        else
-        {
-            pCommuDeal->pDmaRsvBuffSelect = DMA_RSVBUFF_SELECT1;
-            dma_channel_disable(g_MotorDmaBuff[V_MOTOR].dmaNo, g_MotorDmaBuff[V_MOTOR].dmaRxch);//切换缓冲区，配置缓冲区长度需要先禁用DMA
-            dmaStruct.memory0_addr = (uint32_t)(pCommuDeal->pDmaRsvBuff1);  
-            dma_single_data_mode_init(g_MotorDmaBuff[V_MOTOR].dmaNo, g_MotorDmaBuff[V_MOTOR].dmaRxch, &dmaStruct);
-            dma_channel_enable(g_MotorDmaBuff[V_MOTOR].dmaNo, g_MotorDmaBuff[V_MOTOR].dmaRxch);   
-        }
     } 
 }
 
-
-// uint8_t rx_OK = 0;
 void DMA1_Channel1_IRQHandler(void)
 {  	 
-    dma_single_data_parameter_struct dmaStruct;
 /* 
-* 配合串口1的接收空闲中断。功能是复位DMA的偏移量
+* 注意：如果DMA为正常模式，那么当完成一次拷贝后,DMA会自动disable掉。
-* 1、为了串口1的空闲中断在处理数据时防止越界，将 pUartAttr->DamOffset置为0; 
-* 2、DMA为循环方式进行数据搬运的，当搬运完配置的Cnt后，会进入此中断处理函数，
-* 3、当有数据过来时，数据将会拷贝到缓冲区的起始位置;
-* 4、注意：如果DMA为正常模式，那么当完成一次拷贝后,DMA会自动disable掉。
 */
     if(dma_interrupt_flag_get(g_MotorDmaBuff[V_MOTOR].dmaNo, g_MotorDmaBuff[V_MOTOR].dmaRxch, DMA_INT_FLAG_FTF)) 
     {
         dma_interrupt_flag_clear(g_MotorDmaBuff[V_MOTOR].dmaNo, g_MotorDmaBuff[V_MOTOR].dmaRxch, DMA_INT_FLAG_FTF); 
-        
-        CommuInfo_t *pCommuDeal = g_commuInfoBuff[V_MOTOR].pCommuInfo; 
-        //释放信号量，通知接收到一包数据，任务可以处理了
-        OSSemPost(g_horiMotorMutex);
-        /* 切换使用接收缓冲区，这样上层的解析永远从第一个字节起*/
-        if ( pCommuDeal->pDmaRsvBuffSelect == DMA_RSVBUFF_SELECT1 )
-        {
-            pCommuDeal->pDmaRsvBuffSelect = DMA_RSVBUFF_SELECT2;
-            dma_channel_disable(g_MotorDmaBuff[V_MOTOR].dmaNo, g_MotorDmaBuff[V_MOTOR].dmaRxch);//切换缓冲区，配置缓冲区长度需要先禁用DMA
-            dmaStruct.memory0_addr = (uint32_t)(pCommuDeal->pDmaRsvBuff2); 
-            dmaStruct.number = DMA_BUFF_SIZE;
-            dma_single_data_mode_init(g_MotorDmaBuff[V_MOTOR].dmaNo, g_MotorDmaBuff[V_MOTOR].dmaRxch, &dmaStruct);
-            dma_channel_enable(g_MotorDmaBuff[V_MOTOR].dmaNo, g_MotorDmaBuff[V_MOTOR].dmaRxch);
-        }
-        else
-        {
-            pCommuDeal->pDmaRsvBuffSelect = DMA_RSVBUFF_SELECT1;
-            dma_channel_disable(g_MotorDmaBuff[V_MOTOR].dmaNo, g_MotorDmaBuff[V_MOTOR].dmaRxch);//切换缓冲区，配置缓冲区长度需要先禁用DMA
-            dmaStruct.memory0_addr = (uint32_t)(pCommuDeal->pDmaRsvBuff1); 
-            dmaStruct.number = DMA_BUFF_SIZE;
-            dma_single_data_mode_init(g_MotorDmaBuff[V_MOTOR].dmaNo, g_MotorDmaBuff[V_MOTOR].dmaRxch, &dmaStruct);
-            dma_channel_enable(g_MotorDmaBuff[V_MOTOR].dmaNo, g_MotorDmaBuff[V_MOTOR].dmaRxch);   
-        }
-
-
     }
 }
 
@@ -539,6 +456,13 @@ void CommuDrvInit(void)
 */
 bool CommuTransData(uint8_t motorNo, uint8_t* buffer, int32_t len)
 {
+   
+     /*简易判断*/
+    if( buffer == NULL ||  len == 0 )
+    {
+      return false;
+    }
+    /*485切换为发送*/
     if( motorNo == H_MOTOR )
     {
         H_COMMU_RS485_TX;
@@ -556,27 +480,53 @@ bool CommuTransData(uint8_t motorNo, uint8_t* buffer, int32_t len)
         pCommuDeal->dmaTranFlag = DMA_TRANS_BUSY;
         CommuDmaTra(motorNo, buffer, len);
         return true;
-    } 
+    }
     return false;
 }
 
 /**
 * @brief    从接收循环缓冲区中读取指定长度的数据到用户数组
 * @param    motorNo：电机号，H_MOTOR：水平电机，V_MOTOR：垂直电机
-* @param    userBuff：从接收循环缓冲区中接收数据的数组 
+* @param    userBuff：从接收缓冲区中接收数据的数组 
 * @param    len：接收数据的长度
 * @return   none
 */
-void CommuRsvData(uint8_t motorNo, uint8_t* userBuff, uint32_t len)
+bool CommuRsvData(uint8_t motorNo, uint8_t* userBuff, uint32_t len)
 {
-
+    
     CommuInfo_t *pCommuDeal = g_commuInfoBuff[motorNo].pCommuInfo;
-    if ( pCommuDeal->pDmaRsvBuffSelect == DMA_RSVBUFF_SELECT1 )
+    /*简易判断*/
+    if( userBuff == NULL ||  len == 0 )
     {
-        memcpy(userBuff, pCommuDeal->pDmaRsvBuff2, len);
+      return false;
+    }
+    uint16_t horiPosTemp = g_horiLastPos;
+    uint16_t vertPosTemp = g_vertLastPos;
+    /*接收DMA缓冲区数据*/
+    if ( motorNo == H_MOTOR )
+    {
+        if ( ( (horiPosTemp + len) % DMA_BUFF_SIZE ) != g_horiNowPos )//假如是写寄存器，电机返回消息正常是8字节，但如果是写错误，则返回非8字节错误码
+        {
+            return false;
+        }
+        for(uint8_t i = 0;  i < len; i++)
+        {
+           *(userBuff+i) = pCommuDeal->pDmaRsvBuff[horiPosTemp];
+           horiPosTemp = (horiPosTemp + 1) % DMA_BUFF_SIZE;
+        }
     }
     else
     {
-        memcpy(userBuff, pCommuDeal->pDmaRsvBuff1, len);
+        if ( ( (vertPosTemp + len) % DMA_BUFF_SIZE ) != g_vertNowPos )//假如是写寄存器，电机返回消息正常是8字节，但如果是写错误，则返回非8字节错误码
-    }
+        {
+            return false;
+        }
+        for(uint8_t i = 0;  i < len; i++)
+        {
+           *(userBuff+i) = pCommuDeal->pDmaRsvBuff[vertPosTemp];
+           vertPosTemp = (vertPosTemp + 1) % DMA_BUFF_SIZE;
+        } 
+    } 
+    return true;
 }
+#endif

BSP/Driver/servoMotor/motorCommu.h

@@ -4,68 +4,70 @@
 #include "gd32f4xx.h"
 #include "stdbool.h"
 #include "string.h"
-/*
+#include "ptz_type_select.h"
-********************************************************************************************************
-*                             dma缓冲区相关
-********************************************************************************************************
-*/
-typedef struct 
-{  
-	 int16_t dmaTranFlag;              	/*dma发送是否在工作的标志位*/      
-	 int32_t dmaSize;                	/*DMA接收缓冲区的大小*/
-	 uint8_t *pDmaRsvBuff1;              /*指向接收DMA缓冲区1的首地址*/
-	 uint8_t *pDmaRsvBuff2;              /*指向接收DMA缓冲区2的首地址*/
-	 uint8_t pDmaRsvBuffSelect;		/*表示当前正在使用哪个接收缓冲区*/
-}CommuInfo_t;
 
-#define DMA_TRANS_IDLE              0//dma当前未在发送数据
+    #ifdef PTZ_MEDIUM_WORM_SERVO_MOTOR_24V
-#define DMA_TRANS_BUSY              1//dma当前正在发送数据
-#define DMA_BUFF_SIZE               64//dma缓冲区大小
-#define DMA_RSVBUFF_SELECT1         (uint8_t)(0)//当前使用dma接收缓冲区1
-#define DMA_RSVBUFF_SELECT2         (uint8_t)(1)//当前使用dma接收缓冲区2
 
-//extern CommuInfo_t g_commuDeal;//来自motorCommu.c
+    /*
+    ********************************************************************************************************
+    *                             dma缓冲区相关
+    ********************************************************************************************************
+    */
+    typedef struct 
+    {  
+         int16_t dmaTranFlag;              	/*dma发送是否在工作的标志位*/      
+         int32_t dmaSize;                	/*DMA接收缓冲区的大小*/
+         uint8_t *pDmaRsvBuff;              /*指向接收DMA缓冲区的首地址*/
+    }CommuInfo_t;
 
-/*
+    #define DMA_TRANS_IDLE              0//dma当前未在发送数据
-********************************************************************************************************
+    #define DMA_TRANS_BUSY              1//dma当前正在发送数据
-*                             串口设备驱动初始化相关
+    #define DMA_BUFF_SIZE               64//dma缓冲区大小
-********************************************************************************************************
-*/
-#define H_MOTOR         0//数组g_motorCommuInitBuff[MOTOR_NUM]位号
-#define V_MOTOR         1
 
-/*-------------485接收发送宏开关----------------------*/
+    //extern CommuInfo_t g_commuDeal;//来自motorCommu.c
-#define H_COMMU_RS485_TX              gpio_bit_set(GPIOD, GPIO_PIN_10)//水平电机485发送
-#define H_COMMU_RS485_RX              gpio_bit_reset(GPIOD, GPIO_PIN_10)//水平电机485接收
-#define V_COMMU_RS485_TX              gpio_bit_set(GPIOC, GPIO_PIN_8)//垂直电机485发送
-#define V_COMMU_RS485_RX              gpio_bit_reset(GPIOC, GPIO_PIN_8)//垂直电机485接收
 
-/**
+    /*
-* @brief 串口设备通讯驱动初始化
+    ********************************************************************************************************
-* @param
+    *                             串口设备驱动初始化相关
-* @return 
+    ********************************************************************************************************
-*/
+    */
-void CommuDrvInit(void);
+    #define H_MOTOR         0//数组g_motorCommuInitBuff[MOTOR_NUM]位号
+    #define V_MOTOR         1
 
-/**
+    /*-------------485接收发送宏开关----------------------*/
-* @brief    数据的发送
+    #define H_COMMU_RS485_TX              gpio_bit_set(GPIOD, GPIO_PIN_10)//水平电机485发送
-* @param    motorNo：电机号，H_MOTOR：水平电机，V_MOTOR：垂直电机
+    #define H_COMMU_RS485_RX              gpio_bit_reset(GPIOD, GPIO_PIN_10)//水平电机485接收
-* @param    buffer：需要发送的数据
+    #define V_COMMU_RS485_TX              gpio_bit_set(GPIOC, GPIO_PIN_8)//垂直电机485发送
-* @param    len：发送数据的长度
+    #define V_COMMU_RS485_RX              gpio_bit_reset(GPIOC, GPIO_PIN_8)//垂直电机485接收
-* @return   ture:DMA空闲当前数据可以发送；false：DMA正在发送数据，当前数据不能发送
-*/
-bool CommuTransData(uint8_t motorNo, uint8_t* buffer, int32_t len);
 
-/**
+    /**
-* @brief    从接收循环缓冲区中读取指定长度的数据到用户数组
+    * @brief 串口设备通讯驱动初始化
-* @param    motorNo：电机号，H_MOTOR：水平电机，V_MOTOR：垂直电机
+    * @param
-* @param    userBuff：从接收循环缓冲区中接收数据的数组 
+    * @return 
-* @param    len：接收数据的长度
+    */
-* @return   none
+    void CommuDrvInit(void);
-*/
+
-void CommuRsvData(uint8_t motorNo, uint8_t* userBuff, uint32_t len);
+    /**
+    * @brief    数据的发送
+    * @param    motorNo：电机号，H_MOTOR：水平电机，V_MOTOR：垂直电机
+    * @param    buffer：需要发送的数据
+    * @param    len：发送数据的长度
+    * @return   ture:DMA空闲当前数据可以发送；false：DMA正在发送数据，当前数据不能发送
+    */
+    bool CommuTransData(uint8_t motorNo, uint8_t* buffer, int32_t len);
+
+    /**
+    * @brief    从接收循环缓冲区中读取指定长度的数据到用户数组
+    * @param    motorNo：电机号，H_MOTOR：水平电机，V_MOTOR：垂直电机
+    * @param    userBuff：从接收循环缓冲区中接收数据的数组 
+    * @param    len：接收数据的长度
+    * @return   none
+    */
+    bool CommuRsvData(uint8_t motorNo, uint8_t* userBuff, uint32_t len);
 
 
-///*用于结构体数组赋值，方便外部使用此结构体数组*/
+    ///*用于结构体数组赋值，方便外部使用此结构体数组*/
-//CommuHwInfo_t GetMotorCommuBuffStr(uint8_t motorNo);
+    //CommuHwInfo_t GetMotorCommuBuffStr(uint8_t motorNo);
+    #endif
+    
 #endif

BSP/Driver/servoMotor/servoMotor.c

@@ -1,115 +1,111 @@
 #include "servoMotor.h"
 #include <ucos_ii.h>
-/*
-水平垂直电机使能开关引脚配置
-*/
-static void MotorSwitchGpioCofig(void)
-{
-    /*GPIO时钟初始化*/
-    rcu_periph_clock_enable(RCU_GPIOE);
-    /*水平电机打开引脚*/
-    gpio_mode_set(GPIOE, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE,  GPIO_PIN_0);
-    gpio_output_options_set(GPIOE, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ,  GPIO_PIN_0);
-    /*垂直电机打开引脚*/
-    gpio_mode_set(GPIOE, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE,  GPIO_PIN_1);
-    gpio_output_options_set(GPIOE, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ,  GPIO_PIN_1);
-    
-}
 
-static uint8_t g_writeOneRegBuff[WRITE_ONE_REG_BUFFNUM];//由于写寄存器，电机返回的数据和写入的数据完全一致，故设此数组
+#ifdef PTZ_MEDIUM_WORM_SERVO_MOTOR_24V
-
+    /*
-
+    水平垂直电机使能开关引脚配置
-/*
+    */
-从机地址    功能码    寄存器地址高   寄存器地址低  数据高位    数据低位    crc校验高位     crc校验低位
+    static void MotorSwitchGpioCofig(void)
-    01H     06H         02H             00H         00H         01H         49H             B2H
-*/
-/**
-* @brief    伺服电机速度模式写单个寄存器
-* @param    motorNo：写垂直电机还是水平电机
-* @param    regAddr：要写的寄存器
-* @param    data：要向寄存器写入的数据
-* @return   false:写失败，当前DMA正在发送数据
-*/
-bool WriteMotorOneReg(uint8_t motorNo, uint16_t regAddr, uint16_t data)
-{
-      uint8_t frameBuff[WRITE_ONE_REG_BUFFNUM] = {0};
-      uint16_t crc;
-      if ( motorNo == H_MOTOR )
-      {
-        frameBuff[0] = H_MOTOR_ADDR;//由于采用一主一从模式，所以水平电机垂直电机从机地址都是0x01，云台后期也不会扩展
-      }
-      else
-      {
-        frameBuff[0] = V_MOTOR_ADDR;
-      }
-      frameBuff[1] = WRITE_ONE_REG;
-      frameBuff[2] = regAddr >> WRITE_ONE_REG_BUFFNUM;
-      frameBuff[3] = regAddr & 0xff;
-      frameBuff[4] = data >> WRITE_ONE_REG_BUFFNUM;
-      frameBuff[5] = data & 0xff;
-      crc = ModbusCRC16(frameBuff, WRITE_ONE_REG_BUFFNUM - 2);
-      frameBuff[6] = crc & 0xff;
-      frameBuff[7] = crc >> WRITE_ONE_REG_BUFFNUM;
-  
-      if ( CommuTransData(motorNo, frameBuff, WRITE_ONE_REG_BUFFNUM) == false)
-      {
-         return false;
-      }
-      memcpy(g_writeOneRegBuff, frameBuff, WRITE_ONE_REG_BUFFNUM);
-      return true;
-}
-/**
-* @brief    写单个寄存器成功后，电机会返回一帧与写格式完全一致的数据
-* @param    motorNo：写垂直电机还是水平电机
-* @param    userBuff：用来接收返回的数据的数组
-* @param    len：接收的数据长度，对于此函数必须为WRITE_ONE_REG_BUFFNUM
-* @return   false:错误，读出的内容与写入的内容不一致
-*/
-bool MotorReplyForWrite(uint8_t motorNo, uint8_t* userBuff, uint8_t len)
-{
-    CommuRsvData(motorNo, userBuff, len);
-    for( uint8_t i = 0; i < len; i++ )
     {
-        if ( userBuff[i] != g_writeOneRegBuff[i] )
+        /*GPIO时钟初始化*/
-        {
+        rcu_periph_clock_enable(RCU_GPIOE);
-            memset(g_writeOneRegBuff, 0x00, len);
+        /*水平电机打开引脚*/
-            return false;
+        gpio_mode_set(GPIOE, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE,  GPIO_PIN_0);
-        }
+        gpio_output_options_set(GPIOE, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ,  GPIO_PIN_0);
+        /*垂直电机打开引脚*/
+        gpio_mode_set(GPIOE, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE,  GPIO_PIN_1);
+        gpio_output_options_set(GPIOE, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ,  GPIO_PIN_1);
+        
     }
-    memset(g_writeOneRegBuff, 0x00, len);
-    return true;
-}
 
 
-/**
+    /*
-* @brief 伺服电机驱动初始化
+    从机地址    功能码    寄存器地址高   寄存器地址低  数据高位    数据低位    crc校验高位     crc校验低位
-* @param
+        01H     06H         02H             00H         00H         01H         49H             B2H
-* @return 
+    */
-*/
+    /**
-void servoMotorInit(void)
+    * @brief    伺服电机速度模式写单个寄存器
-{
+    * @param    motorNo：写垂直电机还是水平电机
-    MotorSwitchGpioCofig();//两个电机电源的开关PE0,1引脚，如果其它地方实现了，可以不需要
+    * @param    regAddr：要写的寄存器
-    H_MOTOR_OPEN;
+    * @param    data：要向寄存器写入的数据
-    OSTimeDlyHMSM(0u, 0u, 0u, 1000u);
+    * @return   返回数组地址
-    V_MOTOR_OPEN;
+    */
-    
+    static uint8_t g_HwriteOneRegBuff[WRITE_ONE_REG_FRAME_NUM];
-    CommuDrvInit();//伺服电机RS485通讯初始化
+    static uint8_t g_VwriteOneRegBuff[WRITE_ONE_REG_FRAME_NUM];
-    OSTimeDlyHMSM(0u, 0u, 0u, 500u);//等待硬件初始化成功
+    uint8_t* WriteMotorOneReg(uint8_t motorNo, uint16_t regAddr, int16_t data)
-    WriteMotorOneReg(H_MOTOR, H02_CONTR_MODE_SELEC, 0);//H0200,选择速度模式
+    {
-    OSTimeDlyHMSM(0u, 0u, 0u, 5u);
+        static uint8_t g_writeOneRegBuff[WRITE_ONE_REG_FRAME_NUM];
-    
+        uint16_t crc;
-    WriteMotorOneReg(H_MOTOR, H04_DO1_FUNC_SELEC, 19);
+        g_writeOneRegBuff[0] = 0x01;
-    OSTimeDlyHMSM(0u, 0u, 0u, 5u);
+        g_writeOneRegBuff[1] = WRITE_ONE_REG;
-    
+        g_writeOneRegBuff[2] = regAddr >> 8;
-    WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, 100);//速度设置为100rpm
+        g_writeOneRegBuff[3] = (uint8_t)(regAddr & 0xff);
-    OSTimeDlyHMSM(0u, 0u, 0u, 5u);
+        g_writeOneRegBuff[4] = data >> 8;
-    
+        g_writeOneRegBuff[5] = data & 0xff;
-    WriteMotorOneReg(H_MOTOR, H06_SPEED_UP_SLOPE_VALUE, 3000);//加速度3000
+        crc = ModbusCRC16(g_writeOneRegBuff, WRITE_ONE_REG_FRAME_NUM - 2);
-    OSTimeDlyHMSM(0u, 0u, 0u, 5u);
+        g_writeOneRegBuff[6] = (uint8_t)(crc & 0xff);
-    
+        g_writeOneRegBuff[7] = crc >> 8;
-    WriteMotorOneReg(H_MOTOR, H06_SPEED_DOWN_SLOPE_VALUE, 2000);//减速度2000
+        if ( motorNo == H_MOTOR )
-    OSTimeDlyHMSM(0u, 0u, 0u, 5u);
+            {
-    
+                memcpy(g_HwriteOneRegBuff, g_writeOneRegBuff, WRITE_ONE_REG_FRAME_NUM);
-    WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 1);//只启动水平电机  
+                return g_HwriteOneRegBuff;
-    OSTimeDlyHMSM(0u, 0u, 0u, 5u);
+            }
-}
+
+        memcpy(g_VwriteOneRegBuff, g_writeOneRegBuff, WRITE_ONE_REG_FRAME_NUM);
+        return g_VwriteOneRegBuff;
+    }
+
+
+
+    /*
+    从机地址    功能码    寄存器地址高   寄存器地址低  reg数量高位    数量低位    crc校验高位     crc校验低位
+        01H     03H         0BH             00H         00H         01H         86H             2EH
+    */
+    /**
+    * @brief    伺服电机速度模式读单个寄存器
+    * @param    motorNo：读垂直电机还是水平电机
+    * @param    regAddr：要读的寄存器
+    * @return   返回数组地址
+    */
+    static uint8_t g_HreadOneRegBuff[READ_ONE_REG_FRAME_NUM];
+    static uint8_t g_VreadOneRegBuff[READ_ONE_REG_FRAME_NUM];
+    uint8_t* ReadMotorOneReg(uint8_t motorNo, uint16_t regAddr)
+    {
+          static uint8_t g_readOneRegBuff[READ_ONE_REG_FRAME_NUM];
+          uint16_t crc;
+          g_readOneRegBuff[0] = 0x01;
+          g_readOneRegBuff[1] = READ_ONE_REG;
+          g_readOneRegBuff[2] = regAddr >> 8;
+          g_readOneRegBuff[3] = regAddr & 0xff;
+          g_readOneRegBuff[4] = 0x00;
+          g_readOneRegBuff[5] = 0x01;
+          crc = ModbusCRC16(g_readOneRegBuff, READ_ONE_REG_FRAME_NUM - 2);
+          g_readOneRegBuff[6] = crc & 0xff;
+          g_readOneRegBuff[7] = crc >> 8;
+        if ( motorNo == H_MOTOR )
+            {
+                memcpy(g_HreadOneRegBuff, g_readOneRegBuff, WRITE_ONE_REG_FRAME_NUM);
+                return g_HreadOneRegBuff;
+            }
+
+        memcpy(g_VreadOneRegBuff, g_readOneRegBuff, WRITE_ONE_REG_FRAME_NUM);
+        return g_VreadOneRegBuff; 
+    }
+
+
+
+    /**
+    * @brief 伺服电机驱动初始化
+    * @param
+    * @return 
+    */
+    void servoMotorInit(void)
+    {
+        MotorSwitchGpioCofig();//两个电机电源的开关PE0,1引脚，如果其它地方实现了，可以不需要
+        H_MOTOR_OPEN;
+        OSTimeDlyHMSM(0u, 0u, 0u, 1000u);
+        V_MOTOR_OPEN;
+        
+        CommuDrvInit();//伺服电机RS485通讯初始化
+    }
+#endif

BSP/Driver/servoMotor/servoMotor.h

@@ -4,80 +4,88 @@
 #include "motorCommu.h"
 #include "modbus_crc.h"
 #include "stdbool.h"
-/*
+#include "ptz_type_select.h"
-********************************************************************************************************
-*                             电机硬件相关
-********************************************************************************************************
-*/
-#define H_MOTOR_OPEN    gpio_bit_set(GPIOE,GPIO_PIN_0)      //水平电机电源打开
-#define H_MOTOR_STOP    gpio_bit_reset(GPIOE,GPIO_PIN_0)    //水平电机电源关闭
-#define V_MOTOR_OPEN    gpio_bit_set(GPIOE,GPIO_PIN_1)      //垂直电机电源打开
-#define V_MOTOR_STOP    gpio_bit_reset(GPIOE,GPIO_PIN_1)    //垂直电机电源关闭
 
-#define H_MOTOR_ADDR    0x01//水平电机地址
+    #ifdef PTZ_MEDIUM_WORM_SERVO_MOTOR_24V
-#define V_MOTOR_ADDR    0x01//垂直电机地址
+    /*
-/*
+    ********************************************************************************************************
-********************************************************************************************************
+    *                             电机硬件相关
-*                             功能码命令
+    ********************************************************************************************************
-********************************************************************************************************
+    */
-*/
+    #define H_MOTOR_OPEN    gpio_bit_set(GPIOE,GPIO_PIN_0)      //水平电机电源打开
-#define     READ_ONE_REG            0X03//读单个寄存器
+    #define H_MOTOR_STOP    gpio_bit_reset(GPIOE,GPIO_PIN_0)    //水平电机电源关闭
-#define     READ_MULT_CONSE_REG     0X03//读多个连续的寄存器Read multiple consecutive registers
+    #define V_MOTOR_OPEN    gpio_bit_set(GPIOE,GPIO_PIN_1)      //垂直电机电源打开
-#define     WRITE_ONE_REG           0X06//写单个寄存器
+    #define V_MOTOR_STOP    gpio_bit_reset(GPIOE,GPIO_PIN_1)    //垂直电机电源关闭
-#define     WRITE_MULT_CONSE_REG    0x10//写多个连续的寄存器
 
-#define     WRITE_ONE_REG_BUFFNUM   8//写单个寄存器，数据帧的字节个数
+    #define H_MOTOR_ADDR    0x01//水平电机地址
-/*
+    #define V_MOTOR_ADDR    0x01//垂直电机地址
-********************************************************************************************************
+    /*
-*                             寄存器参数
+    ********************************************************************************************************
-********************************************************************************************************
+    *                             功能码命令
-*/
+    ********************************************************************************************************
-/*基本控制参数H02*/
+    */
-#define H02_CONTR_MODE_SELEC        0X0200//（Control mode selection）控制模式选择0：速度模式，1：位置模式，2：转矩模式  
+    #define     READ_ONE_REG            0X03//读单个寄存器
+    #define     READ_MULT_CONSE_REG     0X03//读多个连续的寄存器Read multiple consecutive registers
+    #define     WRITE_ONE_REG           0X06//写单个寄存器
+    #define     WRITE_MULT_CONSE_REG    0x10//写多个连续的寄存器
 
-/*DI/DO参数H03~H04*/
+    #define     WRITE_ONE_REG_FRAME_NUM     8//写单个寄存器，数据帧的字节个数
-#define H03_DI1_FUNC_SELEC          0X0302//DI1端子功能选择,一个 DI 功能选项只能关联一个 DI 端子，不可重复分配
+    #define     READ_ONE_REG_FRAME_NUM      8//读单个寄存器，数据帧的字节个数
-#define H03_DI1_LOGICAL_SELEC       0X0303//DI1端子逻辑选择
+    /*
-#define H04_DO1_FUNC_SELEC          0X0400//DO1端子功能选择
+    ********************************************************************************************************
-#define H04_DO1_LOGICAL_SELEC       0X0401//DO1端子逻辑选择
+    *                             寄存器参数
+    ********************************************************************************************************
+    */
+    /*基本控制参数H02*/
+    #define H02_CONTR_MODE_SELEC        0X0200//（Control mode selection）控制模式选择0：速度模式，1：位置模式，2：转矩模式  
+    #define H02_MOTOR_DIR_SELEC         0X0202//电机旋转方向选择
+    /*DI/DO参数H03~H04*/
+    #define H03_DI1_FUNC_SELEC          0X0302//DI1端子功能选择,一个 DI 功能选项只能关联一个 DI 端子，不可重复分配
+    #define H03_DI1_LOGICAL_SELEC       0X0303//DI1端子逻辑选择
+    #define H04_DO1_FUNC_SELEC          0X0400//DO1端子功能选择
+    #define H04_DO1_LOGICAL_SELEC       0X0401//DO1端子逻辑选择
 
-/*速度控制参数H06*/
+    /*速度控制参数H06*/
-#define H06_SPEED_COMMAND_SELEC     0X0602//速度指令选择
+    #define H06_SPEED_COMMAND_SELEC     0X0602//速度指令选择
-#define H06_SPEED_COMMU_SET_VALUE   0X0603//速度指令通讯设置值,当 H06_02=0 时，通过此参数设定电机运行转速
+    #define H06_SPEED_COMMU_SET_VALUE   0X0603//速度指令通讯设置值,当 H06_02=0 时，通过此参数设定电机运行转速
-#define H06_SPEED_UP_SLOPE_VALUE    0X0605//速度指令加速斜坡时间常数
+    #define H06_SPEED_UP_SLOPE_VALUE    0X0605//速度指令加速斜坡时间常数
-#define H06_SPEED_DOWN_SLOPE_VALUE  0X0606//速度指令减速斜坡时间常数
+    #define H06_SPEED_DOWN_SLOPE_VALUE  0X0606//速度指令减速斜坡时间常数
-#define H06_SPEED_REACH_MAX         0X0618//速度到达信号阈值
+    #define H06_SPEED_REACH_MAX         0X0618//速度到达信号阈值
 
-/*RS485通讯与功能参数H0C*/
+    /*RS485通讯与功能参数H0C*/
-#define H0C_COMMU_PARAM_EEPR_UPDATE 0X0C13//MODBUS通讯写入是否更新到 EEPROM，设置1为写入
+    #define H0C_COMMU_PARAM_EEPR_UPDATE 0X0C13//MODBUS通讯写入是否更新到 EEPROM，设置1为写入
+
+    /*监视只读参数*/
+    #define READ_MOTOR_SPEED_NOW        0X0B00//读取电机滤波后的实时转速
 
 
 
 
-/**
+    /**
-* @brief    伺服电机速度模式写单个寄存器
+    * @brief    伺服电机速度模式写单个寄存器
-* @param    motorNo：写垂直电机还是水平电机
+    * @param    motorNo：写垂直电机还是水平电机
-* @param    regAddr：要写的寄存器
+    * @param    regAddr：要写的寄存器
-* @param    data：要向寄存器写入的数据
+    * @param    data：要向寄存器写入的数据
-* @return   false:写失败，当前DMA正在发送数据
+    * @return   false:写失败，当前DMA正在发送数据
-*/
+    */
-bool WriteMotorOneReg(uint8_t motorNo, uint16_t regAddr, uint16_t data);
+    uint8_t* WriteMotorOneReg(uint8_t motorNo, uint16_t regAddr, int16_t data);
-
-/**
-* @brief    写单个寄存器成功后，电机会返回一帧与写格式完全一致的数据
-* @param    motorNo：写垂直电机还是水平电机
-* @param    userBuff：用来接收返回的数据的数组
-* @param    len：接收的数据长度，对于此函数必须为WRITE_ONE_REG_BUFFNUM
-* @return   false:错误，读出的内容与写入的内容不一致
-*/
-bool MotorReplyForWrite(uint8_t motorNo, uint8_t* userBuff, uint8_t len);
-
-/**
-* @brief 伺服电机驱动初始化
-* @param
-* @return 
-*/
-void servoMotorInit(void);
 
 
+    /**
+    * @brief    伺服电机速度模式读单个寄存器
+    * @param    motorNo：读垂直电机还是水平电机
+    * @param    regAddr：要读的寄存器
+    * @return   返回数组地址
+    */
+    uint8_t* ReadMotorOneReg(uint8_t motorNo, uint16_t regAddr);
+
+
+
+    /**
+    * @brief 伺服电机驱动初始化
+    * @param
+    * @return 
+    */
+    void servoMotorInit(void);
+    #endif
 #endif

PROJECT/OS2.ewp

@@ -826,7 +826,7 @@
                 </option>
                 <option>
                     <name>IlinkIcfFile</name>
-                    <state>D:\CompanyCode\newPro\servoMotor_xr\BSP\IAR\GD32F450xE.icf</state>
+                    <state>D:\psx\Pan-Tilt\1.software\HY\new_ptz\servoMotor\BSP\IAR\GD32F450xE.icf</state>
                 </option>
                 <option>
                     <name>IlinkIcfFileSlave</name>