Software/Embedded_SW/Embedded/Modules/Control/control.c


1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
1<?xml version="1.0" encoding="utf-8"?>
<root>
  <!-- 
    Microsoft ResX Schema 
    
    Version 2.0
    
    The primary goals of this format is to allow a simple XML format 
    that is mostly human readable. The generation and parsing of the 
    various data types are done through the TypeConverter classes 
    associated with the data types.
    
    Example:
    
    ... ado.net/XML headers & schema ...
    <resheader name="resmimetype">text/microsoft-resx</resheader>
    <resheader name="version">2.0</resheader>
    <resheader name="reader">System.Resources.ResXResourceReader, System.Windows.Forms, ...</resheader>
    <resheader name="writer">System.Resources.ResXResourceWriter, System.Windows.Forms, ...</resheader>
    <data name="Name1"><value>this is my long string</value><comment>this is a comment</comment></data>
    <data name="Color1" type="System.Drawing.Color, System.Drawing">Blue</data>
    <data name="Bitmap1" mimetype="application/x-microsoft.net.object.binary.base64">
        <value>[base64 mime encoded serialized .NET Framework object]</value>
    </data>
    <data name="Icon1" type="System.Drawing.Icon, System.Drawing" mimetype="application/x-microsoft.net.object.bytearray.base64">
        <value>[base64 mime encoded string representing a byte array form of the .NET Framework object]</value>
        <comment>This is a comment</comment>
    </data>
                
    There are any number of "resheader" rows that contain simple 
    name/value pairs.
    
    Each data row contains a name, and value. The row also contains a 
    type or mimetype. Type corresponds to a .NET class that support 
    text/value conversion through the TypeConverter architecture. 
    Classes that don't support this are serialized and stored with the 
    mimetype set.
    
    The mimetype is used for serialized objects, and tells the 
    ResXResourceReader how to depersist the object. This is currently not 
    extensible. For a given mimetype the value must be set accordingly:
    
    Note - application/x-microsoft.net.object.binary.base64 is the format 
    that the ResXResourceWriter will generate, however the reader can 
    read any of the formats listed below.
    
    mimetype: application/x-microsoft.net.object.binary.base64
    value   : The object must be serialized with 
            : System.Runtime.Serialization.Formatters.Binary.BinaryFormatter
            : and then encoded with base64 encoding.
    
    mimetype: application/x-microsoft.net.object.soap.base64
    value   : The object must be serialized with 
            : System.Runtime.Serialization.Formatters.Soap.SoapFormatter
            : and then encoded with base64 encoding.

    mimetype: application/x-microsoft.net.object.bytearray.base64
    value   : The object must be serialized into a byte array 
            : using a System.ComponentModel.TypeConverter
            : and then encoded with base64 encoding.
    -->
  <xsd:schema id="root" xmlns="" xmlns:xsd="http://www.w3.org/2001/XMLSchema" xmlns:msdata="urn:schemas-microsoft-com:xml-msdata">
    <xsd:import namespace="http://www.w3.org/XML/1998/namespace" />
    <xsd:element name="root" msdata:IsDataSet="true">
      <xsd:complexType>
        <xsd:choice maxOccurs="unbounded">
          <xsd:element name="metadata">
            <xsd:complexType>
              <xsd:sequence>
                <xsd:element name="value" type="xsd:string" minOccurs="0" />
              </xsd:sequence>
              <xsd:attribute name="name" use="required" type="xsd:string" />
              <xsd:attribute name="type" type="xsd:string" />
              <xsd:attribute name="mimetype" type="xsd:string" />
              <xsd:attribute ref="xml:space" />
            </xsd:complexType>
          </xsd:element>
          <xsd:element name="assembly">
            <xsd:complexType>
              <xsd:attribute name="alias" type="xsd:string" />
              <xsd:attribute name="name" type="xsd:string" />
            </xsd:complexType>
          </xsd:element>
          <xsd:element name="data">
            <xsd:complexType>
              <xsd:sequence>
                <xsd:element name="value" type="xsd:string" minOccurs="0" msdata:Ordinal="1" />
                <xsd:element name="comment" type="xsd:string" minOccurs="0" msdata:Ordinal="2" />
              </xsd:sequence>
              <xsd:attribute name="name" type="xsd:string" use="required" msdata:Ordinal="1" />
              <xsd:attribute name="type" type="xsd:string" msdata:Ordinal="3" />
              <xsd:attribute name="mimetype" type="xsd:string" msdata:Ordinal="4" />
              <xsd:attribute ref="xml:space" />
            </xsd:complexType>
          </xsd:element>
          <xsd:element name="resheader">
            <xsd:complexType>
              <xsd:sequence>
                <xsd:element name="value" type="xsd:string" minOccurs="0" msdata:Ordinal="1" />
              </xsd:sequence>
              <xsd:attribute name="name" type="xsd:string" use="required" />
            </xsd:complexType>
          </xsd:element>
        </xsd:choice>
      </xsd:complexType>
    </xsd:element>
  </xsd:schema>
  <resheader name="resmimetype">
    <value>text/microsoft-resx</value>
  </resheader>
  <resheader name="version">
    <value>2.0</value>
  </resheader>
  <resheader name="reader">
    <value>System.Resources.ResXResourceReader, System.Windows.Forms, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
  </resheader>
  <resheader name="writer">
    <value>System.Resources.ResXResourceWriter, System.Windows.Forms, Version=4.0.0.0, Culture=neutral, PublicKeyToken=b77a5c561934e089</value>
  </resheader>
</root>
/************************************************************************************************************************
 * control.c
 * Control module
 *
 * The control module is hardware agnostic, not related to a specific hardware module, to enable it to deal with hardware and system changes easily.
 * The control module contains a high priority task that connects the hardware drivers
 * below to the control algorithms of the high level system modules above.
 *
 * The control task is invoked by a message from a 1 millisecond timer interrupt.
 * The module is based on system components registered in the control module, supplying callback hooks for information gathering and distribution.
 * All control clients (drivers and modules) will not perform long procedures that will block the control task.
 *
 * Control interfaces:
 * System Devices list:
 *
 * All control system input devices   will be listed in a system-shared enumareted list, to create a common language between the modules and the drivers.
 * (The list will include all hardware devices, but for this module only control input devices are valid)
 * Drivers:
 * Device registration: a driver that initializes a control input device will call the ControlDeviceRegister function with the deviceId,
 * and a control callback hook. This callback is called with a deviceId as the parameter, and returns an unsigned 32bit integer
 * as a read value and a call status. The callback will be a non-blocking call, and will indicate in the status value if the data is valid.
 *
 * It is the responsibility of the device driver to update the control information according to the module hardware requirements.
 * If the polling of the information is immediate, it can be collected at the callback call (e.g. local GPI).
 *
 * Module registration � control:  a module is registering to receive the value of the input from a specific device.
 * The module indicates what will be the desired frequency of the device polling (in milliseconds).
 * It supplies a callback routine that will receive the deviceId, the control value status and the control value.
 * The callback will be a non-blocking call.
 * The polling frequency is one of a specific list of frequencies: 1/10/100/1000 Hz. (others - TBD)
 *
 *
 * Registering a request for control information from a device that is not registered will be rejected.
 * The device registration process must be performed before module registration process.
 *
 * Unregistering: when control information is not needed, the module will unregister the device polling request from the control module.
 * There is a separate call for control hooks and for report hooks.
 * The unregistering command contains the callback function pointer, to enable distribution of control
 * information of the same device to more than one destination.
 *
 **************************************************************************************************************************/

////////////////////////////////State machine operation////////////////////////////////////
//the state machine operation is used to operate in runtime correct profile flow execution
//by recieved esign flow of the user from the UI
///////////////////////////////////////////////////////////////////////////////////////////
#include "include.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"


/******************** Definitions  ********************************************/
#define MAX_TANGO_CONTROL_DEVICES 120
/******************** STRUCTURES AND ENUMs  ********************************************/


typedef struct
{
  uint32_t PartId;  // the identity of the inspected/controlled part in the Devices enum.
  bool ControlActive;
  uint32_t Parameter1;
  uint16_t IfIndex;
  uint32_t StartTick;
  DataReadCBFunction ControlDataReadPtr;
  ControlCBFunction ControlCallbackPtr;
  uint32_t ControlTiming;
  char *Name;
}ControlDeviceStruc;

typedef enum
{
    OneMillisec,
}controlMessages;

typedef struct ControlMessage{
    uint16_t messageId;
    uint16_t msglen;
    uint32_t tick;
    uint8_t messageData[20];
}ControlMessageStruc;

int ControlPhaseDelay = 300; //the control task enters only after data gathering in the millisecond task is finished.
//this parameters defines how many microseconds in the delay. it is used only on starting the control loop on the first time
/******************** GLOBAL PARAMETERS  ********************************************/
Mailbox_Handle          ControlMsgQ = NULL;
Mailbox_Handle          TenControlMsgQ = NULL;
bool                    ControlRestart;
static GateMutex_Handle gateControlDB;
Task_Handle Control_Task_Handle;
ControlDeviceStruc      ControlArray[MAX_TANGO_CONTROL_DEVICES];
uint32_t                ControlDatalog[MAX_TANGO_CONTROL_DEVICES];
#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 = 0xFF;
/******************** Functions  ********************************************/
void OneMilliSecondFunction(UArg arg0);

//**********************************************************************
/******************** CODE  ********************************************/
//**********************************************************************
uint32_t TemplateDataReadCBFunction (uint32_t deviceID, uint32_t Parameter1)
{
    return 0;
}


void ControlInit(void)
{
    int Device_i;
    Error_Block eb;

    //Mailbox_Params_init(&ControlMsgQ);
    ControlMsgQ = Mailbox_create(sizeof(ControlMessageStruc), 1, NULL,NULL);
    TenControlMsgQ = Mailbox_create(sizeof(ControlMessageStruc), 1, NULL,NULL);

    ControlRestart = false;

    memset(ControlDatalog,0,sizeof(uint32_t)*MAX_TANGO_CONTROL_DEVICES);
    MaxHighDevices = 0xFF;
    for (Device_i = 0; Device_i < MAX_TANGO_CONTROL_DEVICES; Device_i++)
    {
        ControlArray[Device_i].ControlActive = false;
        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)
    {
        System_abort("Could not create USB Wait gate");
    }

    ROM_SysCtlPeripheralEnable(SYSCTL_PERIPH_TIMER0);
    ROM_TimerConfigure(Control_timerBase, TIMER_CFG_PERIODIC);   // 32 bits Timer
    //TimerIntRegister(Control_timerBase, TIMER_A, Timer0Isr);    // Registering  isr
    ROM_TimerEnable(Control_timerBase, TIMER_A);
    ROM_IntEnable(INT_TIMER0A);
    ROM_TimerIntEnable(Control_timerBase, TIMER_TIMA_TIMEOUT);

    //ADCAcquireInit();

    return;
}
void ControlStop(void)
{
    ControlRestart = false;
    ADCAcquireStop();
}
uint32_t ControlActivityLed( uint32_t Parameter1)
{
    static bool flag = false;
    static uint8_t counter;
    const uint8_t Blink_Freq = 7;//odd number

    if (flag==true)
    {
        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);
        else
        if((power.color == BLINK) && (counter % Blink_Freq == 0) )
        {
            Pannel_Leds(POWER_ON_OFF,MODE_OFF);
        }
        ///////////////////////////////////////////////////////////
        if(jog.color == fastBILNK)
            Pannel_Leds(THREAD_JOGGING,MODE_OFF);
        else
        if((jog.color == BLINK) && (counter % Blink_Freq == 0) )
        {
            Pannel_Leds(THREAD_JOGGING,MODE_OFF);
        }
        ///////////////////////////////////////////////////////////
        if(load.color == fastBILNK)
            Pannel_Leds(THREAD_LOAD,MODE_OFF);
        else
        if((load.color == BLINK) && (counter % Blink_Freq == 0) )
        {
            Pannel_Leds(THREAD_LOAD,MODE_OFF);
        }
        ///////////////////////////////////////////////////////////
        if(cart1.color == fastBILNK)
            Pannel_Leds(CART_1,MODE_OFF);
        else
        if((cart1.color == BLINK) && (counter % Blink_Freq == 0) )
        {
            Pannel_Leds(CART_1,MODE_OFF);
        }
        ///////////////////////////////////////////////////////////
        if(cart2.color == fastBILNK)
            Pannel_Leds(CART_2,MODE_OFF);
        else
        if((cart2.color == BLINK) && (counter % Blink_Freq == 0) )
        {
            Pannel_Leds(CART_2,MODE_OFF);
        }
        ///////////////////////////////////////////////////////////
        if(cart3.color == fastBILNK)
            Pannel_Leds(CART_3,MODE_OFF);
        else
        if((cart3.color == BLINK) && (counter % Blink_Freq == 0) )
        {
            Pannel_Leds(CART_3,MODE_OFF);
        }
        flag = false;
    }
    else
    {
        COMM_RED_LED_OFF;
        if (HeaterActive > 0)// Blink the led on heating
            ACTIVITY_RED_LED_ON;// Heaters indication - at least one of  the Heaters is ON

        ACTIVITY_GREEN_LED_OFF;

        if(power.color == fastBILNK)
            Pannel_Leds(POWER_ON_OFF,MODE_ON);
        else
        if((power.color == BLINK) && (counter % Blink_Freq == 0) )
        {
            Pannel_Leds(POWER_ON_OFF,MODE_ON);
        }
        ///////////////////////////////////////////////////////////
        if(jog.color == fastBILNK)
            Pannel_Leds(THREAD_JOGGING,MODE_ON);
        else
        if((jog.color == BLINK) && (counter % Blink_Freq == 0) )
        {
            Pannel_Leds(THREAD_JOGGING,MODE_ON);
        }
        ///////////////////////////////////////////////////////////
        if(load.color == fastBILNK)
            Pannel_Leds(THREAD_LOAD,MODE_ON);
        else
        if((load.color == BLINK) && (counter % Blink_Freq == 0) )
        {
            Pannel_Leds(THREAD_LOAD,MODE_ON);
        }
        ///////////////////////////////////////////////////////////
        if(cart1.color == fastBILNK)
            Pannel_Leds(CART_1,MODE_ON);
        else
        if((cart1.color == BLINK) && (counter % Blink_Freq == 0) )
        {
            Pannel_Leds(CART_1,MODE_ON);
        }
        ///////////////////////////////////////////////////////////
        if(cart2.color == fastBILNK)
            Pannel_Leds(CART_2,MODE_ON);
        else
        if((cart2.color == BLINK) && (counter % Blink_Freq == 0) )
        {
            Pannel_Leds(CART_2,MODE_ON);
        }
        ///////////////////////////////////////////////////////////
        if(cart3.color == fastBILNK)
            Pannel_Leds(CART_3,MODE_ON);
        else
        if((cart3.color == BLINK) && (counter % Blink_Freq == 0) )
        {
            Pannel_Leds(CART_3,MODE_ON);
        }


        flag = true;
    }

    if (counter < 0xFF)
        counter++;
    else
        counter = Blink_Freq + 1;
return OK;
}
uint32_t ControlEmptyCBFunction(uint32_t IfIndex, uint32_t ReadValue)
{
    return OK;
}

void ControlStart(void)
{
    if (ControlRestart == false)
    {
        ControlRestart = true;
        ROM_TimerLoadSet(Control_timerBase, TIMER_A,120000+(ControlPhaseDelay*120)/*one millisecond*/);
        TimerEnable(Control_timerBase, TIMER_A);

        ADCAcquireStart(0,1);
        AddControlCallback("ControlActivityLed", ControlEmptyCBFunction,  eHundredMillisecond, ControlActivityLed,0, 0, 0 );

        SysCtlDelay(12000000);
        MillisecStart();
    }
}

/************************************************************************************************************************************************
 * the control task reads the data from the devices every millisecond.
 * (the data might be old data, if it is polled in a slower rate)
 * for every polled device, there is a need to add a Data read callback, with a device id. the read value is always 32bits unsigned integer
 * if there is a need to run a control function based on the read data, then the hardware module will add a control function, specifying the control calling rate
 * 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(char* Name, ControlCBFunction Callback, uint32_t CtrlFrequency, DataReadCBFunction DriverfPtr, uint16_t IfIndex, uint32_t Parameter1, uint32_t Parameter2 )
{
    assert(Callback);
    assert(DriverfPtr);
    unsigned int key;

    uint32_t device_i;
    uint32_t deviceId = 0xFF;
    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 (MaxHighDevices == 0xFF)
            MaxHighDevices = deviceId;
        else
        {
      	  	if ((deviceId!=0xFF )&&(deviceId> MaxHighDevices))
     	       	MaxHighDevices = deviceId;
        }
    }
    else
    {
        for(device_i = MAX_TANGO_CONTROL_DEVICES-1;device_i > 0;device_i--)
        {
            if (ControlArray[device_i].ControlActive == false)
            {
                deviceId = device_i;
                break;
            }
        }
    }

    if (deviceId == 0xFF)
    {
        LOG_ERROR(deviceId, "Add Callback failed");
        return 0xFF;
    }
    key = GateMutex_enter(gateControlDB);
    ControlArray[deviceId].ControlTiming = CtrlFrequency;
    ControlArray[deviceId].ControlCallbackPtr = Callback;
    ControlArray[deviceId].ControlActive = true;
    ControlArray[deviceId].ControlDataReadPtr = DriverfPtr;
    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");

    return deviceId;

}
int SafeRemoveHighControlCallback(uint32_t deviceId , ControlCBFunction Callback)
{
    if (RemoveControlCallback(deviceId, Callback )!=OK)
    {
        Report("Fixing Remove control ",__FILE__,__LINE__,(int)GetControlDevice_i(),RpWarning,(int)deviceId,0);
        if (RemoveControlCallback(GetControlDevice_i(),Callback)==OK)
        {
            Report("Remove control callback fixed",ControlArray[GetControlDevice_i()].Name,__LINE__,(int)GetControlDevice_i(),RpWarning,(int)deviceId,0);
        }
        else
        {
            Report("Remove control callback failed",ControlArray[GetControlDevice_i()].Name,__LINE__,(int)GetControlDevice_i(),RpWarning,(int)deviceId,0);
            return ERROR;
        }
    }
    return OK;
}

int  SafeRemoveControlCallback(uint32_t deviceId , ControlCBFunction Callback)
{
    if (RemoveControlCallback(deviceId, Callback )!=OK)
    {
        Report("Fixing Remove control ",__FILE__,__LINE__,(int)GetControlLowDevice_i(),RpWarning,(int)deviceId,0);
        if (RemoveControlCallback(GetControlLowDevice_i(),Callback)==OK)
        {
            Report("Remove control callback fixed",ControlArray[GetControlDevice_i()].Name,__LINE__,(int)GetControlLowDevice_i(),RpWarning,(int)deviceId,0);
        }
        else
        {
            Report("Remove control callback failed",ControlArray[GetControlDevice_i()].Name,__LINE__,(int)GetControlLowDevice_i(),RpWarning,(int)deviceId,0);
            return ERROR;
        }
    }
    return OK;
}

int     RemoveControlCallback(uint32_t deviceId , ControlCBFunction Callback)
{
    if (deviceId == 0xFF)
        return ERROR;
    assert(deviceId < MAX_TANGO_CONTROL_DEVICES);
    unsigned int key;

    if (Callback == ControlArray[deviceId].ControlCallbackPtr)
    {
        key = GateMutex_enter(gateControlDB);
        ControlArray[deviceId].ControlTiming = eNoControl;
        ControlArray[deviceId].ControlCallbackPtr = NULL;
        ControlArray[deviceId].ControlDataReadPtr = NULL;
        ControlArray[deviceId].ControlActive = false;
        ControlArray[deviceId].Parameter1 = 0;
        ControlArray[deviceId].IfIndex = 0;
        //LOG_ERROR(deviceId, "Remove Callback ");
        GateMutex_leave(gateControlDB, key);
        return OK;
    }
    else
    {
        LOG_ERROR(deviceId, "Remove Callback failed");
        return ERROR;
    }

}

uint32_t millisecondCounter = 0;
void OneMilliSecondControlInterrupt(UArg arg0)
{
    ControlMessageStruc ControlMessage;
    //uint32_t TenmillisecondCounter = 0;

    ROM_IntMasterDisable();
    ROM_TimerIntClear(Control_timerBase, TIMER_TIMA_TIMEOUT);  // Clear the timer interrupt

    if (ControlRestart == true)
    {
        ROM_TimerLoadSet(Control_timerBase, TIMER_A,120000/*one millisecond*/);
    }
    else
    {
        ROM_IntDisable(INT_TIMER0A);
	    ROM_IntMasterEnable();
        //ROM_TimerDisable(Control_timerBase, TIMER_A);
        return;
    }

    //send message to the control task
    ControlMessage.messageId = OneMillisec;
    ControlMessage.tick = millisecondCounter++;
    ControlMessage.msglen = sizeof(ControlMessageStruc);
    if (ControlMsgQ != NULL)
        Mailbox_post(ControlMsgQ , &ControlMessage, BIOS_NO_WAIT);
    //if (TenmillisecondCounter)
    {
        //ControlMessage.tick = TenmillisecondCounter;
        if (TenControlMsgQ != NULL)
            Mailbox_post(TenControlMsgQ , &ControlMessage, BIOS_NO_WAIT);
    }
    if (millisecondCounter == 1000000000)
        millisecondCounter = 0;
    //
    // Enable all interrupts.
    //
    ROM_IntMasterEnable();
    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;
}
ControlCBFunction GetControlCallbackFuncPtr(uint32_t ControlId)
{
    if (ControlArray[ControlId].ControlActive)
        return ControlArray[ControlId].ControlCallbackPtr;
    else
        return NULL;

}
uint32_t ControlLoop(uint32_t tick)
{
    if (MaxHighDevices == 0xFF)
        return OK;
    for (ControlDevice_i = 0; ControlDevice_i <= MaxHighDevices;ControlDevice_i++)
    //for (ControlDevice_i = 0; ControlDevice_i < MAX_TANGO_CONTROL_DEVICES;ControlDevice_i++)
    {
        if (ControlArray[ControlDevice_i].ControlActive)
        {
            ControlBacklog[backlogindex]=ControlDevice_i;
            if ( ++backlogindex >= MAX_BACKLOG_SIZE)
                backlogindex = 0;
            switch (ControlArray[ControlDevice_i].ControlTiming)
            {
                case eOneMillisecond:
                    if(ControlArray[ControlDevice_i].ControlDataReadPtr)
                        ControlDatalog[ControlDevice_i] = ControlArray[ControlDevice_i].ControlDataReadPtr( ControlArray[ControlDevice_i].Parameter1);
                    else
                        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 (ControlDevice_i, "Invalid callback ptr");
                    break;
                default:
                    break;
            } //switch
        } //if control active
    }  //for
    //ROM_IntMasterEnable();

    return OK;
}
uint32_t ControlLowLoop(uint32_t tick)
{
    for (ControlLowDevice_i = 0; ControlLowDevice_i < MAX_TANGO_CONTROL_DEVICES;ControlLowDevice_i++)
    {
        if (ControlArray[ControlLowDevice_i].ControlActive)
        {
            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 >= MAX_BACKLOG_SIZE)
                    backlogindex = 0;

                if(ControlArray[ControlLowDevice_i].ControlDataReadPtr)
                    ControlDatalog[ControlLowDevice_i] = ControlArray[ControlLowDevice_i].ControlDataReadPtr( ControlArray[ControlLowDevice_i].Parameter1);
                else
                    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 (ControlLowDevice_i, "Invalid callback ptr");
            }
        } //if control active
    }  //for
    //ROM_IntMasterEnable();

    return OK;
}
/******************************************************************************
 *  ======== messageTsk ========
 *  Task for this function is created statically. See the project's .cfg file.
 *  this message task is created statically in system initialization,
 ******************************************************************************/
void controlTask(UArg arg0, UArg arg1)
{
    ControlMessageStruc ControlMessage;
    //char str[60];
    //uint16_t length;
    //Clock_setTimeout(HostKAClock, 1000);
    //Clock_start(HostKAClock);
    Control_Task_Handle = Task_self();

    while(1)
    {
        Mailbox_pend(ControlMsgQ , &ControlMessage, BIOS_WAIT_FOREVER);
        switch (ControlMessage.messageId)
        {
            case OneMillisec:
                ControlLoop(ControlMessage.tick);
                break;
            default:
                break;
        }
    }
}
/******************************************************************************
 *  ======== messageTsk ========
 *  Task for this function is created statically. See the project's .cfg file.
 *  this message task is created statically in system initialization,
 ******************************************************************************/
void controlLowTask(UArg arg0, UArg arg1)
{
    ControlMessageStruc ControlLowMessage;
    //char str[60];
    //uint16_t length;
    //Clock_setTimeout(HostKAClock, 1000);
    //Clock_start(HostKAClock);
    Control_Task_Handle = Task_self();

    while(1)
    {
        Mailbox_pend(TenControlMsgQ , &ControlLowMessage, BIOS_WAIT_FOREVER);
        switch (ControlLowMessage.messageId)
        {
            case OneMillisec:
                ControlLowLoop(ControlLowMessage.tick);
                break;
            default:
                break;
        }
    }
}