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

1 files changed, 105 insertions, 32 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index d9571f3ff..b4ceebbc2 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
@@ -34,14 +34,13 @@
 /******************** STRUCTURES AND ENUMs  ********************************************/
 typedef struct MillisecMotorData
 {
-    bool Active;
     bool WaitForData;
     bool DataRequired;
     MSecFptr Callback;
     unsigned long Data;
     int Length;
+    bool Active;
 }MillisecMotorDataStruc;
-
 typedef enum
 {
     OneMillisec,
@@ -63,9 +62,11 @@ bool      MotorBusy_Data[MOTOR_SPARE1_1] = {true};
 uint32_t  Dancer_Data[NUM_OF_DANCERS] = {0};
 
 MillisecMotorDataStruc MotorData[NUM_OF_MOTORS] = {0};
-
+MillisecMotorDataStruc SpeedSetPending[NUM_OF_MOTORS] = {0};
+MillisecMotorDataStruc PT100Data[MAX_TEMPERATURE_SENSOR_ID] = {0};
 /******************** GLOBAL PARAMETERS  ********************************************/
 Mailbox_Handle          MillisecMsgQ = NULL;
+Mailbox_Handle          MotorsMsgQ[NUM_OF_MOTORS] = {NULL};
 bool                    MillisecRestart;
 static GateMutex_Handle gateMillisecDB;
 
@@ -80,18 +81,20 @@ uint32_t Control_Delta_Position_Pass(uint32_t Current_Read,uint32_t Previous_Rea
 void MillisecInit(void)
 {
     Error_Block eb;
+    int i;
 
-    MillisecMsgQ = Mailbox_create(sizeof(MillisecMessageStruc), 20, NULL,NULL);
+    Error_init(&eb);
+
+    MillisecMsgQ = Mailbox_create(sizeof(MillisecMessageStruc), 20, NULL,&eb);
+    for (i=0;i<NUM_OF_MOTORS;i++)
+    {
+        MotorsMsgQ[i] = Mailbox_create(sizeof(MillisecMotorDataStruc), 6, NULL,&eb);
+    }
 
     MillisecRestart = false;
 
     memset(MillisecDatalog,0,sizeof(uint32_t)*MAX_TANGO_CONTROL_DEVICES);
 
-    int i;
-    for (i=0;i<NUM_OF_MOTORS;i++)
-    {
-        MotorData[i].Active = false;
-    }
     gateMillisecDB = GateMutex_create(NULL, &eb);
     if (gateMillisecDB == NULL)
     {
@@ -149,32 +152,62 @@ void OneMilliSecondMillisecInterrupt(UArg arg0)
     ROM_IntMasterEnable();
     return ;
 }
+int32_t MillisecWriteToTempSensor(uint32_t TempSensorId, unsigned long Data, int Length, MSecFptr Callback)
+{
+    if (TempSensorId >= MAX_TEMPERATURE_SENSOR_ID) return -1;
+    PT100Data[TempSensorId].Callback = Callback;
+    PT100Data[TempSensorId].Data = Data;
+    PT100Data[TempSensorId].Length = Length;
+    PT100Data[TempSensorId].DataRequired = false;
+    PT100Data[TempSensorId].Active = true;
+}
+int32_t MillisecReadFromTempSensor(uint32_t TempSensorId, unsigned long Data, int Length, MSecFptr Callback)
+{
+    if (TempSensorId >= MAX_TEMPERATURE_SENSOR_ID) return -1;
+    PT100Data[TempSensorId].Callback = Callback;
+    PT100Data[TempSensorId].Data = Data;
+    PT100Data[TempSensorId].Length = Length;
+    PT100Data[TempSensorId].DataRequired = true;
+    PT100Data[TempSensorId].Active = true;
+
+}
 //typedef  uint32_t (* MSecFptr)(uint32_t deviceID, uint32_t ReadValue);
+int32_t MillisecSetMotorSpeed(TimerMotors_t MotorId, unsigned long Data, int Length, MSecFptr Callback)
+{
+    if (MotorId >= NUM_OF_MOTORS) return -1;
+    SpeedSetPending[MotorId].Callback = Callback;
+    SpeedSetPending[MotorId].Data = Data;
+    SpeedSetPending[MotorId].Length = Length;
+    SpeedSetPending[MotorId].DataRequired = false;
+    SpeedSetPending[MotorId].Active = true;
+}
 int32_t MillisecWriteToMotor(TimerMotors_t MotorId, unsigned long Data, int Length, MSecFptr Callback)
 {
+    MillisecMotorDataStruc MotorData = {0};
     if (MotorId >= NUM_OF_MOTORS) return -1;
-    if (MotorData[MotorId].Active == true) return -2;
-    MotorData[MotorId].Callback = Callback;
-    MotorData[MotorId].Data = Data;
-    MotorData[MotorId].Length = Length;
-    MotorData[MotorId].Active = true;
-    MotorData[MotorId].DataRequired = false;
-    return OK;
+    MotorData.Callback = Callback;
+    MotorData.Data = Data;
+    MotorData.Length = Length;
+    MotorData.DataRequired = false;
+    if (MotorsMsgQ[MotorId] != NULL)
+        return Mailbox_post(MotorsMsgQ[MotorId] , &MotorData, BIOS_NO_WAIT);
+    else return false;
 }
 int32_t MillisecReadFromMotor(TimerMotors_t MotorId, unsigned long Data, int Length, MSecFptr Callback)
 {
+    MillisecMotorDataStruc MotorData = {0};
     if (MotorId >= NUM_OF_MOTORS) return -1;
-    if (MotorData[MotorId].Active == true) return -2;
-    MotorData[MotorId].Callback = Callback;
-    MotorData[MotorId].Data = Data;
-    MotorData[MotorId].Length = Length;
-    MotorData[MotorId].Active = true;
-    MotorData[MotorId].DataRequired = true;
-    return OK;
+    MotorData.Callback = Callback;
+    MotorData.Data = Data;
+    MotorData.Length = Length;
+    MotorData.DataRequired = true;
+    if (MotorsMsgQ[MotorId] != NULL)
+        return Mailbox_post(MotorsMsgQ[MotorId] , &MotorData, BIOS_NO_WAIT);
+    else return false;
 }
 uint32_t MillisecLoop(uint32_t tick)
 {
-    int Motor_i;
+    int Motor_i,Sensor_i;
     unsigned int MotorInfo = 0;
     //call all modules Millisec functions
     //test dancers and speed encoders
@@ -185,34 +218,74 @@ uint32_t MillisecLoop(uint32_t tick)
     Onesecond_Tick  =   (tick%eOneSecond == 0)           ?true:false;
 
     //gather Motor data from FPGA
+    FPGA_GetBusy();  //load the busy motor information to all motors
     for (Motor_i = 0;Motor_i < NUM_OF_MOTORS;Motor_i++)
     {
+        if (MotorDriverResponse[Motor_i].Busy == true)
+            continue;
         if (MotorData[Motor_i].WaitForData == true) //Read request sent, data is waiting
         {
             if (MotorGetFPGAResponse(Motor_i,&MotorInfo) == OK) //got the data from the FPGA
             {
                 MotorData[Motor_i].WaitForData = false;
-                MotorData[Motor_i].Callback(Motor_i,MotorInfo);
+                if (MotorData[Motor_i].Callback)
+                    MotorData[Motor_i].Callback(Motor_i,MotorInfo);
             }
         }
-        if (MotorData[Motor_i].Active == true) //new data to send
+        if (SpeedSetPending[Motor_i].Active == true)
+        {
+            MotorSendFPGARequest(Motor_i,SpeedSetPending[Motor_i].Data,SpeedSetPending[Motor_i].Length);
+            if (SpeedSetPending[Motor_i].Callback)
+                SpeedSetPending[Motor_i].Callback(Motor_i,0);
+        }
+        else if (Mailbox_pend(MotorsMsgQ[Motor_i] , &MotorData[Motor_i], BIOS_NO_WAIT)==true)
         {
             if (MotorSendFPGARequest(Motor_i,MotorData[Motor_i].Data,MotorData[Motor_i].Length) == OK) //sent the data to the FPGA
             {
-                MotorData[Motor_i].Active = false; //set the Active to false first, because the callback might send a new request immediately
                 if (MotorData[Motor_i].DataRequired == true)
                 {
                     MotorData[Motor_i].WaitForData = true; // mark the motor for data request next round
                 }
                 else
                 {
-                    MotorData[Motor_i].Callback(Motor_i,0); // call the callback to report execution
+                    if (MotorData[Motor_i].Callback)
+                        MotorData[Motor_i].Callback(Motor_i,0); // call the callback to report execution
                 }
             }
         }
     }
     Dancer_Data[FEEDER_DANCER] = Read_Dancer_Position(FEEDER_DANCER);
-
+    if (Hundred_msTick)
+    {
+        //FPGA_GetTempSensorBusy();
+        for (Sensor_i = 0;Sensor_i < NUM_OF_MOTORS;Sensor_i++)
+        {
+            //if (TempDriverDriverResponse[Sensor_i].Busy == true)
+            //    continue;
+            if (PT100Data[Sensor_i].WaitForData == true) //Read request sent, data is waiting
+            {
+                if (MotorGetFPGAResponse(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)
+            {
+                MotorSendFPGARequest(Sensor_i,PT100Data[Sensor_i].Data,PT100Data[Sensor_i].Length);
+                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
+                }
+            }
+        }
+    }
 #ifndef EVALUATION_BOARD
     /* this cannot be done within one millisecond, and not needed
      * instead, check if there is a motor waiting with data to send or read request
@@ -227,9 +300,9 @@ uint32_t MillisecLoop(uint32_t tick)
     MotorSpeed_Data[MOTOR_RDRIVING] = MotorGetSpeedFromFPGA(MOTOR_RDRIVING);
     MotorStatus_Data[MOTOR_RDRIVING] = MotorGetStatusFromFPGA(MOTOR_RDRIVING);*/
     //gather Dancer data from FPGA
-    Dancer_Data[FEEDER_DANCER] = Read_Dancer_Position(FEEDER_DANCER,0);
-    Dancer_Data[POOLER_DANCER] = Read_Dancer_Position(POOLER_DANCER,0);
-    Dancer_Data[WINDER_DANCER] = Read_Dancer_Position(WINDER_DANCER,0);
+    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);
     //gather data from FPGA
     if (Ten_msTick)
     {