try improved real time handling in control - not tested yet

1 files changed, 31 insertions, 10 deletions

diff --git a/Software/Embedded_SW/Embedded/Modules/Control/control.c b/Software/Embedded_SW/Embedded/Modules/Control/control.c
index 984db7329..8349e7f91 100644
--- a/Software/Embedded_SW/Embedded/Modules/Control/control.c
+++ b/Software/Embedded_SW/Embedded/Modules/Control/control.c
@@ -56,7 +56,7 @@
 #include "MillisecTask.h"
 /******************** Definitions  ********************************************/
 #define INVALID_MSG_ID    0xFFFF
-#define MAX_TANGO_CONTROL_DEVICES 80
+#define MAX_TANGO_CONTROL_DEVICES 100
 /******************** STRUCTURES AND ENUMs  ********************************************/
 
 
@@ -94,9 +94,11 @@ 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};
+#define MAX_BACKLOG_SIZE 100
+uint16_t ControlBacklog[MAX_BACKLOG_SIZE]={0};
 uint16_t backlogindex = 0;
 uint32_t        Control_timerBase = TIMER0_BASE;        //Timer handle
+uint32_t MaxHighDevices = 0;
 /******************** Functions  ********************************************/
 void OneMilliSecondFunction(UArg arg0);
 
@@ -122,7 +124,7 @@ void ControlInit(void)
     ControlRestart = false;
 
     memset(ControlDatalog,0,sizeof(uint32_t)*MAX_TANGO_CONTROL_DEVICES);
-
+    MaxHighDevices = 0;
     for (Device_i = 0; Device_i < MAX_TANGO_CONTROL_DEVICES; Device_i++)
     {
         ControlArray[Device_i].ControlActive = false;
@@ -206,14 +208,31 @@ uint32_t  AddControlCallback( ControlCBFunction Callback, CTRL_TIMING_ENUM CtrlF
 
     uint32_t device_i;
     uint32_t deviceId = 0xFF;
-    for(device_i = 0;device_i < MAX_TANGO_CONTROL_DEVICES;device_i++)
+    if (CtrlFrequency == eOneMillisecond)
+    {
+        for(device_i = 0;device_i < MAX_TANGO_CONTROL_DEVICES;device_i++)
+        {
+            if (ControlArray[device_i].ControlActive == false)
+            {
+                deviceId = device_i;
+                break;
+            }
+        }
+        if ((deviceId!=0xFF )&&(deviceId> MaxHighDevices))
+            MaxHighDevices = deviceId;
+    }
+    else
     {
-        if (ControlArray[device_i].ControlActive == false)
+        for(device_i = MAX_TANGO_CONTROL_DEVICES-1;device_i >=0;device_i--)
         {
-            deviceId = device_i;
-            break;
+            if (ControlArray[device_i].ControlActive == false)
+            {
+                deviceId = device_i;
+                break;
+            }
         }
     }
+
     if (deviceId == 0xFF)
     {
         LOG_ERROR(deviceId, "Add Callback failed");
@@ -329,12 +348,12 @@ uint32_t ControlLoop(uint32_t tick)
     Tick998         =   (tick%eOneSecond == 996)         ?true:false;
 */
     //ROM_IntMasterDisable();
-    for (ControlDevice_i = 0; ControlDevice_i < MAX_TANGO_CONTROL_DEVICES;ControlDevice_i++)
+    for (ControlDevice_i = 0; ControlDevice_i < MaxHighDevices;ControlDevice_i++)
     {
         if (ControlArray[ControlDevice_i].ControlActive)
         {
             ControlBacklog[backlogindex]=ControlDevice_i;
-            if ( ++backlogindex >= 999)
+            if ( ++backlogindex >= MAX_BACKLOG_SIZE)
                 backlogindex = 0;
             switch (ControlArray[ControlDevice_i].ControlTiming)
             {
@@ -377,10 +396,12 @@ uint32_t ControlLowLoop(uint32_t tick)
         {
             if (tick == ControlArray[ControlLowDevice_i].StartTick)
                 continue;
+            if (ControlArray[ControlLowDevice_i].ControlTiming == eOneMillisecond)
+                continue;
             if (((tick - ControlArray[ControlLowDevice_i].StartTick)%ControlArray[ControlLowDevice_i].ControlTiming)==0) // run the control on exact intervals
             {
                 ControlBacklog[backlogindex]=ControlLowDevice_i;
-                if ( ++backlogindex >= 999)
+                if ( ++backlogindex >= MAX_BACKLOG_SIZE)
                     backlogindex = 0;
 
                 if(ControlArray[ControlLowDevice_i].ControlDataReadPtr)