diff --git a/APP/Device/Device_speed/speed_to_servoMotor.c b/APP/Device/Device_speed/speed_to_servoMotor.c
index 50bc663..02d6e88 100644
--- a/APP/Device/Device_speed/speed_to_servoMotor.c
+++ b/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,71 @@ void ptz_send_speed(char dev, char speed)
     \param[out] none 
     \retval     none
 */
-void servoLinkListMemPut(linkList *data)
+void servoHoriLinkListMemPut(uint8_t motorType)
 {
-    if (data == NULL) {
-        return;
+    //水平电机
+    if (motorType == horiMotorType) {
+        //当前发送完成的数据为高优先级链表中的数据
+        if (g_servoMotorLinkList.horiMotor.linkListNum == 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;
+                }                
+                OSMemPut(g_memPtr, (uint8_t *)ptr);
+            }            
+        }
+        //当前发送完成的数据为低优先级链表中的数据
+        else if (g_servoMotorLinkList.horiMotor.linkListNum == 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;
+                }                
+                OSMemPut(g_memPtr, (uint8_t *)ptr);
+            }            
+        }
+    }
+    else if (motorType == vertMotorType) {
+        if (g_servoMotorLinkList.vertMotor.linkListNum == 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;
+                }
+                OSMemPut(g_memPtr, (uint8_t *)ptr);
+            }
+        }
+        else if (g_servoMotorLinkList.vertMotor.linkListNum == 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;
+                }
+                OSMemPut(g_memPtr, (uint8_t *)ptr);
+            }
+        }
     }
-    
-    linkList *ptr;
-    ptr = data;
-    data = data->next;
-    OSMemPut(g_memPtr, (uint8_t *)ptr);
 }
 
 /*!
@@ -119,18 +171,32 @@ void servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priorit
         }
 
         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;
             }
         }
         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;
-            }            
+            }
         }
 
         //释放信号量，通知能发送一次
@@ -143,14 +209,28 @@ void servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priorit
         }
 
         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;
             }
         }
         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;
@@ -289,6 +369,102 @@ static  void  creat_task_vert_servo_task(void)
 	}
 }
 
+void horiServoTimeOut()
+{
+    OSSemPost(g_horiSpeedMutex);
+}
+
+void vertServoTimeOut()
+{
+    OSSemPost(g_vertSpeedMutex);
+}
+
+BSP_OS_TMR horiServoSoftWareTim;
+BSP_OS_TMR vertServoSoftWareTim;
+void servoCommSoftWareTimInit()
+{
+    CPU_INT08U  ServoSoftWareTimErr;
+    horiServoSoftWareTim = OSTmrCreate(10
+                                        , 10
+                                        , OS_TMR_OPT_PERIODIC
+                                        , (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(10
+                                        , 10
+                                        , OS_TMR_OPT_PERIODIC
+                                        , (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)
 {
     g_horiSpeedSem = OSSemCreate(0);
@@ -325,12 +501,15 @@ void init_speed_module(void)
 
     creat_task_hori_servo_task();
     creat_task_vert_servo_task();
-//    creat_task_test();
 
-    
+    servoCommSoftWareTimInit();
+
     OSTimeDlyHMSM(0u, 0u, 0u, 500u);//等待硬件初始化成功
-    servoSendData(horiMotorType, WriteMotorOneReg(H_MOTOR, H02_CONTR_MODE_SELEC, 0)
-              , WRITE_ONE_REG_FRAME_NUM, lowPriority);
+
+    uint8_t buffer[20] = {0};
+    uint8_t buffer_len = 0;
+    buffer_len = WriteMotorOneReg_buffer(H_MOTOR, H02_CONTR_MODE_SELEC, 0, buffer);
+    servoSendData(horiMotorType, buffer, buffer_len, lowPriority);
 }
 
 
diff --git a/APP/Device/Device_speed/speed_to_servoMotor.h b/APP/Device/Device_speed/speed_to_servoMotor.h
index 476fd45..01153c4 100644
--- a/APP/Device/Device_speed/speed_to_servoMotor.h
+++ b/APP/Device/Device_speed/speed_to_servoMotor.h
@@ -67,8 +67,10 @@ extern ptzServoLinkList g_servoMotorLinkList;
 void ptz_send_speed(char dev, char speed);
 void init_speed_module(void);
 
-void servoLinkListMemPut(linkList *data);
 void servoSendData(uint8_t motor,uint8_t *data, uint8_t dataLen, uint8_t priority);
+void servoHoriLinkListMemPut(uint8_t motorType);
+void stopTimeOut(uint8_t motorType);
+void startTimeOut(uint8_t motorType);
 
 #endif