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

7 files changed, 257 insertions, 106 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Control/DriverWithCallbackExample.c b/Software/Embedded_SW/Embedded/Modules/Control/DriverWithCallbackExample.c
index 881034e40..9dcb3affb 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/DriverWithCallbackExample.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/DriverWithCallbackExample.c
@@ -26,7 +26,7 @@ uint32_t DriverActionWithCallback (uint32_t deviceId, uint32_t parameter, callba
     ModuleCallback = callback;
     //start control:
 
-    uint32_t ControlId = AddControlCallback( callback,  eOneMillisecond, NULL, (IfTypeNone*0x100+deviceId),deviceId, parameter );
+    uint32_t ControlId = AddControlCallback(NULL, callback,  eOneMillisecond, NULL, (IfTypeNone*0x100+deviceId),deviceId, parameter );
     return ControlId;
 
 }
@@ -38,7 +38,7 @@ uint32_t ControlCallBackFunction(uint32_t deviceId, uint32_t ReadValue)
            //stop this control loop
            RemoveControlCallback(deviceId, ControlCallBackFunction );
            //possibly: start regular control (speed etc)
-           //uint32_t ControlId = AddControlCallback(ControlCBFunction Callback,  eOneMillisecond, NULL, deviceId, Parameter );
+           //uint32_t ControlId = AddControlCallback(NULL,ControlCBFunction Callback,  eOneMillisecond, NULL, deviceId, Parameter );
 
            //call the module callback
            ModuleCallback(deviceId,ReadValue);
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index 641a249ff..d3376c400 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
@@ -38,8 +38,10 @@
 #include <drivers/FPGA/FPGA_Comm.h>
 #include "drivers/Valves/Valve.h"
 #include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h"
+#include "Drivers/I2C_Communication/I2C.h"
 
 #include "modules/thread/thread_ex.h"
+#include "modules/ifs/ifs.h"
 #include "modules/ids/ids_ex.h"
 #include "modules/Diagnostics/Diagnostics.h"
 #include "Modules/General/MachineStatus.h"
@@ -48,13 +50,16 @@
 #include "drivers/Flash_Memory/fatfs/ff.h"
 #include "drivers/Flash_ram/FlashProgram.h"
 
-#include "third_party/fatfs/src/ffconf.h"
 #include "Common/SWUpdate/FileSystem.h"
 
 #include "Communication/Connection.h"
 #include "Modules/General/buttons.h"
-#include "Drivers/I2C_Communication/Head_Card/PT100/PT100_ADC.h"
+#include "Drivers/I2C_Communication/Head_Card/PT100/Head_PT100_ADC.h"
 
+#ifdef Use_WHS_Card
+#include <Drivers/I2C_Communication/WHS_Card/D_ADS122/WHS_PT100_ADC.h>
+#include <Drivers/I2C_Communication/I2C_Task.h>
+#endif
 Task_Handle Millisecond_Task_Handle;
 /******************** Definitions  ********************************************/
 
@@ -82,7 +87,7 @@ typedef struct MillisecMessage{
 }MillisecMessageStruc;
 
 //uint32_t  ADC_Data[MAX_ADC_DEVICES] = {0};
-//uint32_t  TemperatureSensor_Data[MAX_TEMPERATURE_SENSOR_ID] = {0};
+//uint32_t  TemperatureSensor_Data[MAX_MAIN_CARD_TEMPERATURE_SENSOR_ID] = {0};
 //uint32_t  MotorSpeed_Data[NUM_OF_MOTORS] = {0};
 //uint32_t  MotorStatus_Data[NUM_OF_MOTORS] = {0};
 //uint32_t  MotorPosition_Data[NUM_OF_MOTORS] = {0};
@@ -102,7 +107,7 @@ MillisecMotorDataStruc ScrewSetMaxSpeedPending = {0};
 MillisecMotorDataStruc ScrewMovePending = {0};
 MillisecMotorDataStruc MotorData[NUM_OF_MOTORS] = {0};
 MillisecMotorDataStruc SpeedSetPending[NUM_OF_MOTORS] = {0};
-MillisecMotorDataStruc PT100Data[MAX_TEMPERATURE_SENSOR_ID] = {0};
+MillisecMotorDataStruc PT100Data[MAX_MAIN_CARD_TEMP_SENS_ID] = {0};
 /******************** GLOBAL PARAMETERS  ********************************************/
 Mailbox_Handle          MillisecMsgQ = NULL;
 Mailbox_Handle          TenMillisecMsgQ = NULL;
@@ -112,7 +117,6 @@ static GateMutex_Handle gateMillisecDB;
 
 uint32_t        Millisec_timerBase = TIMER1_BASE;        //Timer handle
 /******************** Functions  ********************************************/
-uint32_t Control_Delta_Position_Pass(uint32_t Current_Read,uint32_t Previous_Read);
 void CalculateVOCAlarms(void);
 //**********************************************************************
 /******************** CODE  ********************************************/
@@ -225,13 +229,18 @@ void OneMilliSecondMillisecInterrupt(UArg arg0)
     return ;
 }
 uint32_t PT100Activity = 0;
+int32_t MillisecReadFromI2CTempSensor(uint32_t TempSensorId,  MSecFptr Callback)
+{
+#warning call I2C temeratures
+    return OK;
+}
+
 int32_t MillisecReadFromTempSensor(uint32_t TempSensorId,  MSecFptr Callback)
 {
-    if (TempSensorId >= MAX_TEMPERATURE_SENSOR_ID) return -1;
+    if (TempSensorId >= MAX_MAIN_CARD_TEMP_SENS_ID) return -1;
     PT100Activity++; //read request
     PT100Data[TempSensorId].Callback = Callback;
     PT100Data[TempSensorId].Active = true;
-
     return OK;
 }
 //typedef  uint32_t (* MSecFptr)(uint32_t deviceID, uint32_t ReadValue);
@@ -407,10 +416,11 @@ uint32_t MillisecLoop(uint32_t tick)
         }
    }
     //FPGA_GetTempSensorBusy();
-   //int PT100Busy[MAX_TEMPERATURE_SENSOR_ID] = {0,0,0,0,0,0,0,0,0,0};
+   //int PT100Busy[MAX_MAIN_CARD_TEMPERATURE_SENSOR_ID] = {0,0,0,0,0,0,0,0,0,0};
    if (PT100Activity)
    {
-       for (Sensor_i = 0;Sensor_i < MAX_TEMPERATURE_SENSOR_ID;Sensor_i++)
+
+       for (Sensor_i = 0;Sensor_i < MAX_TANGO_TEMPERATURE_SENSOR_ID;Sensor_i++)
        {
            /*if (TempSensorResponse[Sensor_i].Busy == true)
            {
@@ -419,19 +429,35 @@ uint32_t MillisecLoop(uint32_t tick)
            }*/
            if (PT100Data[Sensor_i].Active == true)
            {
-               #ifndef  Use_Head_Card
+               if(Sensor_i < MAX_MAIN_CARD_TEMP_SENS_ID)
+               {
                    TemperatureSendSensorDummyClk(Sensor_i);
-                #endif
+               }
                PT100Data[Sensor_i].Active = false;
                PT100Data[Sensor_i].WaitForData = true; // mark the motor for data request next round
                break; //  one PT100 activitiy per MS 
            }
            else if (PT100Data[Sensor_i].WaitForData == true) //Read request sent, data is waiting
            {
-               #ifdef  Use_Head_Card
-                   HeadADCPT100_SendReadDataCommand(Sensor_i);
-               #endif
+               if(Sensor_i < MAX_MAIN_CARD_TEMP_SENS_ID)
+               {
                    TemperatureSensorReadFromFPGA_Res(Sensor_i); //got the data from the FPGA
+               }
+               /*else if(Sensor_i < MAX_HEAD_CARD_TEMP_SENS_ID) - move to the function  control_HeadCard_PT100
+               {
+                   //HeadCard_Toggle_PT100(Sensor_i);
+                   //delay??
+                   //HeadADCPT100_SendReadDataCommand(Sensor_i);
+               }
+               else if(Sensor_i < MAX_WHS_CARD_TEMP_SENS_ID)
+               {
+                   //for WHS...
+               }
+               else
+               {
+                   //TBD
+               }*/
+
                PT100Data[Sensor_i].WaitForData = false;
                PT100Data[Sensor_i].SyncRequired = true;
                if (PT100Data[Sensor_i].Callback)
@@ -440,10 +466,23 @@ uint32_t MillisecLoop(uint32_t tick)
            }
            else if (PT100Data[Sensor_i].SyncRequired == true)
            {
-               #ifdef  Use_Head_Card
-                   HeadADCPT100_StartSync(Sensor_i);
-               #endif
-               TemperatureSensorSync(Sensor_i);
+               if(Sensor_i < MAX_MAIN_CARD_TEMP_SENS_ID)
+               {
+                   TemperatureSensorSync(Sensor_i);
+               }
+               /*else if(Sensor_i < MAX_HEAD_CARD_TEMP_SENS_ID)  - NA move to the function  control_HeadCard_PT100
+               {
+                   //HeadADCPT100_StartSync(Sensor_i);
+               }
+               else if(Sensor_i < MAX_WHS_CARD_TEMP_SENS_ID)
+               {
+                   //StartSync WHS...
+               }
+               else
+               {
+                   //TBD...
+               }*/
+
 
                PT100Data[Sensor_i].SyncRequired = false;
                PT100Activity--;
@@ -460,11 +499,11 @@ uint32_t MillisecLoop(uint32_t tick)
 #endif
     return OK;
 }
-int TemperatureSum[MAX_TEMPERATURE_SENSOR_ID];
-int TemperatureMin[MAX_TEMPERATURE_SENSOR_ID];
-int TemperatureMax[MAX_TEMPERATURE_SENSOR_ID];
-int TemperatureCount[MAX_TEMPERATURE_SENSOR_ID];
-int TemperatureCalc[MAX_TEMPERATURE_SENSOR_ID];
+int TemperatureSum[MAX_HEAD_CARD_TEMP_SENS_ID];
+int TemperatureMin[MAX_HEAD_CARD_TEMP_SENS_ID];
+int TemperatureMax[MAX_HEAD_CARD_TEMP_SENS_ID];
+int TemperatureCount[MAX_HEAD_CARD_TEMP_SENS_ID];
+int TemperatureCalc[MAX_HEAD_CARD_TEMP_SENS_ID];
 void MillisecUpdateTemperatures (TEMPERATURE_SENSOR_ID_ENUM SensorId,int temperature)
 {
    //if(TemperatureCount[SensorId]++>=10)
@@ -489,29 +528,79 @@ int MillisecCalculateTemperatures (TEMPERATURE_SENSOR_ID_ENUM SensorId)
 }
 int MillisecGetTemperatures (TEMPERATURE_SENSOR_ID_ENUM SensorId)
 {
+    if (SensorId > WHS_PT100_4_0X82_1)
+        return 0;
     return TemperatureCalc[SensorId];
 }
-bool RapidPressureRead = false;
+bool RapidPressureRead = true;
+float PressureSum[MAX_SYSTEM_DISPENSERS];
+float PressureMin[MAX_SYSTEM_DISPENSERS];
+float PressureMax[MAX_SYSTEM_DISPENSERS];
+float PressureCount[MAX_SYSTEM_DISPENSERS];
+float PressureCalc[MAX_SYSTEM_DISPENSERS];
+void MillisecUpdatePressures (int SensorId,float Pressure)
+{
+   //if(PressureCount[SensorId]++>=10)
+   //    PressureCount[SensorId] = 0;
+    PressureCount[SensorId]++;
+    if (PressureMax[SensorId]<Pressure) PressureMax[SensorId]=Pressure;
+    if (PressureMin[SensorId]>Pressure) PressureMin[SensorId]=Pressure;
+    PressureSum[SensorId]+=Pressure;
+}
+float MillisecCalculatePressures (int SensorId)
+{
+    float calc = 0;
+    PressureSum[SensorId]-=PressureMax[SensorId];
+    PressureSum[SensorId]-=PressureMin[SensorId];
+    calc = PressureSum[SensorId] / (PressureCount[SensorId]-2);
+
+    PressureSum[SensorId] = 0;
+    PressureCount[SensorId] = 0;
+    PressureMin[SensorId] = 30000;
+    PressureMax[SensorId] = -30000;
+    return calc;
+}
+float MillisecGetPressures (int SensorId)
+{
+    if (SensorId > MAX_SYSTEM_DISPENSERS)
+        return 0;
+    return PressureCalc[SensorId];
+}
+
 void setRapidPressureRead(bool value)
 {
     RapidPressureRead = value;
+    if (GetDiagnosticMode() == Diagnostic_Extreme_Mode)
+        RapidPressureRead = true;
 }
 
 uint16_t PumpCounter = 0;
 uint32_t MillisecLowLoop(uint32_t tick)
 {
-    uint8_t Motor_i,Disp_i,Heater_i,temp;
+    uint8_t Motor_i,Disp_i,temp;
     TEMPERATURE_SENSOR_ID_ENUM Sensor_i;
 
     //static int temp=0;
     //call all modules Millisec functions
     //test dancers and speed encoders
     //check all callback units (state machine waiting for completion of a change)
-    bool Ten_msTick, Hundred_msTick , m90msecTick, Onesecond_Tick,O900Millisecond_Tick,OneMinute_Tick,OneHourTick;
+    bool Ten_msTick, Fifty_msTick, Hundred_msTick , m20msecTick,m70msecTick,m90msecTick, Onesecond_Tick,OneMinute_Tick,OneHourTick,Gradient_Tick;
+    bool O100Millisecond_Tick,O200Millisecond_Tick,O400Millisecond_Tick,O500Millisecond_Tick,O600Millisecond_Tick;
+    //bool O100Millisecond_Tick,O200Millisecond_Tick,O400Millisecond_Tick,O500Millisecond_Tick,O600Millisecond_Tick,O800Millisecond_Tick,O900Millisecond_Tick;
     Ten_msTick      =   (tick%eTenMillisecond == 0)      ?true:false;
+    Fifty_msTick      =   (tick%eHundredMillisecond == 40)      ?true:false; //eFiftyMillisecond
     Hundred_msTick  =   (tick%eHundredMillisecond == 0)  ?true:false;
+    m20msecTick  =   (tick%eHundredMillisecond == 20)  ?true:false;
+    m70msecTick  =   (tick%eHundredMillisecond == 70)  ?true:false;
     m90msecTick  =   (tick%eHundredMillisecond == 90)  ?true:false;
-    O900Millisecond_Tick  =   (tick%eOneSecond == 900)           ?true:false;
+    O100Millisecond_Tick  =   (tick%eOneSecond == 100)           ?true:false;
+    O200Millisecond_Tick  =   (tick%eOneSecond == 200)           ?true:false;
+    O400Millisecond_Tick  =   (tick%eOneSecond == 400)           ?true:false;
+    O500Millisecond_Tick  =   (tick%eOneSecond == 500)           ?true:false;
+    O600Millisecond_Tick  =   (tick%eOneSecond == 600)           ?true:false;
+    //O800Millisecond_Tick  =   (tick%eOneSecond == 800)           ?true:false;
+    //O900Millisecond_Tick  =   (tick%eOneSecond == 900)           ?true:false;
+    Gradient_Tick  =   (tick%400 == 0)           ?true:false;
     Onesecond_Tick  =   (tick%eOneSecond == 0)           ?true:false;
     OneMinute_Tick  =   (tick%eOneMinute == 0)           ?true:false;
     OneHourTick  =   (tick%eOneHour == 0)           ?true:false;
@@ -524,13 +613,45 @@ uint32_t MillisecLowLoop(uint32_t tick)
     {
         //Speed_Data = Calculate_Speed_Sensor_Velocity();
         //MillisecReadFromTempSensor(Sensor_Read, NULL);
-        //if (Sensor_Read++ >= MAX_TEMPERATURE_SENSOR_ID) Sensor_Read = 0;
+        //if (Sensor_Read++ >= MAX_MAIN_CARD_TEMPERATURE_SENSOR_ID) Sensor_Read = 0;
         if(Machine_Idle_Mode == true)
             Machine_Idle_Breathing_Led();
+        Trigger_HeaterWriting();
+
     }
+#ifdef Use_Head_Card
+    if(Fifty_msTick)
+    {
+        Trigger_PT100_Read();//call every 50mSec (minimum delay 30mSec)
+        //Set_HeadCard_PT100();//call every 50mSec (minimum delay 30mSec)
+    }
+#endif
+/*#ifdef Use_Head_Card
+    if (m20msecTick) //read odd PT1000
+    {
+        for (Sensor_i = HEAD_PT100_ZONE_1_0X80_0;Sensor_i < HEAD_PT100_ZONE_2_0X80_1;Sensor_i++)
+        {
+            //MillisecReadFromI2CTempSensor(Sensor_i, NULL);
+        }
+        //Set_HeadCard_PT100();
+#warning call chip select replace in I2C
+    }
+    if (m70msecTick) //read odd PT1000
+    {
+        for (Sensor_i = HEAD_PT100_ZONE_2_0X80_1;Sensor_i < HEAD_PT100_RESERVE_0X8E_1;Sensor_i++)
+        {
+            //MillisecReadFromI2CTempSensor(Sensor_i, NULL);
+        }
+        //Set_HeadCard_PT100();
+    }
+#endif */
     if (m90msecTick)
     {
-        for (Sensor_i = 0;Sensor_i < MAX_TEMPERATURE_SENSOR_ID;Sensor_i++)
+#ifdef Use_Head_Card
+        for (Sensor_i = TEMP_SENSE_ANALOG_DRYER_TEMP1;Sensor_i < MAX_HEAD_CARD_TEMP_SENS_ID;Sensor_i++)
+#else
+        for (Sensor_i = 0;Sensor_i < MAX_MAIN_CARD_TEMP_SENS_ID;Sensor_i++)
+#endif
         {
             MillisecReadFromTempSensor(Sensor_i, NULL);
         }
@@ -540,7 +661,14 @@ uint32_t MillisecLowLoop(uint32_t tick)
         Speed_Data = Calculate_Speed_Sensor_Velocity();
         Read_Buttons_Reg();
         //Ink_Cart_Led();
-        for (Sensor_i = 0;Sensor_i < MAX_TEMPERATURE_SENSOR_ID;Sensor_i++)
+        ///////////////////////////////////////////////////////////////////
+
+
+#ifdef Use_Head_Card
+        for (Sensor_i = TEMP_SENSE_ANALOG_DRYER_TEMP1;Sensor_i < MAX_HEAD_CARD_TEMP_SENS_ID;Sensor_i++)
+#else
+        for (Sensor_i = 0;Sensor_i < MAX_MAIN_CARD_TEMP_SENS_ID;Sensor_i++)
+#endif
         {
             MillisecUpdateTemperatures (Sensor_i,TemperatureSensorRead(Sensor_i));
         }
@@ -556,34 +684,31 @@ uint32_t MillisecLowLoop(uint32_t tick)
         {
             for (Disp_i = 0;Disp_i < MAX_SYSTEM_DISPENSERS;Disp_i++)
             {
-                CalculateDispenserPressure(Disp_i);
-                //Read_MidTank_Pressure_Sensor(Disp_i);
+                MillisecUpdatePressures(Disp_i, CalculateDispenserPressure(Disp_i));
             }
             ADC_TriggerCollection();
         }
-        DispensersCollectionCall();
 
-    }
+#ifdef Use_Head_Card
+    //Head_Read_IO_Reg(0x46, HIGH);//READ HEAD CARD LS
+#endif
 
-    if ((O900Millisecond_Tick)&&(RapidPressureRead == false))
+    }
+    if (Gradient_Tick)
+        DispensersCollectionCall();
+    if (O100Millisecond_Tick)
     {
-        ADC_TriggerCollection();
+        Trigger_Heater_Current_Read();
     }
-    if (Onesecond_Tick)
+    if (O200Millisecond_Tick)
     {
-        if (RapidPressureRead == false)
-        {
-            for (Disp_i = 0;Disp_i < MAX_SYSTEM_DISPENSERS;Disp_i++)
-            {
-                CalculateDispenserPressure(Disp_i);
-                //Read_MidTank_Pressure_Sensor(Disp_i);
-            }
-        }
         FPGA_GetAllDispensersValveBusyOCD();
         temp = Read_Fans_Tacho();
         DrawerFansStatus = temp & 0x1F;
         SystemFansStatus = temp & 0xE0;
-        //KeepAliveOneSecondCall();
+    }
+    if (O400Millisecond_Tick)
+    {
         for (Motor_i = 0;Motor_i < NUM_OF_MOTORS;Motor_i++)
         {
             if (Motor_i == HARDWARE_MOTOR_TYPE__MOTO_SCREW)
@@ -591,25 +716,54 @@ uint32_t MillisecLowLoop(uint32_t tick)
             if (isMotorConfigured(Motor_i))
                 MotorGetStatusFromFPGA(Motor_i);
         }
-        if (!(OneMinute_Tick))
+    }
+    if ((O500Millisecond_Tick)&&(RapidPressureRead == false))
+    {
+        ADC_TriggerCollection();
+    }
+    if (O600Millisecond_Tick)
+    {
+        if (RapidPressureRead == false)
         {
-            for (Heater_i = 0;Heater_i < NUM_OF_CURRENT_HEATERS;Heater_i++)
+            for (Disp_i = 0;Disp_i < MAX_SYSTEM_DISPENSERS;Disp_i++)
             {
-                //Read_Heaters_Current(Heater_i);
+                CalculateDispenserPressure(Disp_i);
             }
         }
-        for (Sensor_i = 0;Sensor_i < MAX_TEMPERATURE_SENSOR_ID;Sensor_i++)
+    }
+    if (Onesecond_Tick)
+    {
+        char Lenstr[160];
+        static int Counter = 0;
+        MachineUpdateResponseFunc();
+        KeepAliveOneSecondCall();
+#ifdef Use_Head_Card
+        TemperatureListString(Lenstr);
+        ReportWithPackageFilter(ThreadFilter,Lenstr,__FILE__,__LINE__,(int)Counter++,RpWarning,(int) msec_millisecondCounter,0);
+//#ifdef Use_Head_Card
+        for (Sensor_i = TEMP_SENSE_ANALOG_DRYER_TEMP1;Sensor_i < MAX_HEAD_CARD_TEMP_SENS_ID;Sensor_i++)
+#else
+        for (Sensor_i = 0;Sensor_i < MAX_MAIN_CARD_TEMP_SENS_ID;Sensor_i++)
+#endif
         {
             TemperatureCalc[Sensor_i] =  MillisecCalculateTemperatures ( Sensor_i);
         }
+        if (RapidPressureRead == true)
+        {
+            for (Disp_i = 0;Disp_i < MAX_SYSTEM_DISPENSERS;Disp_i++)
+            {
+                PressureCalc[Disp_i] = MillisecCalculatePressures(Disp_i);
+            }
+        }
     }
     if (OneMinute_Tick)
     {
-        for (Disp_i = 0;Disp_i < MAX_SYSTEM_DISPENSERS;Disp_i++)
+        Trigger_MidTank_Pressure_Read();
+        /*for (Disp_i = 0;Disp_i < MAX_SYSTEM_DISPENSERS;Disp_i++)
         {
             Read_MidTank_Pressure_Sensor(Disp_i);
-        }
-        MachineUpdateResponseFunc();
+        }*/
+//        MachineUpdateResponseFunc();
         /*        for (Motor_i = 0;Motor_i < NUM_OF_MOTORS;Motor_i++)
         {
             if (Motor_i == HARDWARE_MOTOR_TYPE__MOTO_SCREW)
@@ -630,6 +784,7 @@ uint32_t MillisecLowLoop(uint32_t tick)
             PumpActivation(900);
             PumpCounter = 0;
         }
+        MidTankReading();
     }
     //ROM_IntMasterEnable();
 
@@ -705,7 +860,7 @@ void MillisecLowTask(UArg arg0, UArg arg1)
 */
 /*uint32_t getTemperatureSensorData(int SensorId)
 {
-    assert (SensorId < MAX_TEMPERATURE_SENSOR_ID);
+    assert (SensorId < MAX_MAIN_CARD_TEMPERATURE_SENSOR_ID);
     return TemperatureSensor_Data[SensorId];
 }*/
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h
index 4edc1a8ed..9335a6d08 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.h
@@ -34,6 +34,7 @@ uint32_t getDrawerFansStatus(void);
 uint32_t getSystemFansStatus(void);
 uint8_t getGasReading(void);
 int MillisecGetTemperatures (TEMPERATURE_SENSOR_ID_ENUM SensorId);
+float MillisecGetPressures (int SensorId);
 void setRapidPressureRead(bool value);
 
 
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c b/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c
index 6c1b647be..3d1d353c2 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.c
@@ -3,41 +3,6 @@
 
 float PIDAlgorithmCalculation(float _setPoint,float _mesuredParam , PID_Config_Params *params, float *_pre_error, float *_integral)
 {
-	float error;
-	float derivative;
-	float output;
-
-	//Calculate P,I,D
-	error = _setPoint - _mesuredParam;
-
-	//In case of error too small then stop integration
-	if(fabs(error) > params->epsilon)
-	{
-		*_integral = *_integral + error*params->dt;
-	}
-
-	derivative = (error - *_pre_error)/params->dt;
-
-	output = params->Kp*error + params->Ki**_integral + params->Kd*derivative;
-
-	//Saturation Filter
-	if(output > params->MAX)
-	{
-		output = params->MAX;
-	}
-	else if(output < params->MIN)
-	{
-		output = params->MIN;
-	}
-
-	//Update error
-	*_pre_error = error;
-
-	return output;
-}
-
-float AdvancedPIDAlgorithmCalculation(float _setPoint,float _mesuredParam , PID_Config_Params *params, float *_pre_error, float *_integral)
-{
     float error;
     float derivative;
     float output;
@@ -70,7 +35,7 @@ float AdvancedPIDAlgorithmCalculation(float _setPoint,float _mesuredParam , PID_
 
     return output;
 }
-
+/*
 float TestPIDAlgorithmCalculation(float _setPoint,float _mesuredParam , PID_Config_Params *params, float *_pre_error, float *_integral)
 {
     float error;
@@ -81,14 +46,14 @@ float TestPIDAlgorithmCalculation(float _setPoint,float _mesuredParam , PID_Conf
     error = _setPoint - _mesuredParam;
     //ITerm+= (ki * error);
     *_integral = *_integral + (error*params->Ki);
-    /*  if(ITerm > outMax)
+    /`*  if(ITerm > outMax)
       {
           ITerm= outMax;
       }
       else if(ITerm < outMin)
       {
           ITerm= outMin;
-      } */
+      } *`/
     if(*_integral > params->MAX)
     {
         *_integral = params->MAX;
@@ -100,7 +65,7 @@ float TestPIDAlgorithmCalculation(float _setPoint,float _mesuredParam , PID_Conf
     //  double dInput = (input - lastInput);
     derivative = error - *_pre_error;
 
-      /*Compute PID Output*/
+      /`*Compute PID Output*`/
     //  double output = kp * error + ITerm- kd * dInput;
     output = params->Kp*error/params->ProportionalErrorMultiplier + *_integral/params->IntegralErrorMultiplier + params->Kd*derivative;
 
@@ -119,3 +84,4 @@ float TestPIDAlgorithmCalculation(float _setPoint,float _mesuredParam , PID_Conf
 
     return output;
 }
+*/
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.h b/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.h
index 4792be9ec..4f94f3612 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.h
+++ b/Software/Embedded_SW/Embedded/Modules/Control/PIDAlgo.h
@@ -17,6 +17,5 @@ typedef struct
 
 }PID_Config_Params;
 float PIDAlgorithmCalculation(float _setPoint,float _mesuredParam , PID_Config_Params *params, float *_pre_error, float *_integral);
-float AdvancedPIDAlgorithmCalculation(float _setPoint,float _mesuredParam , PID_Config_Params *params, float *_pre_error, float *_integral);
 
 #endif /* MODULES_PIDALGO_H_ */
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/control.c b/Software/Embedded_SW/Embedded/Modules/Control/control.c
index 80afb7ac6..e093815a9 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/control.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/control.c
@@ -45,19 +45,29 @@
 //by recieved esign flow of the user from the UI
 ///////////////////////////////////////////////////////////////////////////////////////////
 #include "include.h"
-#include "Modules/General/GeneralHardware.h"
 
 #include <driverlib/timer.h>
 #include <inc/hw_ints.h>
 
+#include  <PMR/Diagnostics/EventType.pb-c.h>
+
 #include "drivers/adc_sampling/adc.h"
+#include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h"
+#include "drivers/FPGA/FPGA_SPI_Comm.h"
+
 #include "Modules/General/buttons.h"
+#include "Modules/General/GeneralHardware.h"
+#include "Modules/AlarmHandling/AlarmHandling.h"
+
+#include "StateMachines/Printing/PrintingSTM.h"
 
 #include "control.h"
 #include "MillisecTask.h"
-#include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h"
+
+
+
 /******************** Definitions  ********************************************/
-#define MAX_TANGO_CONTROL_DEVICES 100
+#define MAX_TANGO_CONTROL_DEVICES 120
 /******************** STRUCTURES AND ENUMs  ********************************************/
 
 
@@ -71,6 +81,7 @@ typedef struct
   DataReadCBFunction ControlDataReadPtr;
   ControlCBFunction ControlCallbackPtr;
   uint32_t ControlTiming;
+  char *Name;
 }ControlDeviceStruc;
 
 typedef enum
@@ -132,6 +143,7 @@ void ControlInit(void)
         ControlArray[Device_i].ControlCallbackPtr = NULL;
         ControlArray[Device_i].ControlDataReadPtr = NULL;
         ControlArray[Device_i].ControlTiming = eNoControl;
+        ControlArray[Device_i].Name = NULL;
     }
     gateControlDB = GateMutex_create(NULL, &eb);
     if (gateControlDB == NULL)
@@ -166,7 +178,23 @@ uint32_t ControlActivityLed( uint32_t Parameter1)
         COMM_RED_LED_ON;
         ACTIVITY_RED_LED_OFF; // Heaters indication - all the Heaters OFF
         if(FPGA_WD_Occurred == true)
+        {
+            FPGA_WD_Occurred = false;
+            AlarmHandlingSetAlarm(EVENT_TYPE__FPGA_WATCHDOG_ACTIVATED,true);
+            if (JobIsActive())
+            {
+                JobEndReason = JOB_MOTOR_ALARM;
+                SendJobProgress(0.0,0,false, "Hardware Failure Error");
+                AbortJob("FPGA Watchdog Error");
+            }
+            ReportWithPackageFilter(FPGAFilter, "FPGA Watchdog Error",__FILE__,__LINE__,0,RpError, 0,0);
+
             ACTIVITY_GREEN_LED_ON;
+            FPGA_SetMotorsInit();
+            Motor_ReconfigAllMotors();
+        }
+		else
+            ACTIVITY_GREEN_LED_OFF;
 
         if(power.color == fastBILNK)
             Pannel_Leds(POWER_ON_OFF,MODE_OFF);
@@ -298,7 +326,7 @@ void ControlStart(void)
         TimerEnable(Control_timerBase, TIMER_A);
 
         ADCAcquireStart(0,1);
-        AddControlCallback( ControlEmptyCBFunction,  eHundredMillisecond, ControlActivityLed,0, 0, 0 );
+        AddControlCallback("ControlActivityLed", ControlEmptyCBFunction,  eHundredMillisecond, ControlActivityLed,0, 0, 0 );
 
         SysCtlDelay(12000000);
         MillisecStart();
@@ -313,7 +341,7 @@ void ControlStart(void)
  * both these callbacks can be removed. if a new call is arriving, it invalidates the previous one (no dual control or data)
  *
  ***************************************************************************************************************************************************/
-uint32_t  AddControlCallback( ControlCBFunction Callback, uint32_t CtrlFrequency, DataReadCBFunction DriverfPtr, uint16_t IfIndex, uint32_t Parameter1, uint32_t Parameter2 )
+uint32_t  AddControlCallback(char* Name, ControlCBFunction Callback, uint32_t CtrlFrequency, DataReadCBFunction DriverfPtr, uint16_t IfIndex, uint32_t Parameter1, uint32_t Parameter2 )
 {
     assert(Callback);
     assert(DriverfPtr);
@@ -364,6 +392,7 @@ uint32_t  AddControlCallback( ControlCBFunction Callback, uint32_t CtrlFrequency
     ControlArray[deviceId].Parameter1 = Parameter1;
     ControlArray[deviceId].IfIndex = IfIndex;
     ControlArray[deviceId].StartTick = millisecondCounter;
+    ControlArray[deviceId].Name = Name;
     GateMutex_leave(gateControlDB, key);
     //LOG_ERROR(deviceId, "Add Callback");
 
@@ -377,11 +406,11 @@ int SafeRemoveHighControlCallback(uint32_t deviceId , ControlCBFunction Callback
         Report("Fixing Remove control ",__FILE__,__LINE__,(int)GetControlDevice_i(),RpWarning,(int)deviceId,0);
         if (RemoveControlCallback(GetControlDevice_i(),Callback)==OK)
         {
-            Report("Remove control callback fixed",__FILE__,__LINE__,(int)GetControlDevice_i(),RpWarning,(int)deviceId,0);
+            Report("Remove control callback fixed",ControlArray[GetControlDevice_i()].Name,__LINE__,(int)GetControlDevice_i(),RpWarning,(int)deviceId,0);
         }
         else
         {
-            Report("Remove control callback failed",__FILE__,__LINE__,(int)GetControlDevice_i(),RpWarning,(int)deviceId,0);
+            Report("Remove control callback failed",ControlArray[GetControlDevice_i()].Name,__LINE__,(int)GetControlDevice_i(),RpWarning,(int)deviceId,0);
             return ERROR;
         }
     }
@@ -395,11 +424,11 @@ int  SafeRemoveControlCallback(uint32_t deviceId , ControlCBFunction Callback)
         Report("Fixing Remove control ",__FILE__,__LINE__,(int)GetControlLowDevice_i(),RpWarning,(int)deviceId,0);
         if (RemoveControlCallback(GetControlLowDevice_i(),Callback)==OK)
         {
-            Report("Remove control callback fixed",__FILE__,__LINE__,(int)GetControlLowDevice_i(),RpWarning,(int)deviceId,0);
+            Report("Remove control callback fixed",ControlArray[GetControlDevice_i()].Name,__LINE__,(int)GetControlLowDevice_i(),RpWarning,(int)deviceId,0);
         }
         else
         {
-            Report("Remove control callback failed",__FILE__,__LINE__,(int)GetControlLowDevice_i(),RpWarning,(int)deviceId,0);
+            Report("Remove control callback failed",ControlArray[GetControlDevice_i()].Name,__LINE__,(int)GetControlLowDevice_i(),RpWarning,(int)deviceId,0);
             return ERROR;
         }
     }
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/control.h b/Software/Embedded_SW/Embedded/Modules/Control/control.h
index 450646a68..78e24f495 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/control.h
+++ b/Software/Embedded_SW/Embedded/Modules/Control/control.h
@@ -17,6 +17,7 @@ typedef enum {
     eNoControl = 0,
     eOneMillisecond = 1,
     eTenMillisecond = 10,
+    eFiftyMillisecond = 50,
     eHundredMillisecond = 100,
     eOneSecond = 1000,
     eOneMinute = 60000,
@@ -38,7 +39,7 @@ typedef enum
 void        ControlInit(void);
 void        ControlStop(void);
 void        ControlStart(void);
-uint32_t    AddControlCallback( ControlCBFunction Callback, uint32_t CtrlFrequency, DataReadCBFunction DriverfPtr, uint16_t IfIndex, uint32_t Parameter1, uint32_t Parameter2 );
+uint32_t    AddControlCallback(char *Name, ControlCBFunction Callback, uint32_t CtrlFrequency, DataReadCBFunction DriverfPtr, uint16_t IfIndex, uint32_t Parameter1, uint32_t Parameter2 );
 int         RemoveControlCallback(uint32_t deviceId, ControlCBFunction Callback );
 //The safe remove command can be used ONLY when called from the control callback itself
 int         SafeRemoveControlCallback(uint32_t deviceId , ControlCBFunction Callback);