CCS/LAUNCHXL-CC1310: RF_cmdPropTx transmits at wrong start time

#define RF_CTRL_SET_SWI_PRIORITY                  6

#include "smartrf_settings/smartrf_settings.h"
#include "Board.h"
#include "StateMachine.h"
#include <math.h>
#include <nodetube.h>
#include <msg.h>
//#include "rfSynchronizedPacket.h"

#include <N_rx_util.h>
#include <N_task.h>

/* stdlib */
#include <stdbool.h>
#include <stdlib.h>
#include <stdint.h>
#include <assert.h>

/* TiRTOS Kernel */
#include <ti/sysbios/BIOS.h>
#include <ti/sysbios/knl/Task.h>

/* High-level Ti-Drivers */
#include <ti/drivers/pin/PINCC26XX.h>
#include <ti/drivers/rf/RF.h>
#include <driverlib/sys_ctrl.h>
#include <driverlib/rom.h>

#include <ti/devices/DeviceFamily.h>
#include DeviceFamily_constructPath(driverlib/rf_common_cmd.h)
#include DeviceFamily_constructPath(driverlib/rf_mailbox.h)
#include DeviceFamily_constructPath(driverlib/rf_prop_cmd.h)
#include DeviceFamily_constructPath(driverlib/rf_prop_mailbox.h)


uint32_t rxTimestamp;
uint32_t txTimestamp;
uint32_t verifyTime;
uint32_t vTime;
uint8_t invCount,invCount2,invCount3,invCount4,invCount5,invCount6;
uint8_t routeFlag;
uint8_t checkResp=0;
uint8_t firstNode;
uint32_t sync_TT1 = 0;
uint32_t calTime=0;
uint32_t diffTime=0;
uint8_t nName;
uint8_t narr[10];
uint8_t NodeDTCnt[10];
uint8_t wakeFlag=1;
uint8_t NetworkFail=0, target=0, PartialTCnt=0;
uint8_t p, cnt_nodes=0, node_pos=0, changeTcounter=0, ccnt =0, nonewnode=0, newcount=0;
uint8_t PacketSize = 13;
uint8_t missedCounter=0, packetrec=0, q=0, k=0, wait_gnd=0;
uint8_t try =0, proxy=0;
uint8_t inc=0, resetcounter=0, redundant=0,resume_gnd=0;
uint8_t AltNarr[10];
uint8_t lastcounter=0, repeat_node=0,mpn=0;
uint32_t OrderSentTime=0, time_now=0;

uint32_t ntime =0;
uint32_t qtime =0;

uint32_t currentTime;
uint32_t commandTime;

//************************** Clock event for update of received data**************************************/

static Clock_Struct stateClock;
uint8_t holdID;
uint8_t holdsrc;

/*************************** Advanced TX command for sending long preamble *******************************/
//rfc_CMD_PROP_TX_t RF_cmdPropTx;

//enum rf_mode  {
//    RX_MODE,
//    TX_MODE,
//};

uint16_t me = 0;

/************************ Define events that can be posted to the application state machine ***************/
typedef enum {
    Event_PacketReceived = StateMachine_Event00,
    Event_SyncMissed = StateMachine_Event01,
    Event_NewMissed = StateMachine_Event02,
} Event;

/******************************* Declare state handler functions for the application. **********************/

StateMachine_DECLARE_STATE(PrepareSyncedRxState);
StateMachine_DECLARE_STATE(PrepareSyncedTxState);
StateMachine_DECLARE_STATE(WaitingForSyncState);
StateMachine_DECLARE_STATE(SyncedRxState);
StateMachine_DECLARE_STATE(SyncedTxRxState);
StateMachine_DECLARE_STATE(PreapareSyncedRxTxState);
StateMachine_DECLARE_STATE(PeriodicBeaconState);

/***** Defines *****/
#define MAIN_TASK_STACK_SIZE 1024
#define MAIN_TASK_PRIORITY   2

/* Packet RX Configuration */
#define NUM_APPENDED_BYTES     5  /* The Data Entries data field will contain:
 * - 1 address byte in the header (RF_cmdPropRx.rxConf.bIncludeHdr = 0x1)
 * - up to sizeof BeaconPacket bytes payload
 * - 4 bytes for the RAT time stamp (RF_cmdPropRx.rxConf.bAppendTimestamp = 0x1) */

#define SYMBOL_RATE            200000//50000                    /* 50 kbits per second */
#define US_PER_SYMBOL          (1000000 / SYMBOL_RATE)
#define PREAMBLE_BITS          32
#define SYNCWORD_BITS          32
#define BEACON_INTERVAL_MS     2000//2000

#define RX_START_MARGIN        RF_convertUsToRatTicks(500)   /* An arbitrarily chosen value to compensate for
 * the potential drift of the RAT and the RTC. */

#define RX_TIMEOUT_TICKS       RF_convertUsToRatTicks((PREAMBLE_BITS + SYNCWORD_BITS) * US_PER_SYMBOL)
/* Tight, but ideal duration for receiving all bits of
 * the preamble and the sync word. */

#define RX_TIMEOUT_MARGIN      RF_convertUsToRatTicks(1000)  /* Arbitrarily chosen margin added to the RX timeout
 * to compensate calculation errors. */

#define RX_START_TO_SETTLE_TICKS   256 /* Time between RX start trigger and the radio
 * being ready to receive the first preamble bit.
 * This is a fixed value for CMD_PROP_RX. */
#define TX_START_TO_PREAMBLE_TICKS 384 /* Time between TX start trigger and first bit on air.
 * This is a fixed value for CMD_PROP_TX. */
/***** Variable declarations *****/
Task_Struct mainTask;    /* not static so you can see in ROV */
static uint8_t mainTaskStack[MAIN_TASK_STACK_SIZE];

static StateMachine_Struct stateMachine;

static RF_Object rfObject;
static RF_Handle rfHandle;

/* Queue object that the RF Core will fill with data */
static dataQueue_t rxQueue;

/* A single queue item that points to a data buffer  */
static rfc_dataEntryPointer_t rxItem;

/* Word-aligned buffer for the packet payload + meta data. */
static uint8_t rxBuffer[((sizeof(BeaconPacket) + NUM_APPENDED_BYTES) + 8)];

BeaconPacket beacon;
nodeQ* head;

/***** Prototypes *****/
//void StateTask_init();
void WORcallback(RF_Handle h, RF_CmdHandle ch, RF_EventMask e);
void PrepareSyncedRxState_function();
void PrepareSyncedTxState_function();
void PeriodicBeaconState_function();
void SyncedRxState_rxCallback(RF_Handle h, RF_CmdHandle ch, RF_EventMask e);
void SyncedTxRxState_txrxCallback(RF_Handle h, RF_CmdHandle ch, RF_EventMask e);
void WaitingForSyncState_rxCallback(RF_Handle h, RF_CmdHandle ch, RF_EventMask e);
void configureTx();
void TransmitTx();
void configureRx();
static void ClockSwiFxn(uintptr_t arg0);
void errorCallback(RF_Handle h, RF_CmdHandle ch, RF_EventMask e);


/*****************************INITIALIZE MACHINE STATE TASK*************************************************************************/
/* Comment: set to PrepareSyncedTxState
 ************************************************************************************************************************************/

void StateTask_init(PIN_Handle ledPinHandle)
{


    //******************** Initialize Clock for Timouts************************************/
    Clock_Params clockParams;
    Clock_Params_init(&clockParams);

    //*********************Target Response Clock*******************************************/
    clockParams.period = 20 * (1000/Clock_tickPeriod), // 1000 ms, conversion from ms to clock ticks.
            // Initialize the clock object / Clock_Struct previously added globally.
            Clock_construct(&stateClock, ClockSwiFxn,
                            0, // Initial delay before first timeout
                            &clockParams);

    pinHandle = ledPinHandle;

    //*********************** Initialise the application state machine.********************/
    StateMachine_construct(&stateMachine);

    /* Initialise the main task. It will execute
     * the state machine StateMachine_exec() function.
     */
    Task_Params params;
    Task_Params_init(&params);
    params.stackSize = MAIN_TASK_STACK_SIZE;
    params.priority = MAIN_TASK_PRIORITY;
    params.stack = &mainTaskStack;
    params.arg0 = (UArg)&stateMachine;
    params.arg1 = (UArg)PrepareSyncedTxState_function;
    Task_construct(&mainTask, (Task_FuncPtr)&StateMachine_exec, &params, NULL);
}

/*****************************PREPARED_SYNCED_RX_STATE_FUNCTION*************************************************************************/
/* Comment:
 ***************************************************************************************************************************************/

void PrepareSyncedRxState_function()
{
    if(tricklastnode==1)
        Semaphore_pend(semTxHandle, BIOS_WAIT_FOREVER); //proper fix later

    /************************* Construct a circular RX queue with a single pointer-entry item. ******************/
    rxItem.config.type = DATA_ENTRY_TYPE_PTR;
    rxItem.config.lenSz = 0;
    rxItem.length = sizeof(rxBuffer);
    rxItem.pNextEntry = (uint8_t*)&rxItem;
    rxItem.pData = (uint8_t*)&rxBuffer[0];
    rxItem.status = DATA_ENTRY_PENDING;
    rxQueue.pCurrEntry = (uint8_t*)&rxItem;
    rxQueue.pLastEntry = NULL;

    /************************* Modify CMD_PROP_RX command for application needs **********************************/
    RF_cmdPropRx.pQueue = &rxQueue;                /* Set the Data Entity queue for received data */
    RF_cmdPropRx.rxConf.bAutoFlushIgnored = true;  /* Discard ignored packets from Rx queue */
    RF_cmdPropRx.rxConf.bAutoFlushCrcErr = true;   /* Discard packets with CRC error from Rx queue */
    RF_cmdPropRx.rxConf.bIncludeHdr = true;        /* Put length field in front of queue entries. */
    RF_cmdPropRx.rxConf.bAppendTimestamp = true;   /* Append RX time stamp to the packet payload */
    RF_cmdPropRx.maxPktLen = sizeof(BeaconPacket); /* Implement packet length filtering to avoid PROP_ERROR_RXBUF */
    RF_cmdPropRx.pktConf.bRepeatOk = false;        /* Stop after receiving a single valid packet */
    RF_cmdPropRx.pktConf.bRepeatNok = true;
    RF_cmdPropRx.pOutput   = (uint8_t*)&rxStatistics;

    /***************** Request access to the radio. This does not power-up the RF core, but only initialise
     * the driver and cache the setup command. ********************************************************************/
    RF_Params rfParams;
    //rfParams.nPowerUpDurationMargin = 315;
    RF_Params_init(&rfParams);
    rfHandle = RF_open(&rfObject, &RF_prop, (RF_RadioSetup*)&RF_cmdPropRadioDivSetup, &rfParams);
    assert(rfHandle != NULL);

    /* Set the frequency. Now the RF driver powers the RF core up and runs the setup command from above.
     * The FS command is executed and also cached for later use when the RF driver does an automatic
     * power up. */
    RF_EventMask result = RF_runCmd(rfHandle, (RF_Op*)&RF_cmdFs, RF_PriorityNormal, NULL, 0);
    assert((result == RF_EventLastCmdDone) && ((volatile RF_Op*)&RF_cmdFs)->status == DONE_OK);

    /* Route the LNA signal to an LED to indicate that the RF core is
     * active and receiving data.
     * Available signals are listed in the proprietary RF user's guide.
     */
    PINCC26XX_setMux(pinHandle, Board_LED2, PINCC26XX_MUX_RFC_GPO0);

    StateMachine_setNextState(&stateMachine, WaitingForSyncState);
}

/*****************************************PREPARE_SYNCED_TX_STATE FUNCTION***************************************************************/
/* Comment:
/****************************************************************************************************************************************/

void PrepareSyncedTxState_function()
{
    Semaphore_pend(semRxHandle, BIOS_WAIT_FOREVER);



    /******************************* Prepare the packet *********************************/
    RF_cmdPropTx.pktLen = sizeof(beacon);
    RF_cmdPropTx.pPkt = (uint8_t*)&beacon;
    RF_cmdPropTx.startTrigger.triggerType = TRIG_ABSTIME;
    RF_cmdPropTx.startTime = 0;

    //    RF_cmdPropTx.startTrigger.pastTrig = 1;
    beacon.beaconInterval = RF_convertMsToRatTicks(BEACON_INTERVAL_MS);

    if(routeFlag)
    {
        //prepare routing packet*******************************/
        beacon.msg.startbit = RF_STX;
        beacon.msg.mid = MSGID_REQ_NARR_AGAIN;
        beacon.msg.src = theNode.myid;
        beacon.msg.des = 0xFF;
        beacon.msg.msgl= newcount+cnt_nodes-(node_pos);//node_count;//0x05;
        uint8_t nCon = 0x0B;

        nodeQ* temp;
        temp = head;

        for (q=0; q<newcount; q++)
        {
            beacon.msg.data[q]=temp->nodeid;
            temp = temp->next;
        }

        for (q=0; q<newcount; q++)
        {
            if((beacon.msg.data[q] & 0xF0)==0xC0)
            {
                if(q==0)
                    beacon.msg.data[0]=beacon.msg.data[q];
                else
                {
                    firstNode =  beacon.msg.data[0];
                    beacon.msg.data[0]=beacon.msg.data[q];
                    for(k=1;k<q;k++)
                        beacon.msg.data[k+2]=beacon.msg.data[k];
                    beacon.msg.data[1]=firstNode;
                }
            }
        }
        for(q=0; q<(cnt_nodes-node_pos); q++)
        {
            beacon.msg.data[q+newcount] = (nCon<<4) | (narr[node_pos+q]);
        }
        for(q=0; q<(newcount+cnt_nodes-(node_pos)); q++)
        {
            narr[q]=beacon.msg.data[q];
        }
        cnt_nodes=newcount+cnt_nodes-(node_pos);
        node_pos=newcount;
    }

    if(wait_gnd)
    {
        uint8_t nCon = 0x0B;
        beacon.msg.startbit = RF_STX;
        beacon.msg.mid = MSGID_SYSTEM_RESET_FB;//MSGID_ACK_PRESENCE;
        beacon.msg.src = theNode.myid;
        beacon.msg.des = mpn;//(nCon<<4) | target;
        beacon.msg.msgl= 0x04;
    }
    if(resume_gnd)
    {
        uint8_t nCon = 0x0B;
        beacon.msg.startbit = RF_STX;
        beacon.msg.mid = MSGID_DATA_SENSOR;//MSGID_ACK_PRESENCE;
        beacon.msg.src = theNode.myid;
        beacon.msg.des = (nCon<<4) | target;
        beacon.msg.msgl= 0x04;
    }

    /*******************************************************************************************************************************************/
    //RF_close(rfHandle);
    /* Request access to the radio. This does not power-up the RF core, but only initialise
     * the driver and cache the setup command. */
    RF_Params rfParams;
    //    rfParams.nPowerUpDurationMargin = 5000;
    //    rfParams.pErrCb = errorCallback;
    //    rfParams.nPowerUpDuration = 10000;
    RF_Params_init(&rfParams);
    rfHandle = RF_open(&rfObject, &RF_prop, (RF_RadioSetup*)&RF_cmdPropRadioDivSetup, &rfParams);
    assert(rfHandle != NULL);

    /* Increase the SWI priority of the internal state machine in the RF driver. The default
     * value is 0. This is necessary because the button callback blocks for a long time and
     * might disturb the RF driver state machine.
     */
    uint32_t swiPriority = 1;
    RF_control(rfHandle, RF_CTRL_SET_SWI_PRIORITY, &swiPriority);

    /* Set the frequency. Now the RF driver powers the RF core up and runs the setup command from above.
     * The FS command is executed and also cached for later use when the RF driver does an automatic
     * power up. */
    RF_EventMask result = RF_runCmd(rfHandle, (RF_Op*)&RF_cmdFs, RF_PriorityNormal, NULL, 0);
    assert((result == RF_EventLastCmdDone) && ((volatile RF_Op*)&RF_cmdFs)->status == DONE_OK);

    /* Use the current time as an anchor point for future time stamps.
     * The Nth transmission in the future will be exactly N * 500ms after
     * this time stamp.  */
    RF_cmdPropTx.startTime = RF_getCurrentTime();
    ntime = RF_cmdPropTx.startTime;

    /* A trigger in the past is triggered as soon as possible.
     * No error is given.
     * This avoids assertion when button debouncing causes delay in TX trigger.  */
    //RF_cmdPropTx.startTrigger.pastTrig = 1;
    RF_cmdPropTx.startTrigger.pastTrig = 1;

    /* Route the PA signal to an LED to indicate ongoing transmissions.
     * Available signals are listed in the proprietary RF user's guide.
     */
    PINCC26XX_setMux(pinHandle, Board_LED2, PINCC26XX_MUX_RFC_GPO1);

    StateMachine_setNextState(&stateMachine, PeriodicBeaconState);
}

/*****************************ERROR CALLBACK FUNCTION*************************************************************************/
/* Comment:
/***************************************************************************************************************************************/

void errorCallback(RF_Handle h, RF_CmdHandle ch, RF_EventMask e) {
    if ((int32_t)ch == RF_ERROR_CMDFS_SYNTH_PROG)
    {
        while(1);
        // Handle error
        // If CMD_FS is executed standalone, re-start CMD_FS in this function if there is time. If not, handle as in previous example
        // When CMD_FS is chained with RX/TX commands, the RX/TX is executed in parallel with this callback by the radio and will subsequently fail. Error can be handled as in previous example.
    }
    else
        while(1);

}

/*****************************WAITING_FOR_SYNC_STATE_FUNCTION*************************************************************************/
/* Comment:
/***************************************************************************************************************************************/

void WaitingForSyncState_function()
{

    rxItem.status = DATA_ENTRY_PENDING;

    /***************************** Start RX command to receive a single packet. **************************/
    RF_cmdPropRx.startTrigger.triggerType = TRIG_NOW;
    if((tricklastnode) & (wait_gnd == 0))
    {
        RF_cmdPropRx.endTrigger.triggerType = TRIG_REL_START;
        RF_cmdPropRx.endTrigger.pastTrig = 1;
        RF_cmdPropRx.endTime = 12000000;
    }
    else if((missedCounter>node_pos) & (wait_gnd == 0))
    {
        RF_cmdPropRx.endTrigger.triggerType = TRIG_REL_START;
        RF_cmdPropRx.endTrigger.pastTrig = 1;
        RF_cmdPropRx.endTime = 20000000 + (node_pos*12000000);
    }
    else if(wait_gnd)
    {
        RF_cmdPropRx.endTrigger.triggerType = TRIG_NEVER;
        //wait_gnd=0;
    }
    else
        RF_cmdPropRx.endTrigger.triggerType = TRIG_NEVER;

    RF_postCmd(rfHandle, (RF_Op*)&RF_cmdPropRx, RF_PriorityNormal, &WaitingForSyncState_rxCallback, RF_EventRxEntryDone);

    for (;;)
    {

        time_now = RF_getCurrentTime();
        if(nonewnode & tricklastnode)
        {
            nonewnode=0;
            if(checkResp==1)
            {
                if(proxy)
                {
                    tricklastnode=0;
                    proxy=0;
                }
                routeFlag =1;
                lastcounter=0;
                checkResp=0;
                StateMachine_setNextState(&stateMachine, PrepareSyncedTxState);
                Semaphore_post(semRxHandle);
            }
            else
            {
                routeFlag=0;
                //tricklastnode=1; //how does affect other scenarios
                missedCounter=0;
                inc=0;
                lastcounter=0;
                StateMachine_setNextState(&stateMachine, PrepareSyncedTxState);
                Semaphore_post(semRxHandle);
                //nName=0xB0 + narr[node_pos+1];
                //break;
            }
            RF_close(rfHandle);
            break;

        }
        if(packetrec)
        {
            packetrec=0;
            StateMachine_EventMask events = StateMachine_pendEvents(&stateMachine, Event_PacketReceived| Event_SyncMissed | Event_NewMissed, BIOS_WAIT_FOREVER);

            if (events & Event_PacketReceived)
            {
                /********************* RX command has already stopped. Now examine the received data. *************************/
                uint8_t length;
                uint32_t rxTime;

                memcpy(&length, rxBuffer, 1);
                if (length != sizeof(BeaconPacket))
                {
                    // This packet is not for us. Wait for next one
                    StateMachine_setNextState(&stateMachine, WaitingForSyncState);
                    break;
                }

                memcpy(&beacon, (uint8_t*)rxBuffer + 1, sizeof(BeaconPacket));
                memcpy(&rxTime, (uint8_t*)rxBuffer + 1 + sizeof(BeaconPacket), 4);

                PIN_setOutputValue(pinHandle, Board_LED2,!PIN_getOutputValue(Board_LED2));

                if((beacon.msg.mid==MSGID_REQ_NARR) || (beacon.msg.mid==MSGID_REQ_NARR_AGAIN))
                {
                    if((beacon.msg.mid==MSGID_REQ_NARR_AGAIN))
                    {
                        for(p=0; p<cnt_nodes; p++)
                            narr[p] = 0x00; //a router table reset here
                        cnt_nodes=0;
                    }
                    //condition if its just the sensor node
                    if((beacon.msg.data[0] & 0xF0) ==0xC0)
                    { p=1;
                    cnt_nodes=1;
                    narr[0] = (beacon.msg.data[0] & 0x0F);
                    }
                    else
                        p=0; //reset p

                    for(p; p<beacon.msg.msgl; p++ )
                    {
                        if((beacon.msg.data[p] & 0xF0)==0xB0)
                            cnt_nodes++;
                        narr[p] = (beacon.msg.data[p] & 0x0F); //populate with the NNs the B's
                    }
                    //*************************************you're the sensor node************************/
                    if(beacon.msg.data[0]==theNode.myid)
                    {
                        if((beacon.msg.mid==MSGID_REQ_NARR_AGAIN) & (cnt_nodes<2))
                            nName=beacon.msg.src;
                        else if((beacon.msg.mid==MSGID_REQ_NARR) & (cnt_nodes<2))
                            nName = 0xA1;
                        else
                            nName = beacon.msg.data[1];

                        if((theNode.myid & 0xF0)!=0xC0)//you are unofficial sensor node OxBX
                            tricklastnode=1;

                        beacon.msg.mid = MSGID_DATA_SENSOR;
                        StateMachine_setNextState(&stateMachine, PrepareSyncedTxState);
                        RF_close(rfHandle);
                        Semaphore_post(semRxHandle);
                        node_pos=0;
                        break;
                    }

                    for(p=1; p<(cnt_nodes-1); p++ )
                    {
                        if(beacon.msg.data[p]==theNode.myid)
                        {
                            nName = beacon.msg.data[p+1];
                            node_pos = p;
                        }
                    }
                    if(beacon.msg.data[(cnt_nodes-1)]==theNode.myid)
                    {
                        nName = 0xA1;
                        node_pos = cnt_nodes - 1;
                    }
                }

                if(beacon.msg.mid==MSGID_SYSTEM_RESET_FB  && beacon.msg.des == theNode.myid)
                {
                    if((theNode.myid & 0xF0) == 0xC0)
                    {
                        beacon.msg.mid = MSGID_DATA_SENSOR;
                        wait_gnd=0;
                        resume_gnd=1;
                        StateMachine_setNextState(&stateMachine, PrepareSyncedTxState);
                        Semaphore_post(semRxHandle);
                        RF_close(rfHandle);
                        break;
                    }
                    else if(((theNode.myid & 0xF0) == 0xB0) && (tricklastnode==1))
                    {
                        beacon.msg.mid = MSGID_DATA_SENSOR;
                        wait_gnd=0;
                        resume_gnd=1;
                        StateMachine_setNextState(&stateMachine, PrepareSyncedTxState);
                        Semaphore_post(semRxHandle);
                        RF_close(rfHandle);
                        break;
                    }
                    else
                    {
                        //wait_gnd=0;
                        StateMachine_setNextState(&stateMachine, PrepareSyncedTxState);
                        Semaphore_post(semRxHandle);
                        RF_close(rfHandle);
                        break;
                    }
                }

                if(beacon.msg.mid==MSGID_DATA_SENSOR  && beacon.msg.des == theNode.myid)
                {
                    //if(beacon.msg.des==theNode.myid)
                    mpn=beacon.msg.src;
                    {
                        missedCounter=0;
                        changeTcounter=0;
                        inc=0;

                        RF_cmdPropRx.startTime = rxTime - RX_START_TO_SETTLE_TICKS - RX_START_MARGIN;
                        target = (theNode.myid & 0x0F);
                        StateMachine_setNextState(&stateMachine, SyncedTxRxState);
                        break;
                    }
                }

                if(beacon.msg.mid==MSGID_ACK_PRESENCE  && beacon.msg.des == theNode.myid)
                {
                    if((theNode.myid & 0xF0) == 0xC0)
                        wait_gnd=0;
                    //ntime=0;
                    StateMachine_setNextState(&stateMachine, PrepareSyncedTxState);
                    Semaphore_post(semRxHandle);
                    break;
                }
                //                if(beacon.msg.mid==ID_SYSTEM_RESET  && beacon.msg.des == theNode.myid)
                //                {
                //                    SysCtrlSystemReset();
                //                }

                //Arrangement of the normal nodes and sensor nodes based on RSSI*********************************************************/
                if((beacon.msg.mid == MSGID_REQ_JOIN_AGAIN && (beacon.msg.src & 0xF0)==0xB0 && beacon.msg.des==theNode.myid) || (beacon.msg.mid == MSGID_REQ_JOIN_AGAIN && (beacon.msg.src & 0xF0)==0xC0 && beacon.msg.des==theNode.myid))
                {
                    repeat_node=0;
                    if((beacon.msg.src & 0xF0)==0xC0)
                        proxy=1;
                    if((beacon.msg.src & 0xF0)!=0xC0)
                    {
                        for(p=0; p<(cnt_nodes-node_pos); p++)
                        {
                            if((beacon.msg.src & 0x0F)==narr[p+node_pos])
                                repeat_node=1;
                        }
                    }

                    if(repeat_node==0)
                    {
                        head = prepend(head,beacon.msg.src,RSSIout);
                        head = insertion_sort(head);

                        newcount++;
                    }
                    missedCounter=0;
                    inc=0;
                    checkResp=1;
                }

                if(missedCounter>node_pos)
                {

                    if(beacon.msg.mid==MSGID_DATA_SENSOR & beacon.msg.des != theNode.myid )
                    {
                        changeTcounter++;
                        if( (beacon.msg.des & 0x0F)==narr[node_pos-inc])
                        {
                            changeTcounter=0;
                            target = narr[node_pos-inc];
                            missedCounter=0;
                            inc=0;
                            RF_cmdPropRx.startTime = rxTime - RX_START_TO_SETTLE_TICKS - RX_START_MARGIN;

                            StateMachine_setNextState(&stateMachine, SyncedTxRxState);
                            break;
                        }
                        else if((changeTcounter>node_pos*2) & ((node_pos-inc) !=0))
                        {
                            inc++;
                            changeTcounter=0;
                            StateMachine_setNextState(&stateMachine, WaitingForSyncState);
                            break;
                        }
                        else if((node_pos-inc) ==0)
                        {
                            changeTcounter=0;
                            target = narr[node_pos-inc];
                            missedCounter=0;
                            inc=0;
                            RF_cmdPropTx.startTime = RF_getCurrentTime();
                            StateMachine_setNextState(&stateMachine, PeriodicBeaconState);
                            break;
                        }
                    }
                }


                /* rxTime contains a calculated time stamp when the first preamble byte
                 * was sent on air. As a time base for the next wake ups, we calculate
                 * the time when this RX command would have been started for a synchronized
                 * wake up. */

                RF_cmdPropRx.startTime = rxTime - RX_START_TO_SETTLE_TICKS - RX_START_MARGIN;

                StateMachine_setNextState(&stateMachine, WaitingForSyncState);
                break;
            }
            if (events & Event_SyncMissed)
            {
                RF_cmdPropTx.startTime = RF_getCurrentTime();
                StateMachine_setNextState(&stateMachine, PeriodicBeaconState);
                tricklastnode=1;

                break;
            }
        }
    }
}

/*****************************************WAITING_FOR_SYNC_STATE_RXCALLBACK FUNCTION***************************************************************/
/* Comment:
/****************************************************************************************************************************************/

void WaitingForSyncState_rxCallback(RF_Handle h, RF_CmdHandle ch, RF_EventMask e)
{
    if (e & RF_EventLastCmdDone)
    {
        if ((RF_cmdPropRx.status == PROP_DONE_OK) || (RF_cmdPropRx.status == PROP_DONE_ENDED))
        {
            /**************************** Sync word has been found before end trigger and packet has been received ******************/
            StateMachine_postEvents(&stateMachine, Event_PacketReceived);
            packetrec=1;
        }
        else
        {
            if(tricklastnode)
            {
                nonewnode=1;
                //StateMachine_setNextState(&stateMachine, WaitingForSyncState);
            }
            else if((missedCounter>node_pos) & (!tricklastnode))
            {
                StateMachine_postEvents(&stateMachine, Event_SyncMissed);
                packetrec=1;
            }

            //StateMachine_setNextState(&stateMachine, PeriodicBeaconState);
            // Everything else is an error.
            else
                assert(false);
        }
    }
}
// Remember to fix gap

/*****************************************SYNCED_RX_STATE FUNCTION***************************************************************/
/* Comment:
/****************************************************************************************************************************************/

void SyncedRxState_function()
{
    rxItem.status = DATA_ENTRY_PENDING;


    /* Start RX command to receive a single packet. Use an absolute start trigger
     * and a predicted start time. The end time is calculated to be as tight as possible.
     * The compensation margin for RAT drift in both directions needs to be taken into account.  */
    RF_cmdPropRx.startTrigger.triggerType = TRIG_ABSTIME;
    RF_cmdPropRx.startTime += beacon.beaconInterval;
    RF_cmdPropRx.endTrigger.triggerType = TRIG_REL_START;
    RF_cmdPropRx.endTime = RX_START_TO_SETTLE_TICKS + RX_TIMEOUT_TICKS + RX_TIMEOUT_MARGIN + RX_START_MARGIN;

    /* Puts the RX command into the driver queue. Since the start trigger is absolute and has
     * a time somewhere in the future, the RF driver will power down the RF core and wait short
     * before the RX command is due. Then it will run the power-up sequence and dispatch the RX
     * command right on time.
     */
    RF_CmdHandle cmd = RF_postCmd(rfHandle, (RF_Op*)&RF_cmdPropRx, RF_PriorityNormal, &SyncedRxState_rxCallback, RF_EventRxEntryDone);

    for (;;)
    {
        StateMachine_EventMask events = StateMachine_pendEvents(&stateMachine, Event_PacketReceived | Event_SyncMissed, BIOS_WAIT_FOREVER);

        if (events & Event_PacketReceived)
        {
            /* RX command has already stopped. Now examine the received data. */
            uint8_t length;
            uint32_t rxTime;

            memcpy(&length, rxBuffer, 1);
            if (length != sizeof(BeaconPacket))
            {
                // This packet is not for us. Wait for next one
                StateMachine_setNextState(&stateMachine, SyncedRxState);
                break;
            }
            memcpy(&beacon, (uint8_t*)rxBuffer + 1, sizeof(BeaconPacket));
            memcpy(&rxTime, (uint8_t*)rxBuffer + 1 + sizeof(BeaconPacket), 4);

            /* The synchronisation offset might be used to calculate the clock drift between tranStateMachine_itter and receiver. */
            int32_t syncOffsetTime = rxTime - RX_START_TO_SETTLE_TICKS /*RX_START_MARGIN*/ - RF_cmdPropRx.startTime;
            (void)syncOffsetTime; // need to reference the variable to prevent from a compiler warning

            /* Do an immediate re-synchronisation based on the new RX time. */
            RF_cmdPropRx.startTime = rxTime - RX_START_TO_SETTLE_TICKS - RX_START_MARGIN;// - syncOffsetTime;

            //PIN_setOutputValue(pinHandle, Board_LED1, beacon.ledState);
            PIN_setOutputValue(pinHandle, Board_LED1,!PIN_getOutputValue(Board_LED1));

            /* Wait for the next packet */
            StateMachine_setNextState(&stateMachine, SyncedRxState);
            break;
        }

        if (events & Event_SyncMissed)
        {
            /* Sync is missed. That means either we are out of sync or the
             * tranStateMachine_itter is in spontaneous mode. Try to receive the next packet. */
            StateMachine_setNextState(&stateMachine, SyncedRxState);
            me++;
            if(me==5)
                StateMachine_setNextState(&stateMachine, WaitingForSyncState);
            break;
        }
    }
}

/*****************************************SYNCED_RX_STATE_RXCALLBACK FUNCTION***************************************************************/
/* Comment:
/****************************************************************************************************************************************/

void SyncedRxState_rxCallback(RF_Handle h, RF_CmdHandle ch, RF_EventMask e)
{
    if (e & RF_EventLastCmdDone)
    {
        if ((RF_cmdPropRx.status == PROP_DONE_OK) || (RF_cmdPropRx.status == PROP_DONE_ENDED))
        {
            /* Sync word has been found before end trigger and packet has been received */
            StateMachine_postEvents(&stateMachine, Event_PacketReceived);
        }
        else if (RF_cmdPropRx.status == PROP_DONE_RXTIMEOUT)
        {
            StateMachine_postEvents(&stateMachine, Event_SyncMissed);
        }
    }
}

/*****************************************PERIODIC_BEACON_STATE FUNCTION***************************************************************/
/* Comment:
/****************************************************************************************************************************************/

void PeriodicBeaconState_function()
{

    //RF_cmdPropTx.startTime = RF_getCurrentTime();
    /* Set absolute TX time in the future to utilise "deferred dispatching of commands with absolute timing".
     * This is explained in the proprietary RF user's guide. */
    //invCount++;
    //ntime = RF_getCurrentTime();
    //


    //ntime += (uint32_t)(4000000*2.0f);// RF_convertMsToRatTicks(BEACON_INTERVAL_MS);

    ntime = RF_cmdPropTx.startTime;
    RF_cmdPropTx.startTime += RF_convertMsToRatTicks(BEACON_INTERVAL_MS);
    vTime = RF_getCurrentTime();

    beacon.txTime = RF_cmdPropTx.startTime;
    beacon.ledState = PIN_getInputValue(Board_LED1);

    if(routeFlag)
    {
        RF_EventMask result = RF_runCmd(rfHandle, (RF_Op*)&RF_cmdPropTx, RF_PriorityNormal, NULL, 0);
        assert((result == RF_EventLastCmdDone) && ((volatile RF_Op*)&RF_cmdPropTx)->status == PROP_DONE_OK);
        //StateMachine_setNextState(&stateMachine, PrepareSyncedRxState);

        PIN_setOutputValue(pinHandle, Board_LED1,!PIN_getOutputValue(Board_LED1));
        RF_close(rfHandle);
        if(newcount)
        {
            tricklastnode=0;
            StateMachine_setNextState(&stateMachine, PrepareSyncedRxState);
        }
        else
        {
            beacon.msg.mid = MSGID_DATA_SENSOR;
            Semaphore_post(semRxHandle);
            StateMachine_setNextState(&stateMachine, PrepareSyncedTxState);
        }
        routeFlag=0;
        newcount=0;
    }
    else if (wait_gnd)
    {
        RF_EventMask result = RF_runCmd(rfHandle, (RF_Op*)&RF_cmdPropTx, RF_PriorityNormal, NULL, 0);
        assert((result == RF_EventLastCmdDone) && ((volatile RF_Op*)&RF_cmdPropTx)->status == PROP_DONE_OK);
        //StateMachine_setNextState(&stateMachine, PrepareSyncedRxState);

        PIN_setOutputValue(pinHandle, Board_LED1,!PIN_getOutputValue(Board_LED1));
        RF_close(rfHandle);
        wait_gnd = 0;
        beacon.msg.mid=0x01;
        StateMachine_setNextState(&stateMachine, PrepareSyncedRxState);
        //StateMachine_setNextState(&stateMachine, WaitingForSyncState);

    }
    else
    {
        if( holdID==TRANSMIT_WAKEUP_AGAIN) //restructure the crude construction
        {
            nName=holdsrc;
            holdID=0;
            holdsrc=0;
            beacon.msg.mid=MSGID_REQ_JOIN_AGAIN;
        }


        beacon.msg.des =nName;
        beacon.msg.src =theNode.myid;
        beacon.msg.msgl = PacketSize;

        /* Because the TX command is due in 500ms and we use TRIG_ABSTIME as start trigger type,
         * the RF driver will now power down the RF core and and wait until ~1.5ms before RF_cmdPropTx.startTime.
         * Then the RF driver will power-up the RF core, re-synchronise the RAT and re-run the setup procedure.
         * The setup procedure includes RF_cmdPropRadioDivSetup and RF_cmdFs from above.
         * This will guarantee that RF_cmdPropTx is delivered to the RF core right before it has
         * to start. This is fully transparent to the application. It appears as the RF core was
         * never powered down.
         * This concept is explained in the proprietary RF user's guide. */

        if(beacon.msg.mid==MSGID_REQ_JOIN_AGAIN)
        {
            beacon.msg.startbit = RF_STX;
            //beacon.msg.mid=MSGID_REQ_JOIN;

            RF_EventMask result = RF_runCmd(rfHandle, (RF_Op*)&RF_cmdPropTx, RF_PriorityNormal, NULL, 0);
            assert((result == RF_EventLastCmdDone) && ((volatile RF_Op*)&RF_cmdPropTx)->status == PROP_DONE_OK);
            StateMachine_setNextState(&stateMachine, PrepareSyncedRxState);

            PIN_setOutputValue(pinHandle, Board_LED1,!PIN_getOutputValue(Board_LED1));
            RF_close(rfHandle);
            //invCount6++;
            //while(1);
        }

        else if(beacon.msg.mid==0x00)
        {
            beacon.msg.startbit = RF_STX;
            beacon.msg.mid=MSGID_REQ_JOIN;

            RF_EventMask result = RF_runCmd(rfHandle, (RF_Op*)&RF_cmdPropTx, RF_PriorityNormal, NULL, 0);
            assert((result == RF_EventLastCmdDone) && ((volatile RF_Op*)&RF_cmdPropTx)->status == PROP_DONE_OK);
            StateMachine_setNextState(&stateMachine, PrepareSyncedRxState);

            PIN_setOutputValue(pinHandle, Board_LED1,!PIN_getOutputValue(Board_LED1));
            RF_close(rfHandle);
        }
        else
        {
            for(q=0;q<MAXMSGLENGTH;q++)
            {
                beacon.msg.data[q] = 0x00; //reset data array
            }
            //resetcounter++;            /*******************************FOR FEEDBACK TEST************************/
            beacon.msg.data[0] = theNode.myid;
            beacon.msg.data[1] = 0xff;
            beacon.msg.data[2] = 0xff;
            int j;
            for(j=3;j<PacketSize;j++)//for now dummy data
                beacon.msg.data[j] = (j-2);

            if(resetcounter >32)   //after 32 packets send this reset info for confirmation
                beacon.msg.mid = ID_SYSTEM_RESET;


            ////////////////////////////////////////////////////Some Hack - TRANSMISSION TIME BOUNDS//////////////////////////////////////////////////
            //            // Current timestamp:
            //            currentTime = RF_getCurrentTime();
            //
            //            // Command's start time
            //            commandTime = RF_cmdPropTx.startTime;
            //
            //            // Assume that a command can be scheduled max 3s ahead
            //            const maxTimeAhead = 1000000*8;
            //            // ... and must be min 100�s ahead (time to deliver to RF core, parsing, ...)
            //            const minTimeAhead = 1000*4;
            //
            //            uint32_t timeUntilStart = commandTime - currentTime;
            //            //
            //            //            // Ensure that the constraints are satisfied
            //            assert(timeUntilStart >= minTimeAhead);
            //            assert(timeUntilStart <= maxTimeAhead);
            //            //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

            //            invCount5++;
            //unsigned int invCount55 = RF_cmdPropTx.startTime;
            // if(invCount5==176 && (beacon.msg.src & 0xF0)==0xC0)
            // Abort anything

            //            {
            //                SysCtrlSystemReset();
            //                HapiResetDevice();
            //            }
            //            else if (invCount5==176 && (beacon.msg.src & 0xF0)==0xB0)
            //            {
            //                StateMachine_setNextState(&stateMachine, PrepareSyncedRxState);
            //            }
            //            else
            {
                //if(invCount5!=176)
                {
                    RF_EventMask result = RF_runCmd(rfHandle, (RF_Op*)&RF_cmdPropTx, RF_PriorityNormal, NULL, 0);
                    assert((result == RF_EventLastCmdDone) && ((volatile RF_Op*)&RF_cmdPropTx)->status == PROP_DONE_OK);
                }
                //                RF_CmdHandle ch = RF_postCmd(rfHandle, (RF_Op*)&RF_cmdPropTx, RF_PriorityNormal, NULL,RF_EventRxEntryDone);
                //                invCount6++;
                //                                PIN_setOutputValue(pinHandle, Board_LED1,!PIN_getOutputValue(Board_LED1));
                //                                StateMachine_setNextState(&stateMachine, PeriodicBeaconState);  //always the case if its the sensor node
                //
                //                            RF_close(rfHandle);
                //                            configureTx();
                //                invCount6++;

                //SysCtrlSystemReset();
                //RF_close(rfHandle);
                //while(1);
                // PeriodicBeaconState_function() will be entered again.
                if((lastcounter<9) & (resetcounter < 34))
                    StateMachine_setNextState(&stateMachine, PeriodicBeaconState);  //always the case if its the sensor node

                if(resetcounter >33)
                {
                    resetcounter = 0;
                    RF_close(rfHandle);
                    StateMachine_setNextState(&stateMachine, PrepareSyncedRxState);
                }


                if(tricklastnode==1)
                    lastcounter++;

                if(lastcounter==10)
                {
                    MsgObj msg;
                    lastcounter=0;

                    msg.cmdid= START_RADIO_AGAIN;
                    msg.cmds = 0x09;
                    msg.smsg.mid = MSGID_REQ_WAKEUP_AGAIN;
                    msg.smsg.src = theNode.myid;
                    msg.smsg.des = 0xFF;
                    msg.smsg.msgl = packetLength;
                    StateMachine_setNextState(&stateMachine, PrepareSyncedRxState);
                    Mailbox_post(mbxHandle, &msg, BIOS_NO_WAIT);
                }
            }
        }
    }

}

/*****************************************SYNCED_TX_RX_STATE FUNCTION***************************************************************/
/* Comment:
/****************************************************************************************************************************************/

void SyncedTxRxState_function()
{
    rxItem.status = DATA_ENTRY_PENDING;


    /* Start RX command to receive a single packet. Use an absolute start trigger
     * and a predicted start time. The end time is calculated to be as tight as possible.
     * The compensation margin for RAT drift in both directions needs to be taken into account.  */
    RF_cmdPropRx.startTrigger.triggerType = TRIG_ABSTIME;
    RF_cmdPropRx.startTime += beacon.beaconInterval;
    qtime = RF_cmdPropRx.startTime;
    RF_cmdPropRx.endTrigger.triggerType = TRIG_REL_START;
    RF_cmdPropRx.endTime = RX_START_TO_SETTLE_TICKS + RX_TIMEOUT_TICKS + RX_TIMEOUT_MARGIN + RX_START_MARGIN;

    /* Puts the RX command into the driver queue. Since the start trigger is absolute and has
     * a time somewhere in the future, the RF driver will power down the RF core and wait short
     * before the RX command is due. Then it will run the power-up sequence and dispatch the RX
     * command right on time.
     */
    RF_CmdHandle cmd = RF_postCmd(rfHandle, (RF_Op*)&RF_cmdPropRx, RF_PriorityNormal, &SyncedTxRxState_txrxCallback, RF_EventRxEntryDone);
    RSSIout = rxStatistics.lastRssi;


    for (;;)
    {
        StateMachine_EventMask events = StateMachine_pendEvents(&stateMachine, Event_PacketReceived | Event_SyncMissed | Event_NewMissed, BIOS_WAIT_FOREVER);

        if (events & Event_PacketReceived)
        {

            /* RX command has already stopped. Now examine the received data. */
            uint8_t length;
            uint32_t rxTime;

            memcpy(&length, rxBuffer, 1);
            if (length != sizeof(BeaconPacket))
            {
                // This packet is not for us. Wait for next one
                StateMachine_setNextState(&stateMachine, SyncedTxRxState);
                break;
            }
            int j;

            if((rxBuffer[13] & 0x0F)==target)//if my data
            {
                Clock_start(Clock_handle(&stateClock));
                memcpy(&beacon, (uint8_t*)rxBuffer + 1, sizeof(BeaconPacket));
                memcpy(&rxTime, (uint8_t*)rxBuffer + 1 + sizeof(BeaconPacket), 4);

                ///////////////////////////////////lets do our manipulations here///////////////////////////////////////
                /* The synchronisation offset might be used to calculate the clock drift between tranStateMachine_itter and receiver. */
                int32_t syncOffsetTime = rxTime - RX_START_TO_SETTLE_TICKS /*RX_START_MARGIN*/ - qtime/*RF_cmdPropRx.startTime*/;
                (void)syncOffsetTime; // need to reference the variable to prevent from a compiler warning

                /* Do an immediate re-synchronisation based on the new RX time. */
                RF_cmdPropRx.startTime = rxTime - RX_START_TO_SETTLE_TICKS - RX_START_MARGIN;
                ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////


                //PIN_setOutputValue(pinHandle, Board_LED1, beacon.ledState);
                PIN_setOutputValue(pinHandle, Board_LED1,!PIN_getOutputValue(Board_LED1));

                //20ms then transmit package
                while(wakeFlag==1);
                if(wakeFlag==0)
                {
                    wakeFlag=1; //reset
                    if (Clock_isActive(Clock_handle(&stateClock))){
                        Clock_stop(Clock_handle(&stateClock));
                    }
                    beacon.msg.mid = MSGID_DATA_SENSOR;
                    beacon.msg.des =nName;//beacon.msg.data[1];
                    beacon.msg.src =theNode.myid;
                    beacon.msg.msgl = PacketSize;
                    beacon.msg.data[(node_pos*PacketSize)] = theNode.myid;
                    beacon.msg.data[1+(node_pos*PacketSize)] = RSSIout;
                    beacon.msg.data[2+(node_pos*PacketSize)] = 0xff;

                    for(j=3;j<beacon.msg.msgl;j++)//for now dummy data
                        beacon.msg.data[j+(node_pos*PacketSize)] = (node_pos*10)+(j-2);

                    calTime = RF_getCurrentTime();
                    if(rxBuffer[11] == ID_SYSTEM_RESET)
                        beacon.msg.mid = ID_SYSTEM_RESET;

                    RF_close(rfHandle);
                    configureTx();
                    TransmitTx();
                    RF_close(rfHandle);
                    configureRx();




                    if(beacon.msg.mid == ID_SYSTEM_RESET)
                    {
                        if(redundant)
                        {
                            // RF_close(rfHandle);
                            resume_gnd=0;
                            redundant=0;
                            //StateMachine_setNextState(&stateMachine, PrepareSyncedRxState);
                            StateMachine_setNextState(&stateMachine, WaitingForSyncState);
                            wait_gnd = 1;
                            break;
                        }
                        redundant++;
                    }

                    /* Wait for the next packet */
                    StateMachine_setNextState(&stateMachine, SyncedTxRxState);

                    break;
                }
            }
        }


        if (events & Event_SyncMissed)
        {
            /* Wait for the next packet */

            missedCounter++;
            if(missedCounter>node_pos)
            {
                if((node_pos-inc) == 0)
                {
                    //beacon.msg.mid =MSGID_DATA_SENSOR;
                    RF_cmdPropTx.startTime = RF_getCurrentTime();
                    StateMachine_setNextState(&stateMachine, PeriodicBeaconState);
                    //nName = 0xB0 + narr[node_pos+1];
                    tricklastnode = 1;
                    routeFlag=0;
                    //Semaphore_post(semRxHandle);
                    break;
                }

                StateMachine_setNextState(&stateMachine, WaitingForSyncState);


                break;
            }
            else
            {
                StateMachine_setNextState(&stateMachine, SyncedTxRxState);
                break;

            }
        }

        if (events & Event_NewMissed)
        {
            /* Sync is missed. That means either we are out of sync or the
             * tranStateMachine_itter is in spontaneous mode. Try to receive the next packet. */
            //StateMachine_setNextState(&stateMachine, SyncedTxRxState);
            //me++;
            //if(me==5)
            StateMachine_setNextState(&stateMachine, WaitingForSyncState);
            break;
        }
    }

}

/*****************************************SYNCED_TX_RX_STATE_TXRXCALLBACK FUNCTION***************************************************************/
/* Comment:
/****************************************************************************************************************************************/

void SyncedTxRxState_txrxCallback(RF_Handle h, RF_CmdHandle ch, RF_EventMask e)
{
    if (e & RF_EventLastCmdDone)
    {
        if ((RF_cmdPropRx.status == PROP_DONE_OK) || (RF_cmdPropRx.status == PROP_DONE_ENDED))
        {
            /* Sync word has been found before end trigger and packet has been received */
            StateMachine_postEvents(&stateMachine, Event_PacketReceived);

        }
        else if (RF_cmdPropRx.status == PROP_DONE_RXTIMEOUT)
        {
            StateMachine_postEvents(&stateMachine, Event_SyncMissed);
        }
        else
        {
            StateMachine_postEvents(&stateMachine, Event_NewMissed);
        }
    }
}

/*****************************************CONFIGURE_TX FUNCTION**************************************************************************/
/* Comment:
/****************************************************************************************************************************************/

void configureTx()
{
    //Semaphore_pend(semRxHandle, BIOS_WAIT_FOREVER);
    /* Prepare the packet */
    RF_cmdPropTx.pktLen = sizeof(beacon);
    RF_cmdPropTx.pPkt = (uint8_t*)&beacon;
    RF_cmdPropTx.startTrigger.triggerType = TRIG_ABSTIME;
    RF_cmdPropTx.startTime = 0;
    RF_cmdPropTx.startTime = RF_getCurrentTime();


    /* Request access to the radio. This does not power-up the RF core, but only initialise
     * the driver and cache the setup command. */
    RF_Params rfParams;
    RF_Params_init(&rfParams);
    rfHandle = RF_open(&rfObject, &RF_prop, (RF_RadioSetup*)&RF_cmdPropRadioDivSetup, &rfParams);
    assert(rfHandle != NULL);

    /* Increase the SWI priority of the internal state machine in the RF driver. The default
     * value is 0. This is necessary because the button callback blocks for a long time and
     * might disturb the RF driver state machine.
     */

    /* Set the frequency. Now the RF driver powers the RF core up and runs the setup command from above.
     * The FS command is executed and also cached for later use when the RF driver does an automatic
     * power up. */
    RF_EventMask result = RF_runCmd(rfHandle, (RF_Op*)&RF_cmdFs, RF_PriorityNormal, NULL, 0);
    assert((result == RF_EventLastCmdDone) && ((volatile RF_Op*)&RF_cmdFs)->status == DONE_OK);

    /* Use the current time as an anchor point for future time stamps.
     * The Nth transmission in the future will be exactly N * 500ms after
     * this time stamp.  */
    RF_cmdPropTx.startTime = RF_getCurrentTime();

    /* A trigger in the past is triggered as soon as possible.
     * No error is given.
     * This avoids assertion when button debouncing causes delay in TX trigger.  */
    RF_cmdPropTx.startTrigger.pastTrig = 1;

    /* Route the PA signal to an LED to indicate ongoing transmissions.
     * Available signals are listed in the proprietary RF user's guide.
     */
    //PINCC26XX_setMux(pinHandle, Board_LED2, PINCC26XX_MUX_RFC_GPO1);

}

/*****************************************TRANSMIT_TX FUNCTION***************************************************************/
/* Comment:
/****************************************************************************************************************************************/

void TransmitTx()
{

    /* Set absolute TX time in the future to utilise "deferred dispatching of commands with absolute timing".
     * This is explained in the proprietary RF user's guide. */
    //RF_cmdPropTx.startTime += RF_convertMsToRatTicks(BEACON_INTERVAL_MS);

    beacon.txTime = RF_cmdPropTx.startTime;
    beacon.ledState = PIN_getInputValue(Board_LED1);


    /* Because the TX command is due in 500ms and we use TRIG_ABSTIME as start trigger type,
     * the RF driver will now power down the RF core and and wait until ~1.5ms before RF_cmdPropTx.startTime.
     * Then the RF driver will power-up the RF core, re-synchronise the RAT and re-run the setup procedure.
     * The setup procedure includes RF_cmdPropRadioDivSetup and RF_cmdFs from above.
     * This will guarantee that RF_cmdPropTx is delivered to the RF core right before it has
     * to start. This is fully transparent to the application. It appears as the RF core was
     * never powered down.
     * This concept is explained in the proprietary RF user's guide. */
    RF_EventMask result = RF_runCmd(rfHandle, (RF_Op*)&RF_cmdPropTx, RF_PriorityNormal, NULL, 0);
    assert((result == RF_EventLastCmdDone) && ((volatile RF_Op*)&RF_cmdPropTx)->status == PROP_DONE_OK);

}

/*****************************************CONFIGRURE_RX FUNCTION*************************************************************************/
/* Comment:
/****************************************************************************************************************************************/

void configureRx()
{

    /* Request access to the radio. This does not power-up the RF core, but only initialise
     * the driver and cache the setup command. */
    RF_Params rfParams;
    RF_Params_init(&rfParams);
    rfHandle = RF_open(&rfObject, &RF_prop, (RF_RadioSetup*)&RF_cmdPropRadioDivSetup, &rfParams);
    assert(rfHandle != NULL);

    /* Set the frequency. Now the RF driver powers the RF core up and runs the setup command from above.
     * The FS command is executed and also cached for later use when the RF driver does an automatic
     * power up. */
    RF_EventMask result = RF_runCmd(rfHandle, (RF_Op*)&RF_cmdFs, RF_PriorityNormal, NULL, 0);
    assert((result == RF_EventLastCmdDone) && ((volatile RF_Op*)&RF_cmdFs)->status == DONE_OK);

    /* Route the LNA signal to an LED to indicate that the RF core is
     * active and receiving data.
     * Available signals are listed in the proprietary RF user's guide.
     */
    //PINCC26XX_setMux(pinHandle, Board_LED2, PINCC26XX_MUX_RFC_GPO0);

}


void ClockSwiFxn(uintptr_t arg0)
{
    //reset flag.
    wakeFlag=0;

}