Version 1.4.6.32; enable variating control timing for thread. fix WHS pid

2 files changed, 34 insertions, 9 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c b/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c
index 166062248..d2379c309 100644
--- a/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c
+++ b/Software/Embedded_SW/Embedded/Modules/General/GeneralHardware.c
@@ -390,7 +390,7 @@ uint32_t GenHWInitCallBackFunction(uint32_t IfIndex, uint32_t BusyFlag)
 uint32_t HWConfiguration(UploadHardwareConfigurationRequest* UploadRequest)
 {
     uint32_t status  = 0;
-    int Dancer_i, Dispenser_i,PID_i,tempheaterId;
+    int Dancer_i, Dispenser_i,PID_i,tempheaterId,DispenserId;
     HardwareBlowerType blowerType = HARDWARE_BLOWER_TYPE__DefaultBlower;
 
     if (UploadRequest == NULL)
@@ -474,8 +474,7 @@ uint32_t HWConfiguration(UploadHardwareConfigurationRequest* UploadRequest)
                 (request->pidcontrols[PID_i]->hardwarepidcontroltype < HARDWARE_PID_CONTROL_TYPE__HeadHeaterZ7))
                 status += IDS_DispenserPidRequestMessage(request->pidcontrols[PID_i]);
             else if (request->pidcontrols[PID_i]->hardwarepidcontroltype == HARDWARE_PID_CONTROL_TYPE__WasteControl)
-                status += WHS_PidRequestMessage(request->pidcontrols[PID_i]);
-
+                WHS_PidRequestMessage(request->pidcontrols[PID_i]);
         }
     }
     else
@@ -488,15 +487,16 @@ uint32_t HWConfiguration(UploadHardwareConfigurationRequest* UploadRequest)
     {
         for (Dispenser_i = 0; Dispenser_i < request->n_dispensers ; Dispenser_i++)
         {
-            Valve_Set(IDS_Id_to_AirValve[Dispenser_i], Atm_MidTank_OFF );   //Atm_MidTank_OFF/ON
-            Valve_Set(IDS_Id_to_CartrideValve[Dispenser_i], Atm_MidTank_OFF );   //Atm_MidTank_OFF/ON
+            DispenserId = request->dispensers[Dispenser_i]->index;
+            Valve_Set(IDS_Id_to_AirValve[DispenserId], Atm_MidTank_OFF );   //Atm_MidTank_OFF/ON
+            Valve_Set(IDS_Id_to_CartrideValve[DispenserId], Atm_MidTank_OFF );   //Atm_MidTank_OFF/ON
             status += DispenserConfigMessage(request->dispensers[Dispenser_i]);
-            if (Check_Dispenser_Type(request->dispensers[Dispenser_i]->index) == LS_STATUS_ERROR)
+            if (Check_Dispenser_Type(DispenserId) == LS_STATUS_ERROR)
             {
-                LOG_ERROR (Dispenser_i, "Dispenser identification failed");
+                LOG_ERROR (DispenserId, "Dispenser identification failed");
                 //return ERROR;
             }
-            status |= Dispenser_EEPROM_Read_Data(Dispenser_i);
+            status |= Dispenser_EEPROM_Read_Data(DispenserId);
         }
         Calculateinit();
     }
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++)
             {