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

3 files changed, 43 insertions, 166 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index 64b6ac4e0..aaa4b1228 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
@@ -212,53 +212,6 @@ int32_t MillisecReadFromTempSensor(uint32_t TempSensorId,  MSecFptr Callback)
 //typedef  uint32_t (* MSecFptr)(uint32_t deviceID, uint32_t ReadValue);
 uint32_t MotorActivity = 0;
 
-int32_t MillisecMoveScrew(unsigned long Data, int Length, MSecFptr Callback)
-{
-    //==========================
-    /*MSBacklog[MsecLogindex]=Data;
-    Motor_Id[MsecLogindex]=HARDWARE_MOTOR_TYPE__MOTO_SCREW;
-    MSTick[MsecLogindex]=msec_millisecondCounter;
-    MsecLogindex++;
-    if (MsecLogindex>=LOG_SIZE)
-        MsecLogindex = 0;*/
-
-    //==========================
-
-    ScrewMovePending.Callback = Callback;
-    ScrewMovePending.Data = Data;
-    ScrewMovePending.Length = Length;
-    ScrewMovePending.DataRequired = false;
-    if (ScrewMovePending.Active == false)
-    {
-        MotorActivity++;
-        ScrewMovePending.Active = true;
-    }
-
-    return OK;
-}
-
-int32_t MillisecSetScrewSpeed(unsigned long Data, int Length, MSecFptr Callback)
-{
-    //==========================
-/*    MSBacklog[MsecLogindex]=Data;
-    Motor_Id[MsecLogindex]=HARDWARE_MOTOR_TYPE__MOTO_SCREW;
-    MsecLogindex++;
-    if (MsecLogindex>=LOG_SIZE)
-        MsecLogindex = 0;
-  */  //==========================
-
-    ScrewSetMaxSpeedPending.Callback = Callback;
-    ScrewSetMaxSpeedPending.Data = Data;
-    ScrewSetMaxSpeedPending.Length = Length;
-    ScrewSetMaxSpeedPending.DataRequired = false;
-    if (ScrewSetMaxSpeedPending.Active == false)
-    {
-        MotorActivity++;
-        ScrewSetMaxSpeedPending.Active = true;
-    }
-
-    return OK;
-}
 int32_t MillisecSetMotorSpeed(TimerMotors_t MotorId, unsigned long Data, int Length, MSecFptr Callback)
 {
     if (MotorId >= NUM_OF_MOTORS) return -1;
@@ -402,27 +355,6 @@ uint32_t MillisecLoop(uint32_t tick)
                 }
                 MotorActivity--;
             }
-            if (Motor_i == HARDWARE_MOTOR_TYPE__MOTO_SCREW)
-            {
-                if (ScrewSetMaxSpeedPending.Active == true)
-                {
-                    MotorSendFPGARequest(HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewSetMaxSpeedPending.Data,ScrewSetMaxSpeedPending.Length);
-                    MotorActivity--;
-                    ScrewSetMaxSpeedPending.Active = false;
-                    if (ScrewSetMaxSpeedPending.Callback)
-                        ScrewSetMaxSpeedPending.Callback(Motor_i,0);
-                    continue;
-                }
-                else if (ScrewMovePending.Active == true)
-                {
-                    MotorSendFPGARequest(HARDWARE_MOTOR_TYPE__MOTO_SCREW,ScrewMovePending.Data,ScrewMovePending.Length);
-                    MotorActivity--;
-                    ScrewMovePending.Active = false;
-                    if (ScrewMovePending.Callback)
-                        ScrewMovePending.Callback(Motor_i,0);
-                    continue;
-                }
-            }
             if (SpeedSetPending[Motor_i].Active == true)
             {
                 MotorSendFPGARequest((HardwareMotorType)Motor_i,SpeedSetPending[Motor_i].Data,SpeedSetPending[Motor_i].Length);
@@ -492,7 +424,7 @@ uint32_t MillisecLoop(uint32_t tick)
 }
 uint32_t MillisecLowLoop(uint32_t tick)
 {
-    uint8_t Disp_i;
+    uint8_t Motor_i,Disp_i;
     TEMPERATURE_SENSOR_ID_ENUM Sensor_i;
 
     static int temp=0;
@@ -541,6 +473,13 @@ uint32_t MillisecLowLoop(uint32_t tick)
         FPGA_GetAllDispensersValveBusyOCD();
         DrawerFansStatus = Read_Fans_Tacho();
         KeepAliveOneSecondCall();
+        for (Motor_i = 0;Motor_i < NUM_OF_MOTORS;Motor_i++)
+        {
+            if (Motor_i == HARDWARE_MOTOR_TYPE__MOTO_SCREW)
+                continue;  //
+            if (isMotorConfigured(Motor_i))
+                MotorGetStatusFromFPGA(Motor_i);
+        }
     }
     if (OneMinute_Tick)
     {
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/control.c b/Software/Embedded_SW/Embedded/Modules/Control/control.c
index 001cafeb3..a35b804e4 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/control.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/control.c
@@ -304,12 +304,22 @@ void OneMilliSecondControlInterrupt(UArg arg0)
     return ;
 }
 
+uint32_t ControlDevice_i;
+uint32_t ControlLowDevice_i;
+uint32_t GetControlDevice_i(void)
+{
+    return ControlDevice_i;
+}
+uint32_t GetControlLowDevice_i(void)
+{
+    return ControlLowDevice_i;
+}
 uint32_t ControlLoop(uint32_t tick)
 {
     //call all modules control functions
     //test dancers and speed encoders
     //check all callback units (state machine waiting for completion of a change)
-    uint32_t Device_i;
+    //uint32_t ControlDevice_i;
     /*bool Ten_msTick, Hundred_msTick, Onesecond_Tick,Tick98,Tick998;
     Ten_msTick      =   (tick%eTenMillisecond == 0)      ?true:false;
     Hundred_msTick  =   (tick%eHundredMillisecond == 0)  ?true:false;
@@ -320,24 +330,24 @@ uint32_t ControlLoop(uint32_t tick)
     Tick998         =   (tick%eOneSecond == 996)         ?true:false;
 */
     //ROM_IntMasterDisable();
-    for (Device_i = 0; Device_i < MAX_TANGO_CONTROL_DEVICES;Device_i++)
+    for (ControlDevice_i = 0; ControlDevice_i < MAX_TANGO_CONTROL_DEVICES;ControlDevice_i++)
     {
-        if (ControlArray[Device_i].ControlActive)
+        if (ControlArray[ControlDevice_i].ControlActive)
         {
-            ControlBacklog[backlogindex]=Device_i;
+            ControlBacklog[backlogindex]=ControlDevice_i;
             if ( ++backlogindex >= 999)
                 backlogindex = 0;
-            switch (ControlArray[Device_i].ControlTiming)
+            switch (ControlArray[ControlDevice_i].ControlTiming)
             {
                 case eOneMillisecond:
-                    if(ControlArray[Device_i].ControlDataReadPtr)
-                        ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr( ControlArray[Device_i].Parameter1);
+                    if(ControlArray[ControlDevice_i].ControlDataReadPtr)
+                        ControlDatalog[ControlDevice_i] = ControlArray[ControlDevice_i].ControlDataReadPtr( ControlArray[ControlDevice_i].Parameter1);
                     else
-                        LOG_ERROR (Device_i, "Invalid callback ptr");
-                    if(ControlArray[Device_i].ControlCallbackPtr)
-                        ControlArray[Device_i].ControlCallbackPtr(ControlArray[Device_i].IfIndex, ControlDatalog[Device_i]);
+                        LOG_ERROR (ControlDevice_i, "Invalid callback ptr");
+                    if(ControlArray[ControlDevice_i].ControlCallbackPtr)
+                        ControlArray[ControlDevice_i].ControlCallbackPtr(ControlArray[ControlDevice_i].IfIndex, ControlDatalog[ControlDevice_i]);
                     else
-                        LOG_ERROR (Device_i, "Invalid callback ptr");
+                        LOG_ERROR (ControlDevice_i, "Invalid callback ptr");
                     break;
                 default:
                     break;
@@ -353,7 +363,7 @@ uint32_t ControlLowLoop(uint32_t tick)
     //call all modules control functions
     //test dancers and speed encoders
     //check all callback units (state machine waiting for completion of a change)
-    uint32_t Device_i;
+    //uint32_t Device_i;
     bool Ten_msTick, Hundred_msTick, Onesecond_Tick,Tick98,Tick998;
     Ten_msTick      =   (tick%eTenMillisecond == 0)      ?true:false;
     Hundred_msTick  =   (tick%eHundredMillisecond == 0)  ?true:false;
@@ -362,101 +372,27 @@ uint32_t ControlLowLoop(uint32_t tick)
     Tick998         =   (tick%eOneSecond == 996)         ?true:false;
 
     //ROM_IntMasterDisable();
-    for (Device_i = 0; Device_i < MAX_TANGO_CONTROL_DEVICES;Device_i++)
+    for (ControlLowDevice_i = 0; ControlLowDevice_i < MAX_TANGO_CONTROL_DEVICES;ControlLowDevice_i++)
     {
-        if (ControlArray[Device_i].ControlActive)
+        if (ControlArray[ControlLowDevice_i].ControlActive)
         {
-            if (tick == ControlArray[Device_i].StartTick)
+            if (tick == ControlArray[ControlLowDevice_i].StartTick)
                 continue;
-            if (((tick - ControlArray[Device_i].StartTick)%ControlArray[Device_i].ControlTiming)==0) // run the control on exact intervals
+            if (((tick - ControlArray[ControlLowDevice_i].StartTick)%ControlArray[ControlLowDevice_i].ControlTiming)==0) // run the control on exact intervals
             {
-                ControlBacklog[backlogindex]=Device_i;
+                ControlBacklog[backlogindex]=ControlLowDevice_i;
                 if ( ++backlogindex >= 999)
                     backlogindex = 0;
 
-                if(ControlArray[Device_i].ControlDataReadPtr)
-                    ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr( ControlArray[Device_i].Parameter1);
+                if(ControlArray[ControlLowDevice_i].ControlDataReadPtr)
+                    ControlDatalog[ControlLowDevice_i] = ControlArray[ControlLowDevice_i].ControlDataReadPtr( ControlArray[ControlLowDevice_i].Parameter1);
                 else
-                    LOG_ERROR (Device_i, "Invalid callback ptr");
-                if(ControlArray[Device_i].ControlCallbackPtr)
-                    ControlArray[Device_i].ControlCallbackPtr(ControlArray[Device_i].IfIndex, ControlDatalog[Device_i]);
+                    LOG_ERROR (ControlLowDevice_i, "Invalid callback ptr");
+                if(ControlArray[ControlLowDevice_i].ControlCallbackPtr)
+                    ControlArray[ControlLowDevice_i].ControlCallbackPtr(ControlArray[ControlLowDevice_i].IfIndex, ControlDatalog[ControlLowDevice_i]);
                 else
-                    LOG_ERROR (Device_i, "Invalid callback ptr");
+                    LOG_ERROR (ControlLowDevice_i, "Invalid callback ptr");
             }
-            /*
-            ControlBacklog[backlogindex]=Device_i;
-            if ( ++backlogindex >= 999)
-                backlogindex = 0;
-            switch (ControlArray[Device_i].ControlTiming)
-            {
-                case eOneMillisecond:
-                    break;
-                case eTenMillisecond:
-                    if (Ten_msTick)
-                    {
-                        if(ControlArray[Device_i].ControlDataReadPtr)
-                            ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr( ControlArray[Device_i].Parameter1);
-                        else
-                            LOG_ERROR (Device_i, "Invalid callback ptr");
-                        if(ControlArray[Device_i].ControlCallbackPtr)
-                            ControlArray[Device_i].ControlCallbackPtr(ControlArray[Device_i].IfIndex, ControlDatalog[Device_i]);
-                        else
-                            LOG_ERROR (Device_i, "Invalid callback ptr");
-                    }
-                    break;
-                case eHundredMillisecond:
-                    if (Tick98)
-                    {
-                        //there is a need to trigger the data collection from FPGA, from I2C or from ADC 1.5 milliseconds to collect the data
-                        if(ControlArray[Device_i].ControlDataReadPtr)
-                            ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr( ControlArray[Device_i].Parameter1);
-                        else
-                            LOG_ERROR (Device_i, "Invalid callback ptr");
-                    }
-                    if (Hundred_msTick)
-                    {
-                        //there is a need to trigger the data collection from FPGA, from I2C or from ADC 1.5 milliseconds to collect the data
-                        if(ControlArray[Device_i].ControlCallbackPtr)
-                            ControlArray[Device_i].ControlCallbackPtr(ControlArray[Device_i].IfIndex, ControlDatalog[Device_i]);
-                        else
-                            LOG_ERROR (Device_i, "Invalid callback ptr");
-                    }
-                    break;
-                case eOneSecond:
-                    if (Tick998)
-                    {
-                        if(ControlArray[Device_i].ControlDataReadPtr)
-                            ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr( ControlArray[Device_i].Parameter1);
-                        else
-                            LOG_ERROR (Device_i, "Invalid callback ptr");
-                    }
-                    if (Onesecond_Tick)
-                    {
-                        if(ControlArray[Device_i].ControlCallbackPtr)
-                            ControlArray[Device_i].ControlCallbackPtr(ControlArray[Device_i].IfIndex, ControlDatalog[Device_i]);
-                        else
-                            LOG_ERROR (Device_i, "Invalid callback ptr");
-                    }
-                    break;
-                case eNoControl:
-                    memset (&ControlArray[Device_i],0,sizeof(ControlDeviceStruc) );
-                    break;
-                default:
-                    //LOG_ERROR(tick, "Default Timing checked");
-                    if (tick%ControlArray[Device_i].ControlTiming == 0)
-                    {
-                        Report("Default Timing checked",__FILE__,__LINE__,tick,RpWarning,ControlArray[Device_i].ControlTiming,0);
-                        if(ControlArray[Device_i].ControlDataReadPtr)
-                            ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr( ControlArray[Device_i].Parameter1);
-                        else
-                            LOG_ERROR (Device_i, "Invalid callback ptr");
-                        if(ControlArray[Device_i].ControlCallbackPtr)
-                            ControlArray[Device_i].ControlCallbackPtr(ControlArray[Device_i].IfIndex, ControlDatalog[Device_i]);
-                        else
-                            LOG_ERROR (Device_i, "Invalid callback ptr");
-                    }
-                    break;
-            } //switch*/
         } //if control active
     }  //for
     //ROM_IntMasterEnable();
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/control.h b/Software/Embedded_SW/Embedded/Modules/Control/control.h
index b3e574d2a..d50868064 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/control.h
+++ b/Software/Embedded_SW/Embedded/Modules/Control/control.h
@@ -40,6 +40,8 @@ void        ControlStart(void);
 uint32_t    AddControlCallback( ControlCBFunction Callback, CTRL_TIMING_ENUM CtrlFrequency, DataReadCBFunction DriverfPtr, uint16_t IfIndex, uint32_t Parameter1, uint32_t Parameter2 );
 int         RemoveControlCallback(uint32_t deviceId, ControlCBFunction Callback );
 uint32_t    TemplateDataReadCBFunction (uint32_t deviceId, uint32_t Parameter1);
+uint32_t GetControlDevice_i(void);
+uint32_t GetControlLowDevice_i(void);
 
 extern Task_Handle Control_Task_Handle;
 extern uint32_t millisecondCounter;