Merge branch 'master' of https://twinetfs.visualstudio.com/Tango/_git/Tango

1 files changed, 26 insertions, 1 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
index 72b0439ad..e7a333cc2 100644
--- a/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
+++ b/Software/Embedded_SW/Embedded/Modules/Thread/Thread_print.c
@@ -58,6 +58,10 @@ uint32_t JobCounter = 0;
 MotorControlConfig_t MotorControlConfig[MAX_THREAD_MOTORS_NUM];
 uint32_t DeviceId2Motor[MAX_THREAD_MOTORS_NUM];
 
+int MotorTiming[MAX_THREAD_MOTORS_NUM];
+int MotorTimer[MAX_THREAD_MOTORS_NUM];
+
+
 uint32_t PreviousPosition = 0, CurrentPosition = 0;
 double CurrentRequestedLength = 0.0;
 double CurrentProcessedLength = 0.0;
@@ -506,6 +510,18 @@ uint32_t ThreadControlCBFunction(uint32_t IfIndex, uint32_t ReadValue)
     }
     index = IfIndex&0xFF;
 
+    if (MotorTiming[index]>1)
+    {
+        MotorTimer[index]++;
+        if (MotorTimer[index]>=MotorTiming[index])
+        {
+            MotorTimer[index]=0;
+        }
+        else
+        {
+            return OK;
+        }
+    }
     if(MotorControlConfig[index].m_isEnabled )
     {
         //if (MotorDriverResponse[ThreadMotorIdToMotorId[index]].Busy == true)
@@ -874,6 +890,7 @@ uint32_t ThreadPrepare_TensionCallback (int DancerId, double tension)
         ReportWithPackageFilter(ThreadFilter,"ThreadPrepare_TensionCallback  Prepare Ready",__FILE__,__LINE__,DancerId,RpWarning,PrepareWaitCount,0);
         PrepareReady(Module_Thread,ModuleDone);
     }
+    return OK;
 }
 uint32_t ThreadPrepare_Tension (int DancerId, double tension)
 {
@@ -936,7 +953,7 @@ uint32_t ThreadPrepare_Tension (int DancerId, double tension)
     JobTicket* JobTicket = JobDetails;
     uint32_t status = OK;
     CurrentSegmentId = 0;
-
+    float temp_dt = 0;
     JobCounter = 0;
     TotalProcessedLength = 0.0;
     PoolerTotalProcessedLength = 0.0;
@@ -1035,6 +1052,14 @@ uint32_t ThreadPrepare_Tension (int DancerId, double tension)
             MotorControlConfig[Motor_i].m_mesuredParam = 0;
             MotorControlConfig[Motor_i].m_preError = 0;
             MotorControlConfig[Motor_i].m_SetParam = 0;//need to update SetParams on presegment stage
+
+            temp_dt = MotorControlConfig[Motor_i].m_params.dt/0.001;
+            MotorTiming[Motor_i] = (int)temp_dt;
+            if (MotorTiming[Motor_i])
+            {
+                MotorTimer[Motor_i] = MotorTiming[Motor_i]-1;
+                ReportWithPackageFilter(ThreadFilter,"MotorTiming",__FILE__,Motor_i,MotorTiming[Motor_i],RpWarning,MotorTimer[Motor_i],0);
+            }
 //////////////////////////////////////////////////
             for (i = 0;i < (int)MotorsControl[Motor_i].pvinputfilterfactormode; i++)
             {