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

1 files changed, 37 insertions, 28 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index cae09cfd8..3e9218392 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
@@ -159,9 +159,11 @@ void OneMilliSecondMillisecInterrupt(UArg arg0)
     ROM_IntMasterEnable();
     return ;
 }
+uint32_t PT100Activity = 0;
 int32_t MillisecWriteToTempSensor(uint32_t TempSensorId, unsigned long Data, int Length, MSecFptr Callback)
 {
     if (TempSensorId >= MAX_TEMPERATURE_SENSOR_ID) return -1;
+    PT100Activity++;
     PT100Data[TempSensorId].Callback = Callback;
     PT100Data[TempSensorId].Data = Data;
     PT100Data[TempSensorId].Length = Length;
@@ -173,6 +175,8 @@ int32_t MillisecWriteToTempSensor(uint32_t TempSensorId, unsigned long Data, int
 int32_t MillisecReadFromTempSensor(uint32_t TempSensorId, unsigned long Data, int Length, MSecFptr Callback)
 {
     if (TempSensorId >= MAX_TEMPERATURE_SENSOR_ID) return -1;
+    PT100Activity++; //read request
+    PT100Activity++;  //get response
     PT100Data[TempSensorId].Callback = Callback;
     PT100Data[TempSensorId].Data = Data;
     PT100Data[TempSensorId].Length = Length;
@@ -291,34 +295,39 @@ uint32_t MillisecLoop(uint32_t tick)
         }
    }
     //FPGA_GetTempSensorBusy();
-    for (Sensor_i = TEMP_SENSE_ANALOG_MIXCHIP_TEMP;Sensor_i < MAX_TEMPERATURE_SENSOR_ID;Sensor_i++)
-    {
-        //if (TempDriverDriverResponse[Sensor_i].Busy == true)
-        //    continue;
-        if (PT100Data[Sensor_i].WaitForData == true) //Read request sent, data is waiting
-        {
-            if (SPIGetFPGAResponse(Sensor_i,&MotorInfo) == OK) //got the data from the FPGA
-            {
-                PT100Data[Sensor_i].WaitForData = false;
-                if (PT100Data[Sensor_i].Callback)
-                    PT100Data[Sensor_i].Callback(Sensor_i,MotorInfo);
-            }
-        }
-        if (PT100Data[Sensor_i].Active == true)
-        {
-            SPISendFPGARequest(Sensor_i,PT100Data[Sensor_i].Data,PT100Data[Sensor_i].Length);
-            PT100Data[Sensor_i].Active = false;
-            if (PT100Data[Sensor_i].DataRequired == true)
-            {
-                PT100Data[Sensor_i].WaitForData = true; // mark the motor for data request next round
-            }
-            else
-            {
-                if (PT100Data[Sensor_i].Callback)
-                    PT100Data[Sensor_i].Callback(Sensor_i,0); // call the callback to report execution
-            }
-        }
-    }
+   if (PT100Activity)
+   {
+       for (Sensor_i = TEMP_SENSE_ANALOG_MIXCHIP_TEMP;Sensor_i < MAX_TEMPERATURE_SENSOR_ID;Sensor_i++)
+       {
+           //if (TempDriverDriverResponse[Sensor_i].Busy == true)
+           //    continue;
+           if (PT100Data[Sensor_i].WaitForData == true) //Read request sent, data is waiting
+           {
+               if (SPIGetFPGAResponse(Sensor_i,&MotorInfo) == OK) //got the data from the FPGA
+               {
+                   PT100Data[Sensor_i].WaitForData = false;
+                   if (PT100Data[Sensor_i].Callback)
+                       PT100Data[Sensor_i].Callback(Sensor_i,MotorInfo);
+               }
+               PT100Activity--;
+           }
+           if (PT100Data[Sensor_i].Active == true)
+           {
+               SPISendFPGARequest(Sensor_i,PT100Data[Sensor_i].Data,PT100Data[Sensor_i].Length);
+               PT100Data[Sensor_i].Active = false;
+               if (PT100Data[Sensor_i].DataRequired == true)
+               {
+                   PT100Data[Sensor_i].WaitForData = true; // mark the motor for data request next round
+               }
+               else
+               {
+                   if (PT100Data[Sensor_i].Callback)
+                       PT100Data[Sensor_i].Callback(Sensor_i,0); // call the callback to report execution
+               }
+               PT100Activity--;
+           }
+       }
+   }
     Dancer_Data[FEEDER_DANCER] = Read_Dancer_Position(FEEDER_DANCER);
     Dancer_Data[POOLER_DANCER] = Read_Dancer_Position(POOLER_DANCER);
     Dancer_Data[WINDER_DANCER] = Read_Dancer_Position(WINDER_DANCER);