解决链表释放BUG

APP/Device/Device_rotate/rotate_servo.c

@@ -59,7 +59,6 @@ void ptz_sem_post_stop_mutex()
 */
 void set_speed_to_servoMotor(uint8_t motorType, float speed)
 {
-
     /*水平电机*/
     if ( motorType == horiMotorType )//水平电机
     {
@@ -79,47 +78,8 @@ void set_speed_to_servoMotor(uint8_t motorType, float speed)
                                 , WRITE_ONE_REG_FRAME_NUM, lowPriority);
     }
     
-   
-//    //转换为电机端的r/min
-//    static float speedTemp;
-//    speed = speed * PTZ_HORI_RATIO + 0.5;
-//    speedTemp = speed;
-//    speedTemp < 0 ? speedTemp *= -1 : speedTemp; //变为正数
-//    
-//    if (motorType == horiMotorType) {
-//        if (speedTemp > PTZ_HORI_MOTOR_MAX_SPEED) {
-//            speedTemp = PTZ_HORI_MOTOR_MAX_SPEED;
-//        }
-//        else if (speedTemp < PTZ_HORI_MIN_SPEED) {
-//            speedTemp = PTZ_HORI_MOTOR_MIN_SPEED;
-//        }
-//        //设置速度，电机的r/min范围-6000~6000
-//        servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H06_SPEED_COMMU_SET_VALUE, speed)
-//                                , WRITE_ONE_REG_FRAME_NUM, lowPriority);
-//    }
-//    else if (motorType == vertMotorType) {
-//        if (speedTemp > PTZ_VERT_MOTOR_MAX_SPEED) {
-//            speedTemp = PTZ_VERT_MOTOR_MAX_SPEED;
-//        }
-//        else if (speedTemp < PTZ_VERT_MOTOR_MIN_SPEED) {
-//            speedTemp = PTZ_VERT_MOTOR_MIN_SPEED;
-//        }
-//   
-//        //设置速度，电机的r/min范围-6000~6000
-//        servoSendData(vertMotorType, WriteMotorOneReg(V_MOTOR, H06_SPEED_COMMU_SET_VALUE, 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);
@@ -142,9 +102,7 @@ void ptz_hori_start(char direction, float speed)//输入参数的speed是云台
     // 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, speedTemp);
-//    OSTimeDlyHMSM(0u, 0u, 0u, 5u);
+
-    servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 1)
-                              , WRITE_ONE_REG_FRAME_NUM, lowPriority); //使能电机
     g_ptz.hori_speed_set = speedTemp;
     //设定转动方向
     g_ptz.hori_direction_last = g_ptz.hori_direction_set;
@@ -155,6 +113,8 @@ void ptz_hori_start(char direction, float speed)//输入参数的speed是云台
     g_ptz.hori_start_stop_actual = g_ptz.hori_start_stop_set;
         
     ptz_hori_stop_count = 0;
+    servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 1)
+                          , WRITE_ONE_REG_FRAME_NUM, lowPriority); //使能电机
     BSP_OS_SemPost(&ptz_hori_stop_mutex);
 }
 
@@ -162,8 +122,6 @@ void ptz_hori_stop(unsigned short int time)
 {
     BSP_OS_SemWait(&ptz_hori_stop_mutex, 0u);
 
-    set_speed_to_servoMotor(horiMotorType, 0);
-
     //停止电机
     g_ptz.hori_start_stop_set = PTZ_HORI_STOP;
     g_ptz.hori_start_stop_actual = g_ptz.hori_start_stop_set;
@@ -172,13 +130,16 @@ void ptz_hori_stop(unsigned short int time)
   
     g_ptz.hori_speed_set = 0;
     g_ptz.hori_speed_actual = 0;
-  
+//    设置速度为0
+    set_speed_to_servoMotor(horiMotorType, 0);
     if(ptz_hori_stop_count <= 0)
     {
         OSTimeDlyHMSM(0u, 0u, 0u, time);
         ptz_hori_stop_count = 0;
         servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 0)
-                                  , WRITE_ONE_REG_FRAME_NUM, lowPriority); //失能电机 
+                                  , WRITE_ONE_REG_FRAME_NUM, highPriority); //失能电机 
+        // servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 0)
+        //                           , WRITE_ONE_REG_FRAME_NUM, lowPriority); //失能电机
     }
     ptz_hori_stop_count ++;
   //电子稳定
@@ -192,13 +153,14 @@ void ptz_hori_stop(unsigned short int time)
 void ptz_vert_start(char direction, float speed)//输入参数的speed是云台末端的r/min
 {
     BSP_OS_SemWait(&ptz_vert_stop_mutex, 0u);
-
+    static float speedTemp;
     switch ( direction )
     {
         case PTZ_VERT_DIR_UP://伺服电机默认速度为正使云台向上
+            speedTemp = speed;
             break;
         case PTZ_VERT_DIR_DOWN:
-            speed = -speed;
+            speedTemp = -speed;
             break;    
         case PTZ_VERT_DIR_STOP:
             break;
@@ -209,17 +171,17 @@ void ptz_vert_start(char direction, float speed)//输入参数的speed是云台
         -------------------------------------add speed change to here--------------------------------------
     */
     //设定转动速度
-    set_speed_to_servoMotor(vertMotorType, speed);
+    set_speed_to_servoMotor(vertMotorType, speedTemp);
-    servoSendData(vertMotorType, WriteMotorOneReg(H_MOTOR, H03_DI1_LOGICAL_SELEC, 1)
+    servoSendData(vertMotorType, WriteMotorOneReg(V_MOTOR, H03_DI1_LOGICAL_SELEC, 1)
                               , WRITE_ONE_REG_FRAME_NUM, lowPriority); //使能电机
-    g_ptz.hori_speed_set = speed;
+    g_ptz.vert_speed_set = speedTemp;
     //设定转动方向
-    g_ptz.hori_direction_last = g_ptz.hori_direction_set;
+    g_ptz.vert_direction_last = g_ptz.vert_direction_set;
-    g_ptz.hori_direction_set = direction;
+    g_ptz.vert_direction_set = direction;
-    g_ptz.hori_direction_actual = g_ptz.hori_direction_set;
+    g_ptz.vert_direction_actual = g_ptz.vert_direction_set;
     //启动电机
-    g_ptz.hori_start_stop_set = PTZ_HORI_START;
+    g_ptz.vert_start_stop_set = PTZ_VERT_START;
-    g_ptz.hori_start_stop_actual = g_ptz.hori_start_stop_set;
+    g_ptz.vert_start_stop_actual = g_ptz.vert_start_stop_set;
         
     ptz_vert_stop_count = 0;
     
@@ -255,14 +217,832 @@ void ptz_vert_stop(unsigned short int time)
     BSP_OS_SemPost(&ptz_vert_stop_mutex);
 }
 
-static void ptz_hori_rotate_monitor_task()
+static char ptz_hori_rotate_monitor_task()
 {
+  static float BNP_D;//刹车最近点与当前位置的距离
+  static float BFP_D;//刹车最远点与当前位置的距离
+  static unsigned int k;
   
+    // OSSemPend(g_horiMotorMutex, 100, &err);
+  switch(g_ptz.hori_rotate_monitor_switch)
+  {
+    case 0://关闭
+      OSTimeDlyHMSM(0u, 0u, 0u, 10u);
+      break;
+      
+    case PTZ_HORI_RIGHT_KEEP://向右转动，不刹车
+      //关闭转动监控
+      g_ptz.hori_rotate_monitor_switch = 0;
+      //首先判断云台是否在转动
+      if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
+      {
+        //判断转动方向是否相同
+        //如果转向相同，则直接修改设置的转速
+        if(g_ptz.hori_direction_set == PTZ_HORI_DIR_RIGHT)
+        {
+          if ( g_ptz.hori_speed_set != g_ptz.hori_rotate_plan.max_speed )
+          {
+              g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
+              set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR   
+          }
+        }
+        else//如果转向不同
+        {
+          //则先刹车
+          ptz_hori_stop(PTZ_HORI_STOP_TIME);
+          //再启动
+          g_ptz.hori_direction_set = PTZ_HORI_DIR_RIGHT;
+          g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
+          ptz_hori_start(PTZ_HORI_DIR_RIGHT, g_ptz.hori_speed_set);
+        }
+      }
+      else//没有转动
+      {
+        g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
+        ptz_hori_start(PTZ_HORI_DIR_RIGHT, g_ptz.hori_speed_set);
+      }
+      break;
+      
+    case PTZ_HORI_LEFT_KEEP://向左转动，不刹车
+      //关闭转动监控
+      g_ptz.hori_rotate_monitor_switch = 0;
+      //首先判断云台是否在转动
+      if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
+      {
+        //判断转动方向是否相同
+        //如果转向相同，则直接修改设置的转速
+        if(g_ptz.hori_direction_set == PTZ_HORI_DIR_LEFT)
+        {
+          if ( g_ptz.hori_speed_set != g_ptz.hori_rotate_plan.max_speed )
+          {
+              g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
+              set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR   
+          }
+        }
+        else//如果转向不同
+        {
+          //则先刹车
+          ptz_hori_stop(PTZ_HORI_STOP_TIME);
+          //再启动
+          g_ptz.hori_direction_set = PTZ_HORI_DIR_LEFT;
+          g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
+          ptz_hori_start(PTZ_HORI_DIR_LEFT, g_ptz.hori_speed_set);
+        }
+      }
+      else//没有转动
+      {
+        g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
+        ptz_hori_start(PTZ_HORI_DIR_LEFT, g_ptz.hori_speed_set);
+      }
+      break;
+      
+    case PTZ_HORI_GO_TO_ANGLE_A://角度定位
+    case PTZ_HORI_MIN_DISTANCE:
+    case PTZ_HORI_MAX_DISTANCE:
+    case PTZ_HORI_RIGHT_ANGLE:
+    case PTZ_HORI_LEFT_ANGLE:    
+      //首先判断是否需要转动,如果转动距离为0，即同一个位置则不需要转动
+      if(g_ptz.hori_rotate_plan.rotate_switch == 1)
+      {
+         g_ptz.hori_rotate_monitor_switch = PTZ_HORI_GO_TO_ANGLE_B;
+        //如果不是处于同一个位置
+        //判断是否转动
+        //处于转动
+        if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
+        {
+          //判断转向是否相同
+          //如果转向相同
+          if(g_ptz.hori_direction_set == g_ptz.hori_rotate_plan.direction)
+          {
+            g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
+//            set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR
+          }
+          else//转向不同，则直接修改设置的转速
+          {
+            //则先刹车
+            ptz_hori_stop(PTZ_HORI_STOP_TIME);
+            //再启动
+            g_ptz.hori_direction_set = g_ptz.hori_rotate_plan.direction;
+            g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
+            ptz_hori_start(g_ptz.hori_direction_set, g_ptz.hori_speed_set);
+          }
+        }
+        else//没有转动
+        {
+          g_ptz.hori_direction_set = g_ptz.hori_rotate_plan.direction;
+          g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
+          ptz_hori_start(g_ptz.hori_direction_set, g_ptz.hori_speed_set);
+        }
+        
+        //如果离最近刹车距离小于一定程度，则以最小转速转动
+        if(g_ptz.hori_direction_set == PTZ_HORI_DIR_LEFT)
+        {
+          if(g_ptz.hori_angle_actual >= g_ptz.hori_rotate_plan.stop_near_angle)
+          {
+            if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <= 
+               PTZ_HORI_BREAK_SPEED_ANGLE)
+            {
+               g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
+//               set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR  
+            }
+          }
+          else
+          {
+            if((fabs(360.0 - g_ptz.hori_rotate_plan.stop_near_angle) + g_ptz.hori_angle_actual) <=
+               PTZ_HORI_BREAK_SPEED_ANGLE)
+            {
+                  g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
+//                  set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR
+            }
+          }
+        }
+        if(g_ptz.hori_direction_set == PTZ_HORI_DIR_RIGHT)
+        {
+          if(g_ptz.hori_angle_actual <= g_ptz.hori_rotate_plan.stop_near_angle)
+          {
+            if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <= 
+               PTZ_HORI_BREAK_SPEED_ANGLE)
+            {
+              g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
+//              set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR
+            }
+          }
+          else
+          {
+            if((fabs(360.0 - g_ptz.hori_angle_actual) + g_ptz.hori_rotate_plan.stop_near_angle) <= 
+               PTZ_HORI_BREAK_SPEED_ANGLE)
+            {
+              g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
+//              set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR
+            }
+          }
+        }
+        
+       
+      }
+      else//g_ptz.hori_rotate_plan.rotate_switch == 0;
+      {
+        g_ptz.hori_rotate_monitor_switch = 0;
+        //不需要转动则刹车停止，关闭监控
+        g_ptz.hori_arrive_flag = 0;//转动到指定位置
+        ptz_location_return_return(LOCATION_HORI);//定位回传
+        ptz_preset_bit_location_return_return();//预置位到位回传
+        if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
+        {
+          ptz_hori_stop(PTZ_HORI_STOP_TIME);
+        }
+      }
+      break;
+    
+      
+    case PTZ_HORI_RIGHT_CYCLE:
+    case PTZ_HORI_LEFT_CYCLE:
+      if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
+      {
+        //判断转向是否相同
+        //如果转向相同
+        if(g_ptz.hori_direction_set == g_ptz.hori_rotate_plan.direction)
+        {
+          g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
+//          set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR
+        }
+        else//转向不同，则直接修改设置的转速
+        {
+          //则先刹车
+          ptz_hori_stop(PTZ_HORI_STOP_TIME);
+          //再启动
+          g_ptz.hori_direction_set = g_ptz.hori_rotate_plan.direction;
+          g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
+          ptz_hori_start(g_ptz.hori_direction_set, g_ptz.hori_speed_set);
+        }
+      }
+      else//没有转动
+      {
+        g_ptz.hori_direction_set = g_ptz.hori_rotate_plan.direction;
+        g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
+        ptz_hori_start(g_ptz.hori_direction_set, g_ptz.hori_speed_set);
+      }
+      g_ptz.hori_rotate_monitor_switch = PTZ_HORI_CYCLE;
+      break;
+      
+    case PTZ_HORI_CYCLE:
+      BNP_D = fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle);
+      if(BNP_D > 360.0 / 2.0)
+      {
+        BNP_D = 360.0 - BNP_D;
+      }
+      BFP_D = fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_far_angle);
+      if(BFP_D > 360.0 / 2.0)
+      {
+        BFP_D = 360.0 - BFP_D;
+      }
+      /*定位*/
+      if(BNP_D + BFP_D > (g_ptz.hori_rotate_plan.stop_angle_range + 2.0))
+      {
+        g_ptz.hori_rotate_monitor_switch = PTZ_HORI_GO_TO_ANGLE_B;
+      }
+      break;
+      
+    case PTZ_HORI_GO_TO_ANGLE_B:
+      BNP_D = fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle);
+      if(BNP_D > 360.0 / 2.0)
+      {
+        BNP_D = 360.0 - BNP_D;
+      }
+      BFP_D = fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_far_angle);
+      if(BFP_D > 360.0 / 2.0)
+      {
+        BFP_D = 360.0 - BFP_D;
+      }
+      /*定位*/
+      if(BNP_D + BFP_D <= (g_ptz.hori_rotate_plan.stop_angle_range + 0.01))
+      {
+        if(g_ptz.hori_repeat_locate_switch == 0)//重复定位关闭
+        {
+          g_ptz.hori_arrive_flag = 0;//转到指定位置
+          ptz_location_return_return(LOCATION_HORI);//定位回传
+          ptz_preset_bit_location_return_return();//预置位到位回传
+        }
+        g_ptz.hori_rotate_monitor_switch = 0;
+        if(g_ptz.hori_repeat_locate_switch == 1)//重复定位打开
+        {
+          g_ptz.hori_rotate_monitor_switch = PTZ_HORI_REPEAT_LOCATE_A;
+        }
+        ptz_hori_stop(PTZ_HORI_STOP_TIME);
+      }
+      //如果离最近刹车距离小于一定程度，则以最小转速转动
+        if(g_ptz.hori_direction_set == PTZ_HORI_DIR_LEFT)
+        {
+          if(g_ptz.hori_angle_actual >= g_ptz.hori_rotate_plan.stop_near_angle)
+          {
+            if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <= 
+               PTZ_HORI_BREAK_SPEED_ANGLE)
+            {
+               g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
+//               set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR
+            }
+          }
+          else
+          {
+            if((fabs(360.0 - g_ptz.hori_rotate_plan.stop_near_angle) + g_ptz.hori_angle_actual) <=
+               PTZ_HORI_BREAK_SPEED_ANGLE)
+            {
+              g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
+//              set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR
+            }
+          }
+        }
+        if(g_ptz.hori_direction_set == PTZ_HORI_DIR_RIGHT)
+        {
+          if(g_ptz.hori_angle_actual <= g_ptz.hori_rotate_plan.stop_near_angle)
+          {
+            if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <= 
+               PTZ_HORI_BREAK_SPEED_ANGLE)
+            {
+              g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
+//              set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR
+            }
+          }
+          else
+          {
+            if((fabs(360.0 - g_ptz.hori_angle_actual) + g_ptz.hori_rotate_plan.stop_near_angle) <= 
+               PTZ_HORI_BREAK_SPEED_ANGLE)
+            {
+              g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
+//              set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR
+            }
+          }
+        }
+      break;
+      
+    case PTZ_HORI_BRAKE://停止转动
+      g_ptz.hori_rotate_monitor_switch = 0;
+      ptz_hori_stop(PTZ_HORI_STOP_TIME);
+      break;
+    
+    case PTZ_HORI_DEC_BRAKE_A://减速刹车
+      g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
+//      set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR
+      g_ptz.hori_rotate_monitor_switch = PTZ_HORI_DEC_BRAKE_B;
+      k = 0;
+      break;
+      
+    case PTZ_HORI_DEC_BRAKE_B:
+       k++;//K增加1，时间增加1ms
+      if(g_ptz.hori_speed_actual <= PTZ_HORI_MIN_SPEED * 2 ||
+         k >= 500)
+      {
+        k = 0;
+        g_ptz.hori_rotate_monitor_switch = 0;
+        ptz_hori_stop(PTZ_HORI_STOP_TIME);
+      }
+
+      break;
+    
+    //重复定位路径规划
+    case  PTZ_HORI_REPEAT_LOCATE_A:
+      ptz_hori_rotate_plan(PTZ_HORI_MIN_DISTANCE);
+      g_ptz.hori_rotate_monitor_switch = PTZ_HORI_REPEAT_LOCATE_B;
+      break;
+    //重复定位启动控制
+    case  PTZ_HORI_REPEAT_LOCATE_B:
+      //首先判断是否需要转动,如果转动距离为0，即同一个位置则不需要转动
+      if(g_ptz.hori_rotate_plan.rotate_switch == 1)
+      {
+        g_ptz.hori_rotate_monitor_switch = PTZ_HORI_REPEAT_LOCATE_C;
+        //如果不是处于同一个位置
+        //判断是否转动
+        //处于转动
+        if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
+        {
+          //判断转向是否相同
+          //如果转向相同
+          if(g_ptz.hori_direction_set == g_ptz.hori_rotate_plan.direction)
+          {
+            g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
+//            set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR
+          }
+          else//转向不同，则直接修改设置的转速
+          {
+            //则先刹车
+            ptz_hori_stop(PTZ_HORI_STOP_TIME);
+            //再启动
+            g_ptz.hori_direction_set = g_ptz.hori_rotate_plan.direction;
+            g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
+            ptz_hori_start(g_ptz.hori_direction_set, g_ptz.hori_speed_set);
+          }
+        }
+        else//没有转动
+        {
+          g_ptz.hori_direction_set = g_ptz.hori_rotate_plan.direction;
+          g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
+          ptz_hori_start(g_ptz.hori_direction_set, g_ptz.hori_speed_set);
+        }
+        
+        //如果离最近刹车距离小于一定程度，则以最小转速转动
+        if(g_ptz.hori_direction_set == PTZ_HORI_DIR_LEFT)
+        {
+          if(g_ptz.hori_angle_actual >= g_ptz.hori_rotate_plan.stop_near_angle)
+          {
+            if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <= 
+               PTZ_HORI_BREAK_SPEED_ANGLE)
+            {
+               g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
+//               set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR
+            }
+          }
+          else
+          {
+            if((fabs(360.0 - g_ptz.hori_rotate_plan.stop_near_angle) + g_ptz.hori_angle_actual) <=
+               PTZ_HORI_BREAK_SPEED_ANGLE)
+            {
+              g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
+//              set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR
+            }
+          }
+        }
+        if(g_ptz.hori_direction_set == PTZ_HORI_DIR_RIGHT)
+        {
+          if(g_ptz.hori_angle_actual <= g_ptz.hori_rotate_plan.stop_near_angle)
+          {
+            if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <= 
+               PTZ_HORI_BREAK_SPEED_ANGLE)
+            {
+              g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
+//              set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR
+            }
+          }
+          else
+          {
+            if((fabs(360.0 - g_ptz.hori_angle_actual) + g_ptz.hori_rotate_plan.stop_near_angle) <= 
+               PTZ_HORI_BREAK_SPEED_ANGLE)
+            {
+              g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
+//              set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR
+            }
+          }
+        }
+        
+      }
+      else//g_ptz.hori_rotate_plan.rotate_switch == 0;
+      {
+        g_ptz.hori_rotate_monitor_switch = 0;
+        //不需要转动则刹车停止，关闭监控
+        g_ptz.hori_arrive_flag = 0;//转到指定预位置
+        ptz_location_return_return(LOCATION_HORI);//定位回传
+        ptz_preset_bit_location_return_return();//预置位到位回传
+        if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
+        {
+          ptz_hori_stop(PTZ_HORI_STOP_TIME);
+        }
+      }
+      break;
+    //重复定位判断是否达到定位位置
+    case  PTZ_HORI_REPEAT_LOCATE_C:
+      BNP_D = fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle);
+      if(BNP_D > 360.0 / 2.0)
+      {
+        BNP_D = 360.0 - BNP_D;
+      }
+      BFP_D = fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_far_angle);
+      if(BFP_D > 360.0 / 2.0)
+      {
+        BFP_D = 360.0 - BFP_D;
+      }
+      /*定位*/
+      if(BNP_D + BFP_D <= (g_ptz.hori_rotate_plan.stop_angle_range + 0.01))
+      {
+        g_ptz.hori_arrive_flag = 0;//转到指定位置
+        ptz_location_return_return(LOCATION_HORI);//定位回传
+        ptz_preset_bit_location_return_return();//预置位到位回传
+        g_ptz.hori_rotate_monitor_switch = 0;
+        ptz_hori_stop(PTZ_HORI_STOP_TIME);
+      }
+      //如果离最近刹车距离小于一定程度，则以最小转速转动
+      if(g_ptz.hori_direction_set == PTZ_HORI_DIR_LEFT)
+      {
+        if(g_ptz.hori_angle_actual >= g_ptz.hori_rotate_plan.stop_near_angle)
+        {
+          if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <= 
+             PTZ_HORI_BREAK_SPEED_ANGLE)
+          {
+             g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
+//             set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR
+          }
+        }
+        else
+        {
+          if((fabs(360.0 - g_ptz.hori_rotate_plan.stop_near_angle) + g_ptz.hori_angle_actual) <=
+             PTZ_HORI_BREAK_SPEED_ANGLE)
+          {
+            g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
+//            set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR
+          }
+        }
+      }
+      if(g_ptz.hori_direction_set == PTZ_HORI_DIR_RIGHT)
+      {
+        if(g_ptz.hori_angle_actual <= g_ptz.hori_rotate_plan.stop_near_angle)
+        {
+          if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <= 
+             PTZ_HORI_BREAK_SPEED_ANGLE)
+          {
+            g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
+//            set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR
+          }
+        }
+        else
+        {
+          if((fabs(360.0 - g_ptz.hori_angle_actual) + g_ptz.hori_rotate_plan.stop_near_angle) <= 
+             PTZ_HORI_BREAK_SPEED_ANGLE)
+          {
+            g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
+//            set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR
+          }
+        }
+      }
+      break;
+      
+    case PTZ_HORI_RIGHT_ANGLE_INC:
+      //首先判断是否需要转动,如果转动距离为0，即同一个位置则不需要转动
+      if(g_ptz.hori_rotate_plan.rotate_switch == 1)
+      {
+        g_ptz.hori_rotate_monitor_switch = PTZ_HORI_GO_TO_ANGLE_B;
+        //如果不是处于同一个位置
+        //判断是否转动
+        //处于转动
+        if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
+        {
+          //判断转向是否相同
+          //如果转向相同
+          if(g_ptz.hori_direction_set == g_ptz.hori_rotate_plan.direction)
+          {
+            g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
+//            set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR
+          }
+          else//转向不同，则直接修改设置的转速
+          {
+            //则先刹车
+            ptz_hori_stop(PTZ_HORI_STOP_TIME);
+            //再启动
+            g_ptz.hori_direction_set = g_ptz.hori_rotate_plan.direction;
+            g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
+            ptz_hori_start(g_ptz.hori_direction_set, g_ptz.hori_speed_set);
+          }
+        }
+        else//没有转动
+        {
+          g_ptz.hori_direction_set = g_ptz.hori_rotate_plan.direction;
+          g_ptz.hori_speed_set = g_ptz.hori_rotate_plan.max_speed;
+          ptz_hori_start(g_ptz.hori_direction_set, g_ptz.hori_speed_set);
+        }
+        
+        //如果离最近刹车距离小于一定程度，则以最小转速转动
+        if(g_ptz.hori_direction_set == PTZ_HORI_DIR_LEFT)
+        {
+          if(g_ptz.hori_angle_actual >= g_ptz.hori_rotate_plan.stop_near_angle)
+          {
+            if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <= 
+               PTZ_HORI_BREAK_SPEED_ANGLE)
+            {
+               g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
+//               set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR
+            }
+          }
+          else
+          {
+            if((fabs(360.0 - g_ptz.hori_rotate_plan.stop_near_angle) + g_ptz.hori_angle_actual) <=
+               PTZ_HORI_BREAK_SPEED_ANGLE)
+            {
+              g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
+//              set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR
+            }
+          }
+        }
+        if(g_ptz.hori_direction_set == PTZ_HORI_DIR_RIGHT)
+        {
+          if(g_ptz.hori_angle_actual <= g_ptz.hori_rotate_plan.stop_near_angle)
+          {
+            if(fabs(g_ptz.hori_angle_actual - g_ptz.hori_rotate_plan.stop_near_angle) <= 
+               PTZ_HORI_BREAK_SPEED_ANGLE)
+            {
+              g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
+//              set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR
+            }
+          }
+          else
+          {
+            if((fabs(360.0 - g_ptz.hori_angle_actual) + g_ptz.hori_rotate_plan.stop_near_angle) <= 
+               PTZ_HORI_BREAK_SPEED_ANGLE)
+            {
+              g_ptz.hori_speed_set = PTZ_HORI_MIN_SPEED;
+//              set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR
+            }
+          }
+        }
+        
+        
+      }
+      else//g_ptz.hori_rotate_plan.rotate_switch == 0;
+      {
+         g_ptz.hori_rotate_monitor_switch = 0;
+        //不需要转动则刹车停止，关闭监控
+        g_ptz.hori_arrive_flag = 0;//转动到指定位置
+        ptz_location_return_return(LOCATION_HORI);//定位回传
+        ptz_preset_bit_location_return_return();//预置位到位回传
+        if(g_ptz.hori_start_stop_set == PTZ_HORI_START)
+        {
+          ptz_hori_stop(PTZ_HORI_STOP_TIME);
+        }
+      }
+      break;
+      
+    case PTZ_HORI_LEFT_ANGLE_INC:
+      
+      break;
+  }
+//    if ( last_hori_rotate_monitor_swicth != now_hori_rotate_monitor_swicth ) 
+//    {
+//        set_speed_to_servoMotor(horiMotorType, g_ptz.hori_speed_set);//XR
+//    }
+  return 1;
 }
 
-static void ptz_vert_rotate_monitor_task()
+static char ptz_vert_rotate_monitor_task()
 {
+  static float BNP_D;//刹车最近点与当前位置的距离
+  static float BFP_D;//刹车最远点与当前位置的距离
+  static char cmd_type;
+  static unsigned int k;
+  switch(g_ptz.vert_rotate_monitor_switch)
+  {
+    case 0://关闭
+      OSTimeDlyHMSM(0u, 0u, 0u, 10u);
+      break;
       
+    case PTZ_VERT_GO_TO_ANGLE_A://转动到指定角度启动
+    case PTZ_VERT_ANGLE:
+    case PTZ_VERT_UP_KEEP:
+    case PTZ_VERT_DOWN_KEEP:
+      cmd_type = g_ptz.vert_rotate_monitor_switch;
+      g_ptz.vert_rotate_monitor_switch = PTZ_VERT_GO_TO_ANGLE_B;
+      if(g_ptz.vert_rotate_plan.rotate_switch == 1)
+      {
+        //首先判断是否转动
+        //处于转动
+        if(g_ptz.vert_start_stop_set == PTZ_VERT_START)
+        {
+          //判断转向是否相同
+          //如果转向相同
+          if(g_ptz.vert_direction_set == g_ptz.vert_rotate_plan.direction)
+          {
+            g_ptz.vert_speed_set = g_ptz.vert_rotate_plan.max_speed;
+//            set_speed_to_servoMotor(vertMotorType, g_ptz.vert_speed_set);//XR
+          }
+          else//转向不同，则直接修改设置的转速
+          {
+            //则先刹车
+            ptz_vert_stop(PTZ_VERT_STOP_TIME);
+            //再启动
+            g_ptz.vert_direction_set = g_ptz.vert_rotate_plan.direction;
+            g_ptz.vert_speed_set = g_ptz.vert_rotate_plan.max_speed;
+            ptz_vert_start(g_ptz.vert_direction_set, g_ptz.vert_speed_set);
+          }
+        }
+        else//没有转动
+        {
+          g_ptz.vert_direction_set = g_ptz.vert_rotate_plan.direction;
+          g_ptz.vert_speed_set = g_ptz.vert_rotate_plan.max_speed;
+          ptz_vert_start(g_ptz.vert_direction_set, g_ptz.vert_speed_set);
+        }
+        //如果离最近刹车距离小于一定程度，则以最小转速转动
+        if(fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_near_angle) <=
+           PTZ_VERT_BREAK_SPEED_ANGLE)
+        {
+          g_ptz.vert_speed_set = PTZ_VERT_MIN_SPEED;
+//          set_speed_to_servoMotor(vertMotorType, g_ptz.vert_speed_set);//XR
+        }
+        
+      }
+      else//g_ptz.vert_rotate_plan.rotate_switch == 0
+      {
+        //刹车关闭转动
+        g_ptz.vert_arrive_flag = 0;//转到指定预位置
+        ptz_location_return_return(LOCATION_VERT);//定位回传
+        ptz_preset_bit_location_return_return();//预置位到位回传
+        g_ptz.vert_rotate_monitor_switch = 0;
+        if(g_ptz.vert_start_stop_set == PTZ_VERT_START)
+        {
+          ptz_vert_stop(PTZ_VERT_STOP_TIME);
+        }
+      }
+      break;
+      
+    case PTZ_VERT_GO_TO_ANGLE_B://转动到指定角度定位
+      BNP_D = fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_near_angle);
+      BFP_D = fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_far_angle);
+      /*定位*/
+      if(BNP_D + BFP_D <= (g_ptz.vert_rotate_plan.stop_angle_range + 0.01))
+      {
+        if(g_ptz.vert_repeat_locate_switch == 0 &&
+           cmd_type != PTZ_VERT_UP_KEEP &&
+           cmd_type != PTZ_VERT_DOWN_KEEP)
+        {
+          g_ptz.vert_arrive_flag = 0;//转到指定预位置
+          ptz_location_return_return(LOCATION_VERT);//定位回传
+          ptz_preset_bit_location_return_return();//预置位到位回传
+        }
+        g_ptz.vert_rotate_monitor_switch = 0;
+        if(g_ptz.vert_repeat_locate_switch == 1 &&
+           cmd_type != PTZ_VERT_UP_KEEP &&
+           cmd_type != PTZ_VERT_DOWN_KEEP)
+        {
+          g_ptz.vert_rotate_monitor_switch = PTZ_VERT_REPEAT_LOCATE_A;
+        }
+        ptz_vert_stop(PTZ_VERT_STOP_TIME);
+      }
+      //如果离最近刹车距离小于一定程度，则以最小转速转动
+      if(fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_near_angle) <=
+         PTZ_VERT_BREAK_SPEED_ANGLE)
+      {
+        g_ptz.vert_speed_set = PTZ_VERT_MIN_SPEED;
+//        set_speed_to_servoMotor(vertMotorType, g_ptz.vert_speed_set);//XR
+      }
+      break;
+      
+    case PTZ_VERT_BRAKE://刹车
+      g_ptz.vert_rotate_monitor_switch = 0;
+      ptz_vert_stop(PTZ_VERT_STOP_TIME);
+      break;
+      
+    case PTZ_VERT_DEC_BRAKE_A://垂直减速刹车
+       g_ptz.vert_speed_set = PTZ_VERT_MIN_SPEED;
+       g_ptz.vert_rotate_monitor_switch = PTZ_VERT_DEC_BRAKE_B;
+       k = 0;
+      break;
+      
+    case PTZ_VERT_DEC_BRAKE_B: 
+      k ++;//K增加1,时间增加1ms 
+      if(g_ptz.vert_speed_actual <= PTZ_VERT_MIN_SPEED * 2 ||
+         k >= 500)
+      {
+        k = 0;
+        g_ptz.vert_rotate_monitor_switch = 0;
+        ptz_vert_stop(PTZ_VERT_STOP_TIME);
+      }
+      break;
+      
+     //重复定位路径规划
+    case PTZ_VERT_REPEAT_LOCATE_A:
+      ptz_vert_rotate_plan(PTZ_VERT_ANGLE);
+      g_ptz.vert_rotate_monitor_switch = PTZ_VERT_REPEAT_LOCATE_B;
+      break;
+    //重复定位转动启动  
+    case PTZ_VERT_REPEAT_LOCATE_B:
+      if(g_ptz.vert_rotate_plan.rotate_switch == 1)
+      {
+        g_ptz.vert_rotate_monitor_switch = PTZ_VERT_REPEAT_LOCATE_C;
+        //首先判断是否转动
+        //处于转动
+        if(g_ptz.vert_start_stop_set == PTZ_VERT_START)
+        {
+          //判断转向是否相同
+          //如果转向相同
+          if(g_ptz.vert_direction_set == g_ptz.vert_rotate_plan.direction)
+          {
+            g_ptz.vert_speed_set = g_ptz.vert_rotate_plan.max_speed;
+//            set_speed_to_servoMotor(vertMotorType, g_ptz.vert_speed_set);//XR
+          }
+          else//转向不同，则直接修改设置的转速
+          {
+            //则先刹车
+            ptz_vert_stop(PTZ_VERT_STOP_TIME);
+            //再启动
+            g_ptz.vert_direction_set = g_ptz.vert_rotate_plan.direction;
+            g_ptz.vert_speed_set = g_ptz.vert_rotate_plan.max_speed;
+            ptz_vert_start(g_ptz.vert_direction_set, g_ptz.vert_speed_set);
+          }
+        }
+        else//没有转动
+        {
+          g_ptz.vert_direction_set = g_ptz.vert_rotate_plan.direction;
+          g_ptz.vert_speed_set = g_ptz.vert_rotate_plan.max_speed;
+          ptz_vert_start(g_ptz.vert_direction_set, g_ptz.vert_speed_set);
+        }
+        //如果离最近刹车距离小于一定程度，则以最小转速转动
+        if(fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_near_angle) <=
+           PTZ_VERT_BREAK_SPEED_ANGLE)
+        {
+          g_ptz.vert_speed_set = PTZ_VERT_MIN_SPEED;
+//          set_speed_to_servoMotor(vertMotorType, g_ptz.vert_speed_set);//XR
+        }
+        
+      }
+      else//g_ptz.vert_rotate_plan.rotate_switch == 0
+      {
+        //刹车关闭转动
+        g_ptz.vert_arrive_flag = 0;//转到指定位置
+        ptz_location_return_return(LOCATION_VERT);//定位回传
+        ptz_preset_bit_location_return_return();//预置位到位回传
+        g_ptz.vert_rotate_monitor_switch = 0;
+        if(g_ptz.vert_start_stop_set == PTZ_VERT_START)
+        {
+          ptz_vert_stop(PTZ_VERT_STOP_TIME);
+        }
+      }
+      break;
+      
+    case PTZ_VERT_REPEAT_LOCATE_C:
+      BNP_D = fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_near_angle);
+      BFP_D = fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_far_angle);
+      /*定位*/
+      if(BNP_D + BFP_D <= (g_ptz.vert_rotate_plan.stop_angle_range + 0.01))
+      {
+        g_ptz.vert_arrive_flag = 0;//预置位扫描，转到指定预置位
+        ptz_location_return_return(LOCATION_VERT);//定位回传
+        ptz_preset_bit_location_return_return();//预置位到位回传
+        g_ptz.vert_rotate_monitor_switch = 0;
+        ptz_vert_stop(PTZ_VERT_STOP_TIME);
+      }
+      //如果离最近刹车距离小于一定程度，则以最小转速转动
+      if(fabs(g_ptz.vert_angle_actual - g_ptz.vert_rotate_plan.stop_near_angle) <=
+         PTZ_VERT_BREAK_SPEED_ANGLE)
+      {
+        g_ptz.vert_speed_set = PTZ_VERT_MIN_SPEED;
+//        set_speed_to_servoMotor(vertMotorType, g_ptz.vert_speed_set);//XR
+      }
+      break;
+  }
+  
+  //当云台垂直方向处于极限位置时，防止垂直方向角度越界
+#ifdef PTZ_PHOTOELECTRIC_SWITCH
+  if(g_ptz.vert_ps_sw1_state == PS_COVER)//下俯极限位置
+  {
+    if(g_ptz.vert_start_stop_set == PTZ_VERT_START 
+       && g_ptz.vert_direction_set == PTZ_VERT_DIR_DOWN)
+    {
+      g_ptz.vert_arrive_flag = 0;
+      g_ptz.vert_rotate_monitor_switch = 0;
+      
+      ptz_vert_stop(PTZ_VERT_STOP_TIME);//停止延时为0
+      OSTimeDlyHMSM(0u, 0u, 0u, 5u);
+    }
+  }
+  if(g_ptz.vert_ps_sw2_state == PS_COVER)//上仰极限位置
+  {
+    if(g_ptz.vert_start_stop_set == PTZ_VERT_START 
+       && g_ptz.vert_direction_set == PTZ_VERT_DIR_UP)
+    {
+      g_ptz.vert_arrive_flag = 0;
+      g_ptz.vert_rotate_monitor_switch = 0;
+      
+      ptz_vert_stop(PTZ_VERT_STOP_TIME);
+      OSTimeDlyHMSM(0u, 0u, 0u, 5u);
+    }
+  }
+#endif
+ 
+  return 1;
 }
 
 
@@ -271,7 +1051,7 @@ static void ptz_hori_rotate_task()
     while(1)
     {
         ptz_hori_rotate_monitor_task();
-        OSTimeDlyHMSM(0u, 0u, 0u, 10u);
+        OSTimeDlyHMSM(0u, 0u, 0u, 2u);
     }
 }
 
@@ -281,7 +1061,7 @@ static void ptz_vert_rotate_task()
     while(1)
     {
         ptz_vert_rotate_monitor_task();
-        OSTimeDlyHMSM(0u, 0u, 0u, 10u);
+        OSTimeDlyHMSM(0u, 0u, 0u, 2u);
     }
 }
 

APP/Device/Device_speed/servoMotor_recv.c

@@ -258,6 +258,7 @@ static void ptz_recv_hori_servo_task()
         stopTimeOut(H_MOTOR);
         //释放信号量，通知能发送一次
         OSSemPost(g_horiSpeedMutex);        
+        // OSMutexPost(g_horiSpeedMutex);
     }
 }
 
@@ -278,6 +279,7 @@ static void ptz_recv_vert_servo_task()
         }
         stopTimeOut(V_MOTOR);
         //释放信号量，通知能发送一次
+        // OSMutexPost(g_vertSpeedMutex);  
         OSSemPost(g_vertSpeedMutex);  
     }
 }
@@ -332,10 +334,10 @@ static  void  creat_task_servo_recv_task(void)
 void Init_ServoMotorRecv(void)
 {
     CPU_INT08U  err;
-    g_horiMotorMutex = OSSemCreate(1);
+    g_horiMotorMutex = OSSemCreate(0);
-    g_vertMotorMutex = OSSemCreate(1);
+    g_vertMotorMutex = OSSemCreate(0);
-    OSSemPend(g_horiMotorMutex, 1, &err);
+//    OSSemPend(g_horiMotorMutex, 1, &err);
-    OSSemPend(g_vertMotorMutex, 1, &err);
+//    OSSemPend(g_vertMotorMutex, 1, &err);
 
     creat_task_servo_recv_task();
 }

APP/Device/Device_speed/speed_to_servoMotor.c

@@ -61,6 +61,15 @@ void ptz_send_speed(char dev, char speed)
 */
 void servoLinkListMemPut(uint8_t motorType)
 {
+    static BSP_OS_SEM servoLinkListMemPutMutex;
+    static uint8_t flag = 1;
+    if (flag == 1) {
+        BSP_OS_SemCreate(&servoLinkListMemPutMutex, 1, "servoLinkListMemPutMutex");
+        flag = 0;
+    }
+    
+    BSP_OS_SemWait(&servoLinkListMemPutMutex, 0);
+
     //水平电机
     if (motorType == horiMotorType) {
         //当前发送完成的数据为高优先级链表中的数据
@@ -76,7 +85,7 @@ void servoLinkListMemPut(uint8_t motorType)
                     g_servoMotorLinkList.horiMotor.LinkListTail_H = NULL;
                 }  
                 g_servoMotorLinkList.horiMotor.linkListNum--;
-                OSMemPut(g_memPtr, (uint8_t *)ptr);
+                OSMemPut(g_memPtr, ptr);
             }
         }
         //当前发送完成的数据为低优先级链表中的数据
@@ -92,7 +101,7 @@ void servoLinkListMemPut(uint8_t motorType)
                     g_servoMotorLinkList.horiMotor.LinkListTail_L = NULL;
                 }     
                 g_servoMotorLinkList.horiMotor.linkListNum--;
-                OSMemPut(g_memPtr, (uint8_t *)ptr);
+                OSMemPut(g_memPtr, ptr);
             }            
         }
 //        g_servoMotorLinkList.horiMotor.linkListNum--;
@@ -110,7 +119,7 @@ void servoLinkListMemPut(uint8_t motorType)
                     g_servoMotorLinkList.vertMotor.LinkListTail_H = NULL;
                 }
                 g_servoMotorLinkList.vertMotor.linkListNum--;
-                OSMemPut(g_memPtr, (uint8_t *)ptr);
+                OSMemPut(g_memPtr, ptr);
                 
             }
         }
@@ -126,14 +135,14 @@ void servoLinkListMemPut(uint8_t motorType)
                     g_servoMotorLinkList.vertMotor.LinkListTail_L = NULL;
                 }
                 g_servoMotorLinkList.vertMotor.linkListNum--;
-                OSMemPut(g_memPtr, (uint8_t *)ptr);
+                OSMemPut(g_memPtr, ptr);
                 
             }
         }
 //        g_servoMotorLinkList.vertMotor.linkListNum--;
     }
     
-
+    BSP_OS_SemPost(&servoLinkListMemPutMutex);
 }
 
 /*!
@@ -149,6 +158,15 @@ void servoLinkListMemPut(uint8_t motorType)
 */
 bool servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priority)
 {
+    static BSP_OS_SEM servoSendDataMutex;
+    static uint8_t flag = 1;
+    if (flag == 1) {
+        BSP_OS_SemCreate(&servoSendDataMutex, 1, "servoLinkListMemPutMutex");
+        flag = 0;
+    }
+    
+    BSP_OS_SemWait(&servoSendDataMutex, 0);
+
     if ((motor != horiMotorType) && (motor!= vertMotorType)) {
         return false;
     }
@@ -172,17 +190,18 @@ bool servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priorit
     }
     ptr->length = dataLen;
     memcpy(ptr->data, data, dataLen);
+    ptr->next = NULL;
 
     //将节点添加进入链表中
     if (motor == horiMotorType) {
         if (priority == highPriority) {
-            if (g_servoMotorLinkList.horiMotor.linkListNum >= sendDataBufNumber) {
+            if (g_servoMotorLinkList.horiMotor.linkListNum >= sendDataBufNumber - 1) {
                 OSMemPut(g_memPtr, ptr);
                 return false;
             }
         }
         else {
-            if (g_servoMotorLinkList.horiMotor.linkListNum >= sendDataBufNumber - 1) {
+            if (g_servoMotorLinkList.horiMotor.linkListNum >= sendDataBufNumber - 2) {
                 OSMemPut(g_memPtr, ptr);
                 return false;
             }
@@ -201,7 +220,6 @@ bool servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priorit
                 g_servoMotorLinkList.horiMotor.LinkListHead_H
                     = g_servoMotorLinkList.horiMotor.LinkListTail_H;
             }
-            g_servoMotorLinkList.horiMotor.linkListNum++;
         }
         else {
             if (g_servoMotorLinkList.horiMotor.LinkListTail_L == NULL) {
@@ -216,21 +234,21 @@ bool servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priorit
                 g_servoMotorLinkList.horiMotor.LinkListHead_L
                     = g_servoMotorLinkList.horiMotor.LinkListTail_L;
             }
-            g_servoMotorLinkList.horiMotor.linkListNum++;
         }
 
+        g_servoMotorLinkList.horiMotor.linkListNum++;
         //释放信号量，通知能发送一次
         OSSemPost(g_horiSpeedSem);
     }
     else {
         if (priority == highPriority) {
-            if (g_servoMotorLinkList.vertMotor.linkListNum >= sendDataBufNumber) {
+            if (g_servoMotorLinkList.vertMotor.linkListNum >= sendDataBufNumber - 1) {
                 OSMemPut(g_memPtr, ptr);
                 return false;
             }
         }
         else {
-            if (g_servoMotorLinkList.vertMotor.linkListNum >= sendDataBufNumber - 1) {
+            if (g_servoMotorLinkList.vertMotor.linkListNum >= sendDataBufNumber - 2) {
                 OSMemPut(g_memPtr, ptr);
                 return false;
             }
@@ -249,7 +267,6 @@ bool servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priorit
                 g_servoMotorLinkList.vertMotor.LinkListHead_H
                     = g_servoMotorLinkList.vertMotor.LinkListTail_H;
             }
-            g_servoMotorLinkList.vertMotor.linkListNum++;
         }
         else {
             if (g_servoMotorLinkList.vertMotor.LinkListTail_L == NULL) {
@@ -264,12 +281,13 @@ bool servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priorit
                 g_servoMotorLinkList.vertMotor.LinkListHead_L
                     = g_servoMotorLinkList.vertMotor.LinkListTail_L;
             }
-            g_servoMotorLinkList.vertMotor.linkListNum++;
         }
 
+        g_servoMotorLinkList.vertMotor.linkListNum++;
         //释放信号量，通知能发送一次
         OSSemPost(g_vertSpeedSem);
     }
+    BSP_OS_SemPost(&servoSendDataMutex);
     return true;
 }
 
@@ -284,6 +302,7 @@ static void ptz_hori_servo_task()
         // if(g_ptz.hori_start_stop_set == PTZ_HORI_STOP)
         // {
         // }
+        // OSMutexPend(g_horiSpeedMutex, 0, &err);
         OSSemPend(g_horiSpeedMutex, 0, &err);
         OSSemPend(g_horiSpeedSem, 0, &err);
 
@@ -297,7 +316,6 @@ static void ptz_hori_servo_task()
             startTimeOut(horiMotorType);
             continue;
         }
-        
         // 高优先级链表中无数据时，发送低优先级中的数据
         if (g_servoMotorLinkList.horiMotor.LinkListHead_L != NULL) {
             //发送数据
@@ -407,12 +425,14 @@ static  void  creat_task_vert_servo_task(void)
 void horiServoTimeOut()
 {
     servoLinkListMemPut(horiMotorType);
+    // OSMutexPost(g_horiSpeedMutex);    
     OSSemPost(g_horiSpeedMutex);
 }
 
 void vertServoTimeOut()
 {
     servoLinkListMemPut(vertMotorType);
+    // OSMutexPost(g_vertSpeedMutex);
     OSSemPost(g_vertSpeedMutex);
 }
 
@@ -460,6 +480,15 @@ void servoCommSoftWareTimInit()
 */
 void stopTimeOut(uint8_t motorType)
 {
+    static BSP_OS_SEM stopTimeOutMutex;
+    static uint8_t flag = 1;
+    if (flag == 1) {
+        BSP_OS_SemCreate(&stopTimeOutMutex, 1, "servoLinkListMemPutMutex");
+        flag = 0;
+    }
+
+    BSP_OS_SemWait(&stopTimeOutMutex, 0);
+
     if (motorType == horiMotorType) {
         CPU_INT08U  err;
         OSTmrStop(horiServoSoftWareTim, OS_TMR_OPT_NONE, (void *)0, &err);
@@ -474,6 +503,8 @@ void stopTimeOut(uint8_t motorType)
             pdebug(DEBUG_LEVEL_FATAL,"start vertServoSoftWareTim  failed...\n\r");
         }
     }
+
+    BSP_OS_SemPost(&stopTimeOutMutex);
 }
 
 /*!
@@ -486,6 +517,15 @@ void stopTimeOut(uint8_t motorType)
 */
 void startTimeOut(uint8_t motorType)
 {
+    static BSP_OS_SEM startTimeOutMutex;
+    static uint8_t flag = 1;
+    if (flag == 1) {
+        BSP_OS_SemCreate(&startTimeOutMutex, 1, "servoLinkListMemPutMutex");
+        flag = 0;
+    }
+
+    BSP_OS_SemWait(&startTimeOutMutex, 0);
+
     if (motorType == horiMotorType) {
         CPU_INT08U  err;
         OSTmrStart(horiServoSoftWareTim, &err);
@@ -500,19 +540,23 @@ void startTimeOut(uint8_t motorType)
             pdebug(DEBUG_LEVEL_FATAL,"start vertServoSoftWareTim  failed...\n\r");
         }
     }
+    BSP_OS_SemPost(&startTimeOutMutex);
 }
 
 void init_speed_module(void)
 {
+    CPU_INT08U  err;
+
     g_horiSpeedSem = OSSemCreate(0);
     g_vertSpeedSem = OSSemCreate(0);
+    // g_horiSpeedMutex = OSMutexCreate(TASK_HORI_PID_PRIO, &err);
+    // g_vertSpeedMutex = OSMutexCreate(22, &err);
     g_horiSpeedMutex = OSSemCreate(1);
     g_vertSpeedMutex = OSSemCreate(1);
 
-    OSSemPost(g_horiSpeedMutex);
+    // OSSemPost(g_horiSpeedMutex);
-    OSSemPost(g_vertSpeedMutex);
+    // OSSemPost(g_vertSpeedMutex);
 
-    CPU_INT08U  err;
     g_memPtr = OSMemCreate(memBuffer, sendDataBufNumber * 2, linkListNodeLen, &err);
     if (err != OS_ERR_NONE) {
         pdebug(DEBUG_LEVEL_FATAL, "Failed to create the motor memory pool\n\r");
@@ -551,38 +595,11 @@ void init_speed_module(void)
                              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, H06_SPEED_COMMU_SET_VALUE, 0)
                              , WRITE_ONE_REG_FRAME_NUM, lowPriority);
      servoSendData(vertMotorType, WriteMotorOneReg(vertMotorType, H06_SPEED_COMMU_SET_VALUE, 0)
                              , WRITE_ONE_REG_FRAME_NUM, lowPriority);
 
-     
-//     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_Other/device_adc_collect.c

@@ -501,14 +501,12 @@ static char ptz_data_collect_task()
     
     if(j >= 100)
     {
-
+        //读取水平电机实时速度
-        
+        servoSendData(horiMotorType, ReadMotorOneReg(horiMotorType, READ_MOTOR_SPEED_NOW)
-        // //读取水平电机实时速度
+                            , READ_ONE_REG_FRAME_NUM, lowPriority);
-        // 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);
-        // servoSendData(vertMotorType, ReadMotorOneReg(vertMotorType, READ_MOTOR_SPEED_NOW)
-        //                     , READ_ONE_REG_FRAME_NUM, lowPriority);
       j=0;
       //云台不自检关闭，打开采集任务
 #ifndef PTZ_NO_SELF_CHECK

APP/Service/service_selfcheck.c

@@ -956,7 +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);
+//      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;
@@ -1020,6 +1020,7 @@ static unsigned char ptz_vert_complete_self_check_task()
           g_ptz.vert_ps_sw1_up_rise = 0;
           OSTimeDlyHMSM(0u, 0u, 0u, 300u);
           ptz_vert_stop(PTZ_VERT_STOP_TIME);
+          OSTimeDlyHMSM(0u, 0u, 0u, 10u);
           ptz_vert_start(PTZ_VERT_DIR_UP, PTZ_VERT_SELF_CHECK_SPEED);
           g_ptz.vert_self_check++;
         }
@@ -1148,6 +1149,7 @@ static unsigned char ptz_vert_simplify_self_check_task()
           g_ptz.vert_ps_sw2_down_rise = 0;
           OSTimeDlyHMSM(0u, 0u, 0u, 300u);
           ptz_vert_stop(PTZ_VERT_STOP_TIME);
+          OSTimeDlyHMSM(0u, 0u, 0u, 10u);
           ptz_vert_start(PTZ_VERT_DIR_UP, PTZ_VERT_SELF_CHECK_SPEED);
           g_ptz.vert_self_check++;
         }

BSP/Driver/servoMotor/motorCommu.c

@@ -210,11 +210,11 @@ static void DmaCofig(void)
         //dma中断配置
         if( i == H_MOTOR )
         {
-          nvic_irq_enable(g_MotorDmaBuff[H_MOTOR].dmaTxIrq, 1, 2);
+          nvic_irq_enable(g_MotorDmaBuff[H_MOTOR].dmaTxIrq, 2, 2);
         }
         else
         {
-          nvic_irq_enable(g_MotorDmaBuff[V_MOTOR].dmaTxIrq, 1, 1);
+          nvic_irq_enable(g_MotorDmaBuff[V_MOTOR].dmaTxIrq, 2, 1);
         }
         dma_interrupt_enable(g_MotorDmaBuff[i].dmaNo, g_MotorDmaBuff[i].dmaTxch, DMA_CHXCTL_FTFIE);//dma发送完成中断
 
@@ -238,11 +238,11 @@ static void DmaCofig(void)
         //中断配置
         if ( i == H_MOTOR )
         {
-          nvic_irq_enable(g_MotorDmaBuff[H_MOTOR].dmaRxIrq, 0, 2);
+          nvic_irq_enable(g_MotorDmaBuff[H_MOTOR].dmaRxIrq, 1, 2);
         }
         else
         {
-          nvic_irq_enable(g_MotorDmaBuff[V_MOTOR].dmaRxIrq, 0, 1);
+          nvic_irq_enable(g_MotorDmaBuff[V_MOTOR].dmaRxIrq, 1, 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); 

BSP/Driver/servoMotor/servoMotor.c

@@ -30,8 +30,8 @@
     * @param    data：要向寄存器写入的数据
     * @return   返回数组地址
     */
-    static uint8_t g_HwriteOneRegBuff[WRITE_ONE_REG_FRAME_NUM];
+    uint8_t g_HwriteOneRegBuff[WRITE_ONE_REG_FRAME_NUM];
-    static uint8_t g_VwriteOneRegBuff[WRITE_ONE_REG_FRAME_NUM];
+    uint8_t g_VwriteOneRegBuff[WRITE_ONE_REG_FRAME_NUM];
     uint8_t* WriteMotorOneReg(uint8_t motorNo, uint16_t regAddr, int16_t data)
     {
         static uint8_t g_writeOneRegBuff[WRITE_ONE_REG_FRAME_NUM];

GD32F4xx_Firmware_Library/SYSTICK/gd32f4xx_it.c

@@ -60,6 +60,10 @@ void NMI_Handler(void)
 void HardFault_Handler(void)
 {
   term_printf("****************** HardFault_Handler ********************/\r\n\r\n");
+
+    
+    beep_enable();
+
     /* if Hard Fault exception occurs, go to infinite loop */
     while(1){
     }

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>