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
pre { line-height: 125%; }
td.linenos .normal { color: inherit; background-color: transparent; padding-left: 5px; padding-right: 5px; }
span.linenos { color: inherit; background-color: transparent; padding-left: 5px; padding-right: 5px; }
td.linenos .special { color: #000000; background-color: #ffffc0; padding-left: 5px; padding-right: 5px; }
span.linenos.special { color: #000000; background-color: #ffffc0; padding-left: 5px; padding-right: 5px; }
.highlight .hll { background-color: #ffffcc }
.highlight .c { color: #888888 } /* Comment */
.highlight .err { color: #a61717; background-color: #e3d2d2 } /* Error */
.highlight .k { color: #008800; font-weight: bold } /* Keyword */
.highlight .ch { color: #888888 } /* Comment.Hashbang */
.highlight .cm { color: #888888 } /* Comment.Multiline */
.highlight .cp { color: #cc0000; font-weight: bold } /* Comment.Preproc */
.highlight .cpf { color: #888888 } /* Comment.PreprocFile */
.highlight .c1 { color: #888888 } /* Comment.Single */
.highlight .cs { color: #cc0000; font-weight: bold; background-color: #fff0f0 } /* Comment.Special */
.highlight .gd { color: #000000; background-color: #ffdddd } /* Generic.Deleted */
.highlight .ge { font-style: italic } /* Generic.Emph */
.highlight .ges { font-weight: bold; font-style: italic } /* Generic.EmphStrong */
.highlight .gr { color: #aa0000 } /* Generic.Error */
.highlight .gh { color: #333333 } /* Generic.Heading */
.highlight .gi { color: #000000; background-color: #ddffdd } /* Generic.Inserted */
.highlight .go { color: #888888 } /* Generic.Output */
.highlight .gp { color: #555555 } /* Generic.Prompt */
.highlight .gs { font-weight: bold } /* Generic.Strong */
.highlight .gu { color: #666666 } /* Generic.Subheading */
.highlight .gt { color: #aa0000 } /* Generic.Traceback */
.highlight .kc { color: #008800; font-weight: bold } /* Keyword.Constant */
.highlight .kd { color: #008800; font-weight: bold } /* Keyword.Declaration */
.highlight .kn { color: #008800; font-weight: bold } /* Keyword.Namespace */
.highlight .kp { color: #008800 } /* Keyword.Pseudo */
.highlight .kr { color: #008800; font-weight: bold } /* Keyword.Reserved */
.highlight .kt { color: #888888; font-weight: bold } /* Keyword.Type */
.highlight .m { color: #0000DD; font-weight: bold } /* Literal.Number */
.highlight .s { color: #dd2200; background-color: #fff0f0 } /* Literal.String */
.highlight .na { color: #336699 } /* Name.Attribute */
.highlight .nb { color: #003388 } /* Name.Builtin */
.highlight .nc { color: #bb0066; font-weight: bold } /* Name.Class */
.highlight .no { color: #003366; font-weight: bold } /* Name.Constant */
.highlight .nd { color: #555555 } /* Name.Decorator */
.highlight .ne { color: #bb0066; font-weight: bold } /* Name.Exception */
.highlight .nf { color: #0066bb; font-weight: bold } /* Name.Function */
.highlight .nl { color: #336699; font-style: italic } /* Name.Label */
.highlight .nn { color: #bb0066; font-weight: bold } /* Name.Namespace */
.highlight .py { color: #336699; font-weight: bold } /* Name.Property */
.highlight .nt { color: #bb0066; font-weight: bold } /* Name.Tag */
.highlight .nv { color: #336699 } /* Name.Variable */
.highlight .ow { color: #008800 } /* Operator.Word */
.highlight .w { color: #bbbbbb } /* Text.Whitespace */
.highlight .mb { color: #0000DD; font-weight: bold } /* Literal.Number.Bin */
.highlight .mf { color: #0000DD; font-weight: bold } /* Literal.Number.Float */
.highlight .mh { color: #0000DD; font-weight: bold } /* Literal.Number.Hex */
.highlight .mi { color: #0000DD; font-weight: bold } /* Literal.Number.Integer */
.highlight .mo { color: #0000DD; font-weight: bold } /* Literal.Number.Oct */
.highlight .sa { color: #dd2200; background-color: #fff0f0 } /* Literal.String.Affix */
.highlight .sb { color: #dd2200; background-color: #fff0f0 } /* Literal.String.Backtick */
.highlight .sc { color: #dd2200; background-color: #fff0f0 } /* Literal.String.Char */
.highlight .dl { color: #dd2200; background-color: #fff0f0 } /* Literal.String.Delimiter */
.highlight .sd { color: #dd2200; background-color: #fff0f0 } /* Literal.String.Doc */
.highlight .s2 { color: #dd2200; background-color: #fff0f0 } /* Literal.String.Double */
.highlight .se { color: #0044dd; background-color: #fff0f0 } /* Literal.String.Escape */
.highlight .sh { color: #dd2200; background-color: #fff0f0 } /* Literal.String.Heredoc */
.highlight .si { color: #3333bb; background-color: #fff0f0 } /* Literal.String.Interpol */
.highlight .sx { color: #22bb22; background-color: #f0fff0 } /* Literal.String.Other */
.highlight .sr { color: #008800; background-color: #fff0ff } /* Literal.String.Regex */
.highlight .s1 { color: #dd2200; background-color: #fff0f0 } /* Literal.String.Single */
.highlight .ss { color: #aa6600; background-color: #fff0f0 } /* Literal.String.Symbol */
.highlight .bp { color: #003388 } /* Name.Builtin.Pseudo */
.highlight .fm { color: #0066bb; font-weight: bold } /* Name.Function.Magic */
.highlight .vc { color: #336699 } /* Name.Variable.Class */
.highlight .vg { color: #dd7700 } /* Name.Variable.Global */
.highlight .vi { color: #3333bb } /* Name.Variable.Instance */
.highlight .vm { color: #336699 } /* Name.Variable.Magic */
.highlight .il { color: #0000DD; font-weight: bold } /* Literal.Number.Integer.Long */using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace Tango.CodeGeneration
{
    /// <summary>
    /// Represents a single enumeration field.
    /// </summary>
    public class EnumerationField
    {
        /// <summary>
        /// Gets or sets the field name.
        /// </summary>
        public String Name { get; set; }

        /// <summary>
        /// Gets or sets the value.
        /// </summary>
        public int Value { get; set; }

        /// <summary>
        /// Gets or sets an optional description.
        /// </summary>
        public String Description { get; set; }
    }
}
/************************************************************************************************************************
 * 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/FPGA/Full_Vme/FPGA_Programming_Up.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"


#define DURATION_LIMIT 6
/******************** Definitions  ********************************************/
#define MAX_TANGO_CONTROL_DEVICES 100
/******************** 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};
uint32_t ControlTime[MAX_TANGO_CONTROL_DEVICES]={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();
}
int FPGA_ReInit_Count = 0; ///avoid too many reinitializations of motors as happens when FPGA is corrupted
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)
        {
            if (FPGABurningActive == false)
            {
                FPGA_WD_Occurred = false;
                AlarmHandlingSetAlarm(EVENT_TYPE__FPGA_WATCHDOG_ACTIVATED,true);
                if (JobIsActive())
                {
                    JobEndReason = JOB_MOTOR_ALARM;
                    usnprintf(AlarmReasonStr, 100, "Hardware Failure Error");
                    SendJobProgress(0.0,0,false, "Hardware Failure Error");
                    AbortJob("FPGA Watchdog Error");
                }
                if (FPGA_ReInit_Count++<20)
                {
                    ReportWithPackageFilter(FPGAFilter, "FPGA Watchdog Error",__FILE__,__LINE__,0,RpError, 0,0);

                    ACTIVITY_GREEN_LED_ON;
                    MotorConfiguredTimeout = 100;

                    FPGA_SetMotorsInit();
                    Motor_ReconfigAllMotors();
                }
            }
        }
		else
            ACTIVITY_GREEN_LED_OFF;

        if(power.color == colorOFF) Pannel_Leds(POWER_ON_OFF,MODE_OFF);
        if(jog.color   == colorOFF) Pannel_Leds(THREAD_JOGGING,MODE_OFF);
        if(load.color  == colorOFF) Pannel_Leds(THREAD_JOGGING,MODE_OFF);
        if(cart1.color == colorOFF) Pannel_Leds(CART_1,MODE_OFF);
        if(cart2.color == colorOFF) Pannel_Leds(CART_2,MODE_OFF);
        if(cart3.color == colorOFF) Pannel_Leds(CART_3,MODE_OFF);

        if(power.color == colorON) Pannel_Leds(POWER_ON_OFF,MODE_ON);
        if(jog.color   == colorON) Pannel_Leds(THREAD_JOGGING,MODE_ON);
        if(load.color  == colorON) Pannel_Leds(THREAD_JOGGING,MODE_ON);
        if(cart1.color == colorON) Pannel_Leds(CART_1,MODE_ON);
        if(cart2.color == colorON) Pannel_Leds(CART_2,MODE_ON);
        if(cart3.color == colorON) Pannel_Leds(CART_3,MODE_ON);

        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");
        AlarmHandlingSetAlarm(EVENT_TYPE__FPGA_WATCHDOG_ACTIVATED,true);
        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 prevtick = 0;
#ifdef CONTROL_DEBUG
void ResetControlTime(void)
{
    memset(ControlTime,0,sizeof(ControlTime));
}
#endif
uint32_t ControlLowLoop(uint32_t tick)
{
    uint32_t skipped_ticks = 0;
#ifdef CONTROL_DEBUG
    uint32_t tempp,tempq,delta;
    uint32_t sys_ticks_start = msec_millisecondCounter,sys_ticks_end,max = 0,dev = 0;
#endif
    if (tick-prevtick>1)
    {
        skipped_ticks = tick-prevtick-1;
        //if (tick-prevtick>10)
         //   Report("ControlLowLoop skipped",__FILE__,tick,(int)prevtick,RpWarning,(int)skipped_ticks,0);
    }
    prevtick = tick;

    for (ControlLowDevice_i = 0; ControlLowDevice_i < MAX_TANGO_CONTROL_DEVICES;ControlLowDevice_i++)
    {
        if (ControlArray[ControlLowDevice_i].ControlActive)
        {
            if (tick - ControlArray[ControlLowDevice_i].StartTick<=skipped_ticks)
                continue;
            if (ControlArray[ControlLowDevice_i].ControlTiming == eOneMillisecond)
                continue;
            if (ControlArray[ControlLowDevice_i].StartTick == tick)
                continue;

            if (((tick - ControlArray[ControlLowDevice_i].StartTick)%ControlArray[ControlLowDevice_i].ControlTiming)<=skipped_ticks) // run the control on exact intervals
            {
                ControlBacklog[backlogindex]=ControlLowDevice_i;
                if ( ++backlogindex >= MAX_BACKLOG_SIZE)
                    backlogindex = 0;
#ifdef CONTROL_DEBUG
                tempp = HibernateRTCSSGet();
#endif
                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");
#ifdef CONTROL_DEBUG
                tempq = HibernateRTCSSGet();
                if (tempq < tempp)
                {
                  delta = (32768 - tempp) + tempq + 1;
                }
                else
                  delta = tempq - tempp;
                if (ControlTime[ControlLowDevice_i]<delta)
                {
                    ControlTime[ControlLowDevice_i] = delta;
                }
#endif
            }
        } //if control active
    }  //for
#ifdef CONTROL_DEBUG
    sys_ticks_end= msec_millisecondCounter;
    if (sys_ticks_end-sys_ticks_start > DURATION_LIMIT)
    {
        Report("ControlLowLoop long",__FILE__,sys_ticks_end-sys_ticks_start,(int)sys_ticks_end,RpWarning,(int)sys_ticks_start,0);
        for (ControlLowDevice_i = 0; ControlLowDevice_i < MAX_TANGO_CONTROL_DEVICES;ControlLowDevice_i++)
        {
            if (ControlTime[ControlLowDevice_i]>max)
            {
                max = ControlTime[ControlLowDevice_i];
                dev = ControlLowDevice_i;
            }
        }
        //Report(ControlArray[dev].Name,__FILE__,__LINE__,dev,RpWarning,max,0);
    }
#endif
    //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;
        }
    }
}