aboutsummaryrefslogtreecommitdiffstats
path: root/Software/Embedded_SW/Embedded/Modules/Control/MillisecTask.c
blob: 19376926557033d485041be3366e829b4a0db3bd (plain)
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
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 */
/************************************************************************************************************************
 * control.c
 **************************************************************************************************************************/
#ifndef MODULES_CONTROL_CONTROL_H_
#define MODULES_CONTROL_CONTROL_H_

////////////////////////////////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"

//typedef  uint32_t (* DeviceDataFunction)(uint32_t deviceID, uint32_t *Value);
typedef  uint32_t (* ControlCBFunction)(uint32_t IfIndex, uint32_t ReadValue);
typedef  uint32_t (* DataReadCBFunction)( uint32_t Parameter1);
typedef enum {
    eNoControl = 0,
    eOneMillisecond = 1,
    eTenMillisecond = 10,
    eFiftyMillisecond = 50,
    eHundredMillisecond = 100,
    eOneSecond = 1000,
    eOneMinute = 60000,
    eTenMinutes = 600000,
    eOneHour = 3600000
}CTRL_TIMING_ENUM;
typedef enum
{
    IfTypeNone,
    IfTypeThread,
    IfTypeWinder,
    IfTypeIDS,
    IfTypeHeaters,
    IfTypeWaste,
    IfTypeMotors,
    MAX_INTERFACE_TYPES
}SYSTEM_INTERFACE_TYPES_ID_ENUM;


void        ControlInit(void);
void        ControlStop(void);
void        ControlStart(void);
uint32_t    AddControlCallback(char *Name, ControlCBFunction Callback, uint32_t CtrlFrequency, DataReadCBFunction DriverfPtr, uint16_t IfIndex, uint32_t Parameter1, uint32_t Parameter2 );
int         RemoveControlCallback(uint32_t deviceId, ControlCBFunction Callback );
//The safe remove command can be used ONLY when called from the control callback itself
int         SafeRemoveControlCallback(uint32_t deviceId , ControlCBFunction Callback);
int         SafeRemoveHighControlCallback(uint32_t deviceId , ControlCBFunction Callback);

uint32_t    TemplateDataReadCBFunction (uint32_t deviceId, uint32_t Parameter1);
uint32_t    GetControlDevice_i(void);
uint32_t    GetControlLowDevice_i(void);

ControlCBFunction GetControlCallbackFuncPtr(uint32_t ControlId);

extern Task_Handle Control_Task_Handle;
extern uint32_t millisecondCounter;

#endif
ef='#n496'>496 497 498 499 500 501 502 503 504 505 506 507 508 509 510 511 512 513 514 515 516 517 518 519 520 521 522 523 524 525 526 527 528 529 530 531 532 533 534 535 536 537 538 539 540 541 542 543 544 545 546 547 548 549 550 551 552 553 554 555 556 557 558 559 560 561 562 563 564 565 566 567 568 569 570 571 572 573 574 575 576 577 578 579 580 581 582 583 584 585 586 587 588 589 590 591 592 593 594 595 596 597 598 599 600 601 602 603 604 605 606 607 608 609 610 611 612 613 614 615 616 617 618 619 620 621 622 623 624 625 626 627 628 629 630 631 632 633 634 635 636 637 638 639 640 641 642 643 644 645 646 647 648 649 650 651 652 653 654 655 656 657 658 659 660 661 662 663 664 665 666 667 668 669 670 671 672 673 674 675 676 677 678 679 680 681 682 683 684 685 686 687 688 689 690 691 692 693 694 695 696 697 698 699 700 701 702 703 704 705 706 707 708 709 710 711 712 713 714 715 716 717 718 719 720 721 722 723 724 725 726 727 728 729 730 731 732 733 734 735 736 737 738 739 740 741 742 743 744 745 746 747 748 749 750 751 752 753 754 755 756 757 758 759 760 761 762 763 764 765 766 767 768 769 770 771 772 773 774 775 776 777 778 779 780 781 782 783 784 785 786 787 788 789 790 791 792 793 794 795 796 797 798 799 800 801 802 803 804 805 806 807 808 809 810 811 812 813 814 815 816 817 818 819 820 821 822
/************************************************************************************************************************
 * Millisec.c
 * Millisec module
 *
 * The millisec task is called once every 1 millisecond to gather data from the FPGA crucial modules,
 * so it will be ready for the Millisec operations. it is called 300 (TBD, configurable)
 * microsecond before the Millisec task, so that the data will be ready for the Millisec handling.
 * the ,millisecond task holds the pointer to a double buffer of results, so that the Millisec will handle the
 * most updated data, without disturbing data gathering
 *
 **************************************************************************************************************************/

////////////////////////////////State machine operation////////////////////////////////////
//the state machine operation is used to operate in runtime correct profile flow execution
//by recieved design flow of the user from the UI
///////////////////////////////////////////////////////////////////////////////////////////
#include "include.h"
#include "Modules/General/GeneralHardware.h"
#include "Modules/AlarmHandling/AlarmHandling.h"

#include "MillisecTask.h"
#include <driverlib/timer.h>
#include <Drivers/SSI_Comm/SSI_Comm.h>
#include <Drivers/SSI_Comm/Speed_Sensor/Speed_Sensor.h>
#include <Drivers/SSI_Comm/Dancer/Dancer.h>
#include "drivers/I2C_Communication/ADC_MUX/ADC_MUX.h"
#include <inc/hw_ints.h>

#include "drivers/adc_sampling/adc.h"
#include "control.h"

#include "drivers/Motors/Motor.h"
#include "drivers/Heater/TemperatureSensor.h"
#include "drivers/FPGA/FPGA_SPI_Comm.h"
#include "drivers/FPGA/FPGA.h"
#include <drivers/FPGA/FPGA_Comm.h>
#include "drivers/Valves/Valve.h"
#include "drivers/FPGA/FPGA_GPIO/FPGA_GPIO.h"

#include "modules/thread/thread_ex.h"
#include "modules/ids/ids_ex.h"

#include "drivers/Flash_Memory/Flash_Memory.h"
#include "drivers/Flash_Memory/fatfs/ff.h"
#include "drivers/Flash_ram/FlashProgram.h"

#include "third_party/fatfs/src/ffconf.h"
#include "Common/SWUpdate/FileSystem.h"

#include "Communication/Connection.h"

Task_Handle Millisecond_Task_Handle;
/******************** Definitions  ********************************************/

/******************** STRUCTURES AND ENUMs  ********************************************/
typedef struct MillisecMotorData
{
    bool WaitForData;
    bool DataRequired;
    bool SyncRequired;
    MSecFptr Callback;
    unsigned long Data;
    int Length;
    bool Active;
}MillisecMotorDataStruc;
typedef enum
{
    OneMillisec,
}MillisecMessages;

typedef struct MillisecMessage{
    uint16_t messageId;
    uint16_t msglen;
    uint32_t tick;
    uint8_t messageData[20];
}MillisecMessageStruc;

//uint32_t  ADC_Data[MAX_ADC_DEVICES] = {0};
//uint32_t  TemperatureSensor_Data[MAX_TEMPERATURE_SENSOR_ID] = {0};
//uint32_t  MotorSpeed_Data[NUM_OF_MOTORS] = {0};
//uint32_t  MotorStatus_Data[NUM_OF_MOTORS] = {0};
//uint32_t  MotorPosition_Data[NUM_OF_MOTORS] = {0};
//bool      MotorBusy_Data[NUM_OF_MOTORS] = {true};
uint32_t  Dancer_Data[NUM_OF_DANCERS] = {0};
float  Speed_Data = 0;
uint32_t DrawerFansStatus = 0;
uint32_t SystemFansStatus = 0;
uint8_t Gas_PPM = 0;

bool watchdogCriticalAlarm = false;

uint32_t msec_millisecondCounter = 0;

extern bool Machine_Idle_Mode;

MillisecMotorDataStruc ScrewSetMaxSpeedPending = {0};
MillisecMotorDataStruc ScrewMovePending = {0};
MillisecMotorDataStruc MotorData[NUM_OF_MOTORS] = {0};
MillisecMotorDataStruc SpeedSetPending[NUM_OF_MOTORS] = {0};
MillisecMotorDataStruc PT100Data[MAX_TEMPERATURE_SENSOR_ID] = {0};
/******************** GLOBAL PARAMETERS  ********************************************/
Mailbox_Handle          MillisecMsgQ = NULL;
Mailbox_Handle          TenMillisecMsgQ = NULL;
Mailbox_Handle          MotorsMsgQ[NUM_OF_MOTORS] = {NULL};
bool                    MillisecRestart;
static GateMutex_Handle gateMillisecDB;

uint32_t        Millisec_timerBase = TIMER1_BASE;        //Timer handle
/******************** Functions  ********************************************/
uint32_t Control_Delta_Position_Pass(uint32_t Current_Read,uint32_t Previous_Read);
//**********************************************************************
/******************** CODE  ********************************************/
//**********************************************************************
#define LOG_SIZE 200
uint32_t MSBacklog[LOG_SIZE+1]={0};
uint8_t Motor_Id[LOG_SIZE+1]={0};
uint32_t MSTick[LOG_SIZE+1]={0};
uint16_t MsecLogindex = 0;

void MillisecInit(void)
{
    Error_Block eb;
    int i;

    Error_init(&eb);

    MillisecMsgQ = Mailbox_create(sizeof(MillisecMessageStruc), 2, NULL,&eb);
    TenMillisecMsgQ = Mailbox_create(sizeof(MillisecMessageStruc), 2, NULL,&eb);
    for (i=0;i<NUM_OF_MOTORS;i++)
    {
        MotorsMsgQ[i] = Mailbox_create(sizeof(MillisecMotorDataStruc), 5, NULL,&eb);
    }

    MillisecRestart = false;


    gateMillisecDB = GateMutex_create(NULL, &eb);
    if (gateMillisecDB == NULL)
    {
        System_abort("Could not create USB Wait gate");
    }

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

    ADCAcquireInit();

    return;
}
void MillisecStop(void)
{
    MillisecRestart = false;
    ADCAcquireStop();
}
#ifdef HUNDRED_MICROSECONDS_DANCER_READ
#define HIGH_TASK_FREQUENCY 12000
#else
#define HIGH_TASK_FREQUENCY 120000
#endif
void MillisecStart(void)
{
    MillisecRestart = true;

    ROM_TimerLoadSet(Millisec_timerBase, TIMER_A,HIGH_TASK_FREQUENCY);
    ROM_TimerEnable(Millisec_timerBase, TIMER_A);
    ROM_IntEnable(INT_TIMER1A);
    ROM_TimerIntEnable(Millisec_timerBase, TIMER_TIMA_TIMEOUT);
    ADCAcquireStart(0,1);

}

void OneMilliSecondMillisecInterrupt(UArg arg0)
{
    MillisecMessageStruc MillisecMessage;
    uint32_t Tenmsec_millisecondCounter = 0;
#ifndef HUNDRED_MICROSECONDS_DANCER_READ

    ROM_IntMasterDisable();
    ROM_TimerIntClear(Millisec_timerBase, TIMER_TIMA_TIMEOUT);  // Clear the timer interrupt
    if (MillisecRestart == true)
    {
        ROM_TimerLoadSet(Millisec_timerBase, TIMER_A,HIGH_TASK_FREQUENCY);
    }
    else
	{
        ROM_TimerDisable(Millisec_timerBase,TIMER_A);
	    ROM_IntMasterEnable();
		return;
	}
#endif
    if (msec_millisecondCounter%10 == 0)
    {
        Tenmsec_millisecondCounter = msec_millisecondCounter;
    }
    //send message to the Millisec task
    MillisecMessage.messageId = OneMillisec;
    MillisecMessage.tick = msec_millisecondCounter++;
    MillisecMessage.msglen = sizeof(MillisecMessageStruc);
    if (MillisecMsgQ != NULL)
        Mailbox_post(MillisecMsgQ , &MillisecMessage, BIOS_NO_WAIT);

    if (Tenmsec_millisecondCounter)
    {
        MillisecMessage.messageId = OneMillisec;
        MillisecMessage.tick = Tenmsec_millisecondCounter;
        MillisecMessage.msglen = sizeof(MillisecMessageStruc);
        if (TenMillisecMsgQ != NULL)
            Mailbox_post(TenMillisecMsgQ , &MillisecMessage, BIOS_NO_WAIT);
    }

    //
    // Enable all interrupts.
    //
    ROM_IntMasterEnable();
    return ;
}
uint32_t PT100Activity = 0;
int32_t MillisecReadFromTempSensor(uint32_t TempSensorId,  MSecFptr Callback)
{
    if (TempSensorId >= MAX_TEMPERATURE_SENSOR_ID) return -1;
    PT100Activity++; //read request
    PT100Data[TempSensorId].Callback = Callback;
    PT100Data[TempSensorId].Active = true;

    return OK;
}
//typedef  uint32_t (* MSecFptr)(uint32_t deviceID, uint32_t ReadValue);
uint32_t MotorActivity = 0;

int32_t MillisecSetMotorSpeed(TimerMotors_t MotorId, unsigned long Data, int Length, MSecFptr Callback)
{
    if (MotorId >= NUM_OF_MOTORS) return -1;
    //==========================
    if ((MotorId == HARDWARE_MOTOR_TYPE__MOTO_WINDER)||
        (MotorId == HARDWARE_MOTOR_TYPE__MOTO_RDRIVING)||
        (MotorId == HARDWARE_MOTOR_TYPE__MOTO_LDRIVING)||
		(MotorId == HARDWARE_MOTOR_TYPE__MOTO_SCREW))
    {
    MSBacklog[MsecLogindex]=Data;
    MSTick[MsecLogindex]=msec_millisecondCounter;
    Motor_Id[MsecLogindex]=MotorId;
    MsecLogindex++;
    if (MsecLogindex>=LOG_SIZE)
        MsecLogindex = 0;
    }
    //==========================

    SpeedSetPending[MotorId].Callback = Callback;
    SpeedSetPending[MotorId].Data = Data;
    SpeedSetPending[MotorId].Length = Length;
    SpeedSetPending[MotorId].DataRequired = false;
    if (SpeedSetPending[MotorId].Active == false)
    {
        MotorActivity++;
        SpeedSetPending[MotorId].Active = true;
    }

    return OK;
}
int MillisecFlushMsgQ(TimerMotors_t MotorId)
{
    MillisecMotorDataStruc MotorData = {0};
    int pend = Mailbox_getNumPendingMsgs(MotorsMsgQ[MotorId]);
    int i;
    if (pend)
    {
        for (i=0;i<pend;i++)
            Mailbox_pend(MotorsMsgQ[MotorId] , &MotorData, BIOS_NO_WAIT);
    }
   return pend;
}
int32_t MillisecWriteToMotor(TimerMotors_t MotorId, unsigned long Data, int Length, MSecFptr Callback)
{
    MillisecMotorDataStruc MotorData = {0};
    //==========================
    if ((MotorId == HARDWARE_MOTOR_TYPE__MOTO_WINDER)||
        (MotorId == HARDWARE_MOTOR_TYPE__MOTO_RDRIVING)||
        (MotorId == HARDWARE_MOTOR_TYPE__MOTO_LDRIVING)||
		(MotorId == HARDWARE_MOTOR_TYPE__MOTO_SCREW))
    {
    MSBacklog[MsecLogindex]=Data;
    MSTick[MsecLogindex]=msec_millisecondCounter;
    Motor_Id[MsecLogindex]=MotorId;
    MsecLogindex++;
    if (MsecLogindex>=LOG_SIZE)
        MsecLogindex = 0;
    }
    //==========================
    if (MotorId >= NUM_OF_MOTORS) return -1;
    MotorActivity++;
    MotorData.Callback = Callback;
    MotorData.Data = Data;
    MotorData.Length = Length;
    MotorData.DataRequired = false;
    if (MotorsMsgQ[MotorId] != NULL)
        return Mailbox_post(MotorsMsgQ[MotorId] , &MotorData, BIOS_NO_WAIT);
    else return false;
}
int32_t MillisecReadFromMotor(TimerMotors_t MotorId, unsigned long Data, int Length, MSecFptr Callback)
{
    MillisecMotorDataStruc MotorData = {0};
    if (MotorId >= NUM_OF_MOTORS) return -1;
//==========================
    if ((MotorId == HARDWARE_MOTOR_TYPE__MOTO_WINDER)||
        (MotorId == HARDWARE_MOTOR_TYPE__MOTO_RDRIVING)||
        (MotorId == HARDWARE_MOTOR_TYPE__MOTO_LDRIVING)||
		(MotorId == HARDWARE_MOTOR_TYPE__MOTO_SCREW))
    {
    MSBacklog[MsecLogindex]=Data;
    Motor_Id[MsecLogindex]=MotorId;
    MSTick[MsecLogindex]=msec_millisecondCounter;
    MsecLogindex++;
    if (MsecLogindex>=LOG_SIZE)
        MsecLogindex = 0;
    }
    //==========================
    MotorActivity++;
    MotorActivity++;
    MotorData.Callback = Callback;
    MotorData.Data = Data;
    MotorData.Length = Length;
    MotorData.DataRequired = true;
    if (MotorsMsgQ[MotorId] != NULL)
        return Mailbox_post(MotorsMsgQ[MotorId] , &MotorData, BIOS_NO_WAIT);
    else return false;
}

uint32_t MillisecLoop(uint32_t tick)
{
    uint8_t Motor_i;
    TEMPERATURE_SENSOR_ID_ENUM Sensor_i;
    unsigned int MotorInfo = 0;
    static int temp=0;
    //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,O900Millisecond_Tick,Tick98,OneMinute_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;
    OneMinute_Tick  =   (tick%eOneMinute == 0)           ?true:false;
    Tick98          =   (tick%eHundredMillisecond == 99) ?true:false;
    //gather Motor data from FPGA
    //ROM_IntMasterDisable();
*/
#ifndef EVALUATION_BOARD
        FPGA_GetBusy();  //load the busy motor information to all motors
        FPGA_Read_LS_Safty_Ind_Reg();
        //Read_FPGA_GPI_Rgisters();//FPGA_Read_limit_Switches();
#endif

        temp += MotorActivity;
   if (MotorActivity)
   {
        for (Motor_i = 0;Motor_i < NUM_OF_MOTORS;Motor_i++)
        {
            if (MotorDriverResponse[Motor_i].Busy == true)
            {
                temp++;
                continue;
            }
            if (MotorData[Motor_i].WaitForData == true) //Read request sent, data is waiting
            {
                if (MotorGetFPGAResponse((HardwareMotorType)Motor_i,&MotorInfo) == OK) //got the data from the FPGA
                {
                    MotorData[Motor_i].WaitForData = false;
                    if (MotorData[Motor_i].Callback)
                        MotorData[Motor_i].Callback(Motor_i,MotorInfo);
                }
                MotorActivity--;
            }
            if (SpeedSetPending[Motor_i].Active == true)
            {
                MotorSendFPGARequest((HardwareMotorType)Motor_i,SpeedSetPending[Motor_i].Data,SpeedSetPending[Motor_i].Length);
                MotorActivity--;
                SpeedSetPending[Motor_i].Active = false;
                if (SpeedSetPending[Motor_i].Callback)
                    SpeedSetPending[Motor_i].Callback(Motor_i,0);
            }
            else if (Mailbox_pend(MotorsMsgQ[Motor_i] , &MotorData[Motor_i], BIOS_NO_WAIT)==true)
            {
                if (MotorSendFPGARequest((HardwareMotorType)Motor_i,MotorData[Motor_i].Data,MotorData[Motor_i].Length) == OK) //sent the data to the FPGA
                {
                    if (MotorData[Motor_i].DataRequired == true)
                    {
                        MotorData[Motor_i].WaitForData = true; // mark the motor for data request next round
                    }
                    else
                    {
                        if (MotorData[Motor_i].Callback)
                            MotorData[Motor_i].Callback(Motor_i,0); // call the callback to report execution
                    }
                }
                MotorActivity--;
            }
        }
   }
    //FPGA_GetTempSensorBusy();
   //int PT100Busy[MAX_TEMPERATURE_SENSOR_ID] = {0,0,0,0,0,0,0,0,0,0};
   if (PT100Activity)
   {
       for (Sensor_i = 0;Sensor_i < MAX_TEMPERATURE_SENSOR_ID;Sensor_i++)
       {
           /*if (TempSensorResponse[Sensor_i].Busy == true)
           {
               PT100Busy[Sensor_i]++;
               continue;
           }*/
           if (PT100Data[Sensor_i].Active == true)
           {
               TemperatureSendSensorDummyClk(Sensor_i);
               PT100Data[Sensor_i].Active = false;
               PT100Data[Sensor_i].WaitForData = true; // mark the motor for data request next round
               break; //  one PT100 activitiy per MS 
           }
           else if (PT100Data[Sensor_i].WaitForData == true) //Read request sent, data is waiting
           {
               TemperatureSensorReadFromFPGA_Res(Sensor_i); //got the data from the FPGA
               PT100Data[Sensor_i].WaitForData = false;
               PT100Data[Sensor_i].SyncRequired = true;
               if (PT100Data[Sensor_i].Callback)
                       PT100Data[Sensor_i].Callback(Sensor_i,MotorInfo);
               break; // one PT100 activitiy per MS 
           }
           else if (PT100Data[Sensor_i].SyncRequired == true)
           {
               TemperatureSensorSync(Sensor_i);
               PT100Data[Sensor_i].SyncRequired = false;
               PT100Activity--;
               break; // one PT100 activitiy per MS 
           }
       }
   }
#ifdef HUNDRED_MICROSECONDS_DANCER_READ
   SaveLogData();
#else
    Dancer_Data[FEEDER_DANCER] = Read_Dancer_Position(FEEDER_DANCER);
    Dancer_Data[POOLER_DANCER] = Read_Dancer_Position(POOLER_DANCER);
    Dancer_Data[WINDER_DANCER] = Read_Dancer_Position(WINDER_DANCER);
#endif
    return OK;
}
int TemperatureSum[MAX_TEMPERATURE_SENSOR_ID];
int TemperatureMin[MAX_TEMPERATURE_SENSOR_ID];
int TemperatureMax[MAX_TEMPERATURE_SENSOR_ID];
int TemperatureCount[MAX_TEMPERATURE_SENSOR_ID];
int TemperatureCalc[MAX_TEMPERATURE_SENSOR_ID];
void MillisecUpdateTemperatures (TEMPERATURE_SENSOR_ID_ENUM SensorId,int temperature)
{
   //if(TemperatureCount[SensorId]++>=10)
   //    TemperatureCount[SensorId] = 0;
    TemperatureCount[SensorId]++;
    if (TemperatureMax[SensorId]<temperature) TemperatureMax[SensorId]=temperature;
    if (TemperatureMin[SensorId]>temperature) TemperatureMin[SensorId]=temperature;
    TemperatureSum[SensorId]+=temperature;
}
int MillisecCalculateTemperatures (TEMPERATURE_SENSOR_ID_ENUM SensorId)
{
    int calc = 0;
    TemperatureSum[SensorId]-=TemperatureMax[SensorId];
    TemperatureSum[SensorId]-=TemperatureMin[SensorId];
    calc = TemperatureSum[SensorId] / (TemperatureCount[SensorId]-2);

    TemperatureSum[SensorId] = 0;
    TemperatureCount[SensorId] = 0;
    TemperatureMin[SensorId] = 30000;
    TemperatureMax[SensorId] = -30000;
    return calc;
}
int MillisecGetTemperatures (TEMPERATURE_SENSOR_ID_ENUM SensorId)
{
    return TemperatureCalc[SensorId];
}
uint32_t MillisecLowLoop(uint32_t tick)
{
    uint8_t Motor_i,Disp_i,Heater_i,temp;
    TEMPERATURE_SENSOR_ID_ENUM Sensor_i;

    //static int temp=0;
    //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 , m90msecTick, Onesecond_Tick,O900Millisecond_Tick,OneMinute_Tick;
    Ten_msTick      =   (tick%eTenMillisecond == 0)      ?true:false;
    Hundred_msTick  =   (tick%eHundredMillisecond == 0)  ?true:false;
    m90msecTick  =   (tick%eHundredMillisecond == 90)  ?true:false;
    O900Millisecond_Tick  =   (tick%eOneSecond == 900)           ?true:false;
    Onesecond_Tick  =   (tick%eOneSecond == 0)           ?true:false;
    OneMinute_Tick  =   (tick%eOneMinute == 0)           ?true:false;
    //gather Motor data from FPGA
    //ROM_IntMasterDisable();

    if (Ten_msTick)
    {
        //Speed_Data = Calculate_Speed_Sensor_Velocity();
        //MillisecReadFromTempSensor(Sensor_Read, NULL);
        //if (Sensor_Read++ >= MAX_TEMPERATURE_SENSOR_ID) Sensor_Read = 0;
        if(Machine_Idle_Mode == true)
            Machine_Idle_Breathing_Led();
    }
    if (m90msecTick)
    {
        for (Sensor_i = 0;Sensor_i < MAX_TEMPERATURE_SENSOR_ID;Sensor_i++)
        {
            MillisecReadFromTempSensor(Sensor_i, NULL);
        }
    }
    if (Hundred_msTick)
    {
        Speed_Data = Calculate_Speed_Sensor_Velocity();
        Read_Buttons_Reg();
        for (Sensor_i = 0;Sensor_i < MAX_TEMPERATURE_SENSOR_ID;Sensor_i++)
        {
            MillisecUpdateTemperatures (Sensor_i,TemperatureSensorRead(Sensor_i));
        }
        if (GeneralHwReady == true)
        {
            if (watchdogCriticalAlarm == false)
            {
                Control_WD(ENABLE,5);  //activate heaters/dispenser watchdog, 0.5 seconds
                //LOG_ERROR (1111, "Control_WD");
            }
        }
    }

    if (O900Millisecond_Tick)
    {
        ADC_TriggerCollection();
    }
    if (Onesecond_Tick)
    {
        for (Disp_i = 0;Disp_i < MAX_SYSTEM_DISPENSERS;Disp_i++)
        {
            CalculateDispenserPressure(Disp_i);
            //Read_MidTank_Pressure_Sensor(Disp_i);
        }
        FPGA_GetAllDispensersValveBusyOCD();
        temp = Read_Fans_Tacho();
        DrawerFansStatus = temp & 0x1F;
        SystemFansStatus = temp & 0xE0;
        KeepAliveOneSecondCall();
        for (Motor_i = 0;Motor_i < NUM_OF_MOTORS;Motor_i++)
        {
            if (Motor_i == HARDWARE_MOTOR_TYPE__MOTO_SCREW)
                continue;  //
            if (isMotorConfigured(Motor_i))
                MotorGetStatusFromFPGA(Motor_i);
        }
        for (Heater_i = 0;Heater_i < NUM_OF_CURRENT_HEATERS;Heater_i++)
        {
            Read_Heaters_Current(Heater_i);
        }
        Gas_PPM = Calculate_Gas_Power_Consumption();
        for (Sensor_i = 0;Sensor_i < MAX_TEMPERATURE_SENSOR_ID;Sensor_i++)
        {
            TemperatureCalc[Sensor_i] =  MillisecCalculateTemperatures ( Sensor_i);
        }
    }
    if (OneMinute_Tick)
    {
        for (Disp_i = 0;Disp_i < MAX_SYSTEM_DISPENSERS;Disp_i++)
        {
            Read_MidTank_Pressure_Sensor(Disp_i);
        }
/*        for (Motor_i = 0;Motor_i < NUM_OF_MOTORS;Motor_i++)
        {
            if (Motor_i == HARDWARE_MOTOR_TYPE__MOTO_SCREW)
                continue;  //
            if (isMotorConfigured(Motor_i))
                MotorGetStatusFromFPGA(Motor_i);
        }*/
    }
    //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 MillisecTask(UArg arg0, UArg arg1)
{
    MillisecMessageStruc MillisecMessage;
    //char str[60];
    //uint16_t length;
    //Clock_setTimeout(HostKAClock, 1000);
    //Clock_start(HostKAClock);
    //MillisecInit();
    Millisecond_Task_Handle = Task_self();
    while(1)
    {
        Mailbox_pend(MillisecMsgQ , &MillisecMessage, BIOS_WAIT_FOREVER);
        switch (MillisecMessage.messageId)
        {
            case OneMillisec:
                MillisecLoop(MillisecMessage.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 MillisecLowTask(UArg arg0, UArg arg1)
{
    MillisecMessageStruc MillisecLowMessage;
    //char str[60];
    //uint16_t length;
    //Clock_setTimeout(HostKAClock, 1000);
    //Clock_start(HostKAClock);
    //MillisecInit();
    //Millisecond_Task_Handle = Task_self();
    while(1)
    {
        Mailbox_pend(TenMillisecMsgQ , &MillisecLowMessage, BIOS_WAIT_FOREVER);
        switch (MillisecLowMessage.messageId)
        {
            case OneMillisec:
                MillisecLowLoop(MillisecLowMessage.tick);
                break;
            default:
                break;
        }
    }
}


/*uint32_t getMotorStatusData(int MotorId)
{
    assert (MotorId < NUM_OF_MOTORS);
    return MotorStatus_Data[MotorId];
}*/
/*uint32_t getMotorSpeedData(int MotorId)
{
    assert (MotorId < NUM_OF_MOTORS);
    return MotorSpeed_Data[MotorId];
}
*/
/*uint32_t getTemperatureSensorData(int SensorId)
{
    assert (SensorId < MAX_TEMPERATURE_SENSOR_ID);
    return TemperatureSensor_Data[SensorId];
}*/

/*uint32_t getADCData(int DeviceId)
{
    assert (DeviceId < MAX_ADC_DEVICES);
    return ADC_Data[DeviceId];
}*/
float getSensorSpeedData(void)
{
    return Speed_Data;
}

uint32_t getDrawerFansStatus(void)
{
    return DrawerFansStatus;
}
uint8_t getGasReading(void)
{
    return Gas_PPM;
}

uint32_t getSystemFansStatus(void)
{
    return SystemFansStatus;
}
#ifdef HUNDRED_MICROSECONDS_DANCER_READ
uint32_t DancerData[NUM_OF_DANCERS];
uint32_t Control_Read_Dancer_Position(HardwareDancerType DancerId, uint32_t Parameter1, uint32_t Parameter2)
{
    return DancerData[DancerId];
}
uint32_t dancer1;
uint32_t dancer2;
uint32_t dancer3;
uint32_t dancer1sum;
uint32_t dancer2sum;
uint32_t dancer3sum;
uint32_t dancer_count;
/*-----------------------*/
uint32_t StoreBuffer[2][128];
//char * StoreBuffer[2][512];
/*-----------------------*/
int  StoreBufferId = 0;
int  StoreBufferCounter=0;
uint32_t  BufferCounter=0xEEEEEEEE;
bool storeData=false;
uint8_t len=0;
void HundredMicroTimerInterrupt(int ARG0)
{
    ROM_IntMasterDisable();
    ROM_TimerIntClear(Millisec_timerBase, TIMER_TIMA_TIMEOUT);  // Clear the timer interrupt
    if (MillisecRestart == true)
    {
        ROM_TimerLoadSet(Millisec_timerBase, TIMER_A,HIGH_TASK_FREQUENCY);
    }
    else
    {
        ROM_TimerDisable(Millisec_timerBase,TIMER_A);
        ROM_IntMasterEnable();
        return;
    }
    dancer1 = Read_Dancer_Position(WINDER_DANCER);
    dancer2 = Read_Dancer_Position(POOLER_DANCER);
    dancer3 = Read_Dancer_Position(FEEDER_DANCER);
//data store - logging
//double buffer switch
    /*-----------------------*/
    if (StoreBufferCounter>125)
    //if (StoreBufferCounter>490)
        /*-----------------------*/
    {
        StoreBufferId = 1-StoreBufferId;//switch buffer
        StoreBufferCounter=0;
        storeData = true;
    }
//double buffer initialize
    /*-----------------------*/
    if (StoreBufferCounter==0)
    {
        StoreBuffer[StoreBufferId][StoreBufferCounter] = BufferCounter++;
        StoreBufferCounter++;
    }
    /*-----------------------*/
//store data
    /*-----------------------*/
    StoreBuffer[StoreBufferId][StoreBufferCounter++] = dancer1;
    StoreBuffer[StoreBufferId][StoreBufferCounter++] = dancer2;
    StoreBuffer[StoreBufferId][StoreBufferCounter++] = dancer3;
    /*-----------------------*/
    //len = usprintf(&StoreBuffer[StoreBufferId][StoreBufferCounter], "\r\n%d %d %d", dancer1[dancer_count],dancer2[dancer_count],dancer3[dancer_count]);
    //StoreBufferCounter+=(len+1);
    /*-----------------------*/
    dancer1sum+=dancer1;
    dancer2sum+=dancer2;
    dancer3sum+=dancer3;
    dancer_count++;


    if (dancer_count == 10)
    {
        DancerData[WINDER_DANCER] = dancer1sum/dancer_count;
        DancerData[POOLER_DANCER] = dancer2sum/dancer_count;
        DancerData[FEEDER_DANCER] = dancer3sum/dancer_count;
        dancer_count = 0;
        dancer1sum = 0;
        dancer2sum = 0;
        dancer3sum = 0;
        OneMilliSecondMillisecInterrupt(ARG0);
    }
    ROM_IntMasterEnable();
    return ;

}
char MillisecPath[50] = "0://SysInfo//Millisec.txt";
FIL *FileHandle;
void SaveLogData(void)
{
    uint32_t WrittenBytes = 0;
    int BufferID = 1- StoreBufferId;

    if (storeData == true)
    {
        if (FileHandle)
        {
            f_write(FileHandle,StoreBuffer[BufferID],512,&WrittenBytes );
            storeData = false;
        }
    }
}
void MillisecLogInit(void)
{
    FRESULT Fresult = FR_OK;
    ROM_IntMasterDisable();
    BufferCounter = 0;
    FileHandle = my_malloc(sizeof(FIL));
    if (FileHandle == 0)
        Fresult = FR_DENIED;
    else
        Fresult = f_open(FileHandle,MillisecPath,FA_WRITE |  FA_OPEN_ALWAYS);
    ROM_IntMasterEnable();
    return ;

}
void MillisecLogClose(void)
{
    FRESULT Fresult = FR_OK;
    if (FileHandle == 0)
        Fresult = FR_DENIED;
    else
    {
        storeData = false;
        ROM_IntMasterDisable();
        Fresult = f_close(FileHandle);
        my_free(FileHandle);
        FileHandle = 0;
        ROM_IntMasterEnable();
    }
    return ;

}
#endif
void MillisecInterrupt(UArg arg0)
{
#ifdef HUNDRED_MICROSECONDS_DANCER_READ
    HundredMicroTimerInterrupt(arg0);
#else
    OneMilliSecondMillisecInterrupt(arg0);
#endif
}