conflict merge

2 files changed, 72 insertions, 16 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index b9079919c..67de0271f 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
@@ -16,6 +16,7 @@
 ///////////////////////////////////////////////////////////////////////////////////////////
 #include "include.h"
 #include "Modules/General/GeneralHardware.h"
+#include "Modules/AlarmHandling/AlarmHandling.h"
 
 #include "MillisecTask.h"
 #include <driverlib/timer.h>
@@ -29,11 +30,9 @@
 #include "drivers/Heater/TemperatureSensor.h"
 #include "drivers/FPGA/FPGA_SPI_Comm.h"
 #include "drivers/FPGA/FPGA.h"
-#include "drivers/Valves/Valve.h"
 
 #include "modules/thread/thread_ex.h"
-#include "Drivers/Peripheral_GPIO/GPIO.h"
-#include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h"
+#include "modules/ids/ids_ex.h"
 
 Task_Handle Millisecond_Task_Handle;
 /******************** Definitions  ********************************************/
@@ -137,7 +136,7 @@ void MillisecStart(void)
     ADCAcquireStart(0,1);
 }
 
-
+uint32_t msec_millisecondCounter = 0;
 void OneMilliSecondMillisecInterrupt(UArg arg0)
 {
     MillisecMessageStruc Message;
@@ -151,7 +150,7 @@ void OneMilliSecondMillisecInterrupt(UArg arg0)
 
     //send message to the Millisec task
     Message.messageId = OneMillisec;
-    Message.tick = UsersysTickGet();
+    Message.tick = msec_millisecondCounter++;;
     Message.msglen = sizeof(MillisecMessageStruc);
     if (MillisecMsgQ != NULL)
         Mailbox_post(MillisecMsgQ , &Message, BIOS_NO_WAIT);
@@ -229,17 +228,18 @@ uint32_t MillisecLoop(uint32_t tick)
     //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, Onesecond_Tick;
+    bool Ten_msTick, Hundred_msTick, Onesecond_Tick,O900Millisecond_Tick;
     Ten_msTick      =   (tick%eTenMilliSecond == 0)      ?true:false;
     Hundred_msTick  =   (tick%eHundredMillisecond == 0)  ?true:false;
+    O900Millisecond_Tick  =   (tick%eOneSecond == 900)           ?true:false;
     Onesecond_Tick  =   (tick%eOneSecond == 0)           ?true:false;
     //gather Motor data from FPGA
 #ifndef EVALUATION_BOARD
         FPGA_GetBusy();  //load the busy motor information to all motors
         FPGA_Read_limit_Switches_Registers();
         //Read_FPGA_GPI_Rgisters();//FPGA_Read_limit_Switches();
-        //FPGA_GetDispenserValveBusyOCD();
 #endif
+
         temp += MotorActivity;
    if (MotorActivity)
    {
@@ -341,15 +341,20 @@ uint32_t MillisecLoop(uint32_t tick)
     Dancer_Data[WINDER_DANCER] = Read_Dancer_Position(WINDER_DANCER);
     if (Hundred_msTick)
     {
-        ACTIVITY_RED_LED_CHANGE_MODE;
         for (Sensor_i = 0;Sensor_i < MAX_TEMPERATURE_SENSOR_ID;Sensor_i++)
             MillisecReadFromTempSensor(Sensor_i, NULL);
+        AlarmHandler_SendDiagnostics();
     }
 
-    if (Onesecond_Tick)
+    if (O900Millisecond_Tick)
     {
         ADC_TriggerCollection();
     }
+    if (Onesecond_Tick)
+    {
+        for (Sensor_i = 0;Sensor_i < MAX_SYSTEM_DISPENSERS;Sensor_i++)
+            CalculateDispenserPressure(Sensor_i);
+    }
 
     /*        OLD CODE
     MotorStatus_Data[MOTOR_DRYER_DRIVING] = MotorGetStatusFromFPGA(MOTOR_DRYER_DRIVING);
diff --git a/Software/Embedded_SW/Embedded/Modules/Control/control.c b/Software/Embedded_SW/Embedded/Modules/Control/control.c
index a0ba50078..8e999a668 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/control.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/control.c
@@ -51,6 +51,7 @@
 #include <inc/hw_ints.h>
 
 #include "drivers/adc_sampling/adc.h"
+#include "Drivers/Peripheral_GPIO/GPIO.h"
 #include "control.h"
 #include "MillisecTask.h"
 /******************** Definitions  ********************************************/
@@ -93,6 +94,8 @@ static GateMutex_Handle gateControlDB;
 Task_Handle Control_Task_Handle;
 ControlDeviceStruc      ControlArray[MAX_TANGO_CONTROL_DEVICES];
 uint32_t                ControlDatalog[MAX_TANGO_CONTROL_DEVICES];
+uint16_t ControlBacklog[1000]={0};
+uint16_t backlogindex = 0;
 uint32_t        Control_timerBase = TIMER0_BASE;        //Timer handle
 /******************** Functions  ********************************************/
 void OneMilliSecondFunction(UArg arg0);
@@ -148,6 +151,25 @@ void ControlStop(void)
     ControlRestart = false;
     ADCAcquireStop();
 }
+uint32_t ControlActivityLed( uint32_t Parameter1, uint32_t Parameter2)
+{
+    static bool flag = false;
+    if (flag==true)
+    {
+        COMM_RED_LED_ON;
+        flag = false;
+    }
+    else
+    {
+        COMM_RED_LED_OFF;
+        flag = true;
+    }
+return OK;
+}
+uint32_t ControlEmptyCBFunction(uint32_t IfIndex, uint32_t ReadValue)
+{
+    return OK;
+}
 
 void ControlStart(void)
 {
@@ -159,6 +181,8 @@ void ControlStart(void)
         TimerEnable(Control_timerBase, TIMER_A);
 
         ADCAcquireStart(0,1);
+        AddControlCallback( ControlEmptyCBFunction,  eHundredMillisecond, ControlActivityLed,0, 0, 0 );
+
     }
 }
 
@@ -260,45 +284,72 @@ uint32_t ControlLoop(uint32_t tick)
     Hundred_msTick  =   (tick%eHundredMillisecond == 0)  ?true:false;
     Onesecond_Tick  =   (tick%eOneSecond == 0)           ?true:false;
     Tick98          =   (tick%eHundredMillisecond == 99) ?true:false;
-    Tick998         =   (tick%eOneSecond == 998)         ?true:false;
+    Tick998         =   (tick%eOneSecond == 996)         ?true:false;
 
     for (Device_i = 0; Device_i < MAX_TANGO_CONTROL_DEVICES;Device_i++)
     {
         if (ControlArray[Device_i].ControlActive)
         {
+            ControlBacklog[backlogindex]=Device_i;
+            if ( ++backlogindex >= 999)
+                backlogindex = 0;
             switch (ControlArray[Device_i].ControlTiming)
             {
                 case eOneMillisecond:
+                    if(ControlArray[Device_i].ControlDataReadPtr)
                         ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr( ControlArray[Device_i].Parameter1,ControlArray[Device_i].Parameter2);
+                    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 eTenMilliSecond:
                     if (Ten_msTick)
                     {
-                        ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr( ControlArray[Device_i].Parameter1,ControlArray[Device_i].Parameter2);
-                        ControlArray[Device_i].ControlCallbackPtr(ControlArray[Device_i].IfIndex, ControlDatalog[Device_i]);
+                        if(ControlArray[Device_i].ControlDataReadPtr)
+                            ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr( ControlArray[Device_i].Parameter1,ControlArray[Device_i].Parameter2);
+                        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
-                        ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr( ControlArray[Device_i].Parameter1,ControlArray[Device_i].Parameter2);
+                        if(ControlArray[Device_i].ControlDataReadPtr)
+                            ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr( ControlArray[Device_i].Parameter1,ControlArray[Device_i].Parameter2);
+                        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
-                        ControlArray[Device_i].ControlCallbackPtr(ControlArray[Device_i].IfIndex, ControlDatalog[Device_i]);
+                        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)
                     {
-                        ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr( ControlArray[Device_i].Parameter1,ControlArray[Device_i].Parameter2);
+                        if(ControlArray[Device_i].ControlDataReadPtr)
+                            ControlDatalog[Device_i] = ControlArray[Device_i].ControlDataReadPtr( ControlArray[Device_i].Parameter1,ControlArray[Device_i].Parameter2);
+                        else
+                            LOG_ERROR (Device_i, "Invalid callback ptr");
                     }
                     if (Onesecond_Tick)
                     {
-                        ControlArray[Device_i].ControlCallbackPtr(ControlArray[Device_i].IfIndex, ControlDatalog[Device_i]);
+                        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;
                 default: