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

2 files changed, 60 insertions, 8 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
index a1de29b08..847b777fa 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
@@ -31,6 +31,7 @@
 #include "drivers/FPGA/FPGA.h"
 
 #include "modules/thread/thread_ex.h"
+#include "Drivers/Peripheral_GPIO/GPIO.h"
 
 Task_Handle Millisecond_Task_Handle;
 /******************** Definitions  ********************************************/
@@ -236,7 +237,6 @@ uint32_t MillisecLoop(uint32_t tick)
         FPGA_Read_limit_Switches_Registers();
         //Read_FPGA_GPI_Rgisters();//FPGA_Read_limit_Switches();
 #endif
-
         temp += MotorActivity;
    if (MotorActivity)
    {
@@ -340,6 +340,7 @@ uint32_t MillisecLoop(uint32_t tick)
     {
         for (Sensor_i = 0;Sensor_i < MAX_TEMPERATURE_SENSOR_ID;Sensor_i++)
             MillisecReadFromTempSensor(Sensor_i, NULL);
+        ACTIVITY_RED_LED_CHANGE_MODE;
     }
 
     if (Onesecond_Tick)
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: