bsp.aun2402/source/instance.c

/*! ----------------------------------------------------------------------------
 *  @file    instance.c
 *  @brief   DecaWave application level message exchange for ranging demo
 *
 * @attention
 *
 * Copyright 2015 (c) DecaWave Ltd, Dublin, Ireland.
 *
 * All rights reserved.
 *
 * @author DecaWave
 */
#include "compiler.h"
#include "port.h"
#include "deca_device_api.h"
#include "deca_spi.h"
#include "deca_regs.h"

#include "instance.h"

// -------------------------------------------------------------------------------------------------------------------


// -------------------------------------------------------------------------------------------------------------------
//      Data Definitions
// -------------------------------------------------------------------------------------------------------------------
extern char debugBuffer[256];
extern int debugLen;

extern volatile stTagFinalRecord tagFinalRecord;

extern volatile uint32_t ancRespCount;
extern volatile stAncRespRecord ancRespRecords[8];

extern volatile stInstDebug instDebug;
int  ReceivedReply=5000;

typedef struct _monitor_ticks_
{
    uint32_t startTick;
    uint32_t pollRecvTick;
    uint32_t startSendTick;
    uint32_t temp;
}stDwmMonitor;
volatile stDwmMonitor monitor = {0};

// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
// NOTE: the maximum RX timeout is ~ 65ms
// !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!

// -------------------------------------------------------------------------------------------------------------------
// Functions
// -------------------------------------------------------------------------------------------------------------------
#if defined(REPORT2WIFI)
extern int SendMessageWifi(uint8_t* buf,int len);
#endif
extern int SendMessage(uint8_t* buf,int len);
extern int SendMessageDbg(uint8_t* buf,int len);

// -------------------------------------------------------------------------------------------------------------------
//
// function to construct the message/frame header bytes
//
// -------------------------------------------------------------------------------------------------------------------
//
void instConfigFrameHeader16(instance_data_t *inst)
{
    //set frame type (0-2), SEC (3), Pending (4), ACK (5), PanIDcomp(6)
    inst->msgFrame.content.frameCtrl[0] = 0x1 /*frame type 0x1 == data*/ | 0x40 /*PID comp*/;

	//source/dest addressing modes and frame version
	inst->msgFrame.content.frameCtrl[1] = 0x8 /*dest extended address (16bits)*/ | 0x80 /*src extended address (16bits)*/;

	inst->msgFrame.content.panID[0] = (inst->setting.panID) & 0xff;
	inst->msgFrame.content.panID[1] = inst->setting.panID >> 8;

    inst->msgFrame.content.seqNum = 0;
}

int instSendDlyPacket(instance_data_t *inst, int delayedTx)
{
    int result = 0;

    dwt_writetxfctrl(inst->msgFrame.psduLength, 0);
    if(delayedTx == DWT_START_TX_DELAYED)
    {
        dwt_setdelayedtrxtime(inst->tmAndDelays.delayedReplyTime) ; //should be high 32-bits of delayed TX TS
    }

    //begin delayed TX of frame
    if (dwt_starttx(delayedTx | inst->stateMachine.wait4ack))  // delayed start was too late
    {
        result = 1; //late/error
    }
    else
    {
        //inst->stateMachine.monitor = 1;
    }
    return result;                                              // state changes
}


int instance_calcranges(stTofInformation *array, uint16_t size, int reportRange, uint32_t* mask)
{
	int i;
	int newRange = TOF_REPORT_NUL;
	int distance = 0;

	for(i=0; i<size; i++)
	{
		stTofInformation* tofx = array+i;
		if(tofx->value != INVALID_TOF) //if ToF == 0 - then no new range to report
		{
			distance = reportTOF(i, tofx);
		}

		if(distance == 1)
		{
			newRange = reportRange;
		}
		else
		{
			clearDistTable(i);
		}
		array[i].value = INVALID_TOF;
        array[i].address = BROADCAST_ADDR;

		distance = 0;
	}
    if(newRange==TOF_REPORT_NUL)
    {
		distance = 0;
    }

	return newRange;
}

void ReportTagStatus(uint32_t address)
{
    stTagStatus tagStatus;
    GetTagStatus(address, &tagStatus);
    debugLen = sprintf(debugBuffer,"tag: %04x, st=%04x, pw=%d, hb=%d; ps: %06d, as: %06d ar:%06d %06d %06d %06d, fs: %06d, tof: %06d T:%d\r\n",
                      address, tagStatus.status, tagStatus.power, tagStatus.heartRate,
                      instDebug.ancPollCount, instDebug.andRespSentCount,
                      instDebug.ancRespCounts[0],instDebug.ancRespCounts[1],
                      instDebug.ancRespCounts[2],instDebug.ancRespCounts[3],
                      instDebug.ancFinalCount, instDebug.tofSuccess,
                      portGetTickCnt());
#if defined(REPORT2COM)
    ReportMessage((uint8_t*)debugBuffer, debugLen);
#elif defined(REPORT2WIFI)
    SendMessageWifi((uint8_t*)debugBuffer, debugLen);
#endif
    SendMessage((uint8_t*)debugBuffer,debugLen);
    SendMessageDbg((uint8_t*)debugBuffer,debugLen);
}

int appState_TA_INIT(instance_data_t *inst, uint32_t* status, int message)
{
    *status=TA_INIT;

    memcpy(inst->setting.eui64, &inst->setting.instanceAddress16, ADDR_BYTE_SIZE_S);
    dwt_seteui(inst->setting.eui64);

    dwt_setpanid(inst->setting.panID);

    //set source address
    inst->setting.shortAdd_idx = (inst->setting.instanceAddress16 % (ANCHOR_SHORT_MASK + 1));
    //inst->setting.shortAdd_idx = (inst->setting.instanceAddress16 % NUM_EXPECTED_RESPONSES);
    dwt_setaddress16(inst->setting.instanceAddress16);

    dwt_enableframefilter(DWT_FF_NOTYPE_EN); //allow data, ack frames;

    // First time anchor listens we don't do a delayed RX
    dwt_setrxaftertxdelay(0);
    //change to next state - wait to receive a message
    inst->stateMachine.testAppState = TA_RXE_WAIT ;

    dwt_setrxtimeout(0);
    dwt_setpreambledetecttimeout(0);
    instConfigFrameHeader16(inst);

    return 0;
}

//
int appState_TA_TX_WAIT_CONF(instance_data_t *inst, uint32_t *status, int message)
{
    *status=TA_TX_WAIT_CONF;
    event_data_t* dw_event = instGetEvent(11); //get and clear this event

    //NOTE: Can get the ACK before the TX confirm event for the frame requesting the ACK
    //this happens because if polling the ISR the RX event will be processed 1st and then the TX event
    //thus the reception of the ACK will be processed before the TX confirmation of the frame that requested it.
    if(dw_event->type != DWT_SIG_TX_DONE) //wait for TX done confirmation
    {
        if(dw_event->type == DWT_SIG_RX_TIMEOUT) //got RX timeout - i.e. did not get the response (e.g. ACK)
        {
            //printf("RX timeout in TA_TX_WAIT_CONF (%d)\n", inst->previousState);
            //we need to wait for SIG_TX_DONE and then process the timeout and re-send the frame if needed
            inst->stateMachine.gotTO = 1;
        }
        else
        {
            //inst->stateMachine.done = INST_DONE_WAIT_FOR_NEXT_EVENT;
            inst->stateMachine.done = INST_NOT_DONE_YET;
        }
        uint32_t curTick = HAL_GetTick();
        if(monitor.startSendTick > curTick)
        {
            monitor.startSendTick = curTick;
        }
        if(curTick - monitor.startSendTick < inst->setting.slotPeriod )
        {
            return -1;
        }
        else
        {
            inst->stateMachine.gotTO = 1;
        }
    }

    inst->stateMachine.done = INST_NOT_DONE_YET;

    if (inst->stateMachine.gotTO == 1) //timeout
    {
        if(inst->stateMachine.previousState == TA_TXRESPONSE_SENT_TORX)
        {
            dwt_forcetrxoff();
            dwt_rxreset();
        }

        instProcessRXtimeout(inst);
        inst->stateMachine.gotTO = 0;
        inst->stateMachine.wait4ack = 0 ; //if Resp timeout, clear this

    #if (ANC_RESP_TEST==1)
        {
            debugLen = sprintf((char*)debugBuffer,"ANC_RESP TIMEOUT 0x%04x, src:0x%02x%02x, dst:0x%02x%02x, fctrl:0x%x, %d\r\n",
                               inst->setting.instanceAddress16,
                               inst->msgFrame.content.srcAddr[1],inst->msgFrame.content.srcAddr[0],
                               inst->msgFrame.content.dstAddr[1],inst->msgFrame.content.dstAddr[0],
                               inst->msgFrame.content.frameCtrl[0]|(inst->msgFrame.content.frameCtrl[1]<<8),
                               portGetTickCnt());
    #if defined(REPORT2COM)
            ReportMessage((uint8_t*)debugBuffer, debugLen);
    #elif defined(REPORT2WIFI)
            SendMessageWifi((uint8_t*)debugBuffer, debugLen);
    #endif
            SendMessage((uint8_t*)debugBuffer,debugLen);
            SendMessageDbg((uint8_t*)debugBuffer,debugLen);
        }
    #endif
        return -1;
    }
    else
    {
        //inst->tmAndDelays.txu.txTimeStamp = dw_event->timeStamp;

        if(inst->stateMachine.previousState == TA_TXRESPONSE_SENT_TORX)
        { //Send resp fails
            inst->stateMachine.previousState = TA_TXRESPONSE_WAIT_SEND ;
        }

        inst->stateMachine.testAppState = TA_RXE_WAIT ;                      // After sending, tag expects response/report, anchor waits to receive a final/new poll
    }//fall into the next case (turn on the RX)

    return 0;
}


int appState_TA_RXE_WAIT(instance_data_t *inst, uint32_t *status, int message)
{
    *status = TA_RXE_WAIT;

    if(inst->stateMachine.wait4ack == 0) //if this is set the RX will turn on automatically after TX
    { //turn RX on
        dwt_rxenable(DWT_START_RX_IMMEDIATE) ;  // turn RX on, without delay

        trxStage = RTX_RX;
    }
    else
    {
        inst->stateMachine.wait4ack = 0 ; //clear the flag, the next time we want to turn the RX on it might not be auto
    }

    //inst->stateMachine.done = INST_DONE_WAIT_FOR_NEXT_EVENT; //using RX FWTO
    inst->stateMachine.done = INST_NOT_DONE_YET;

    inst->stateMachine.testAppState = TA_RX_WAIT_DATA;   // let this state handle it

    // end case TA_RXE_WAIT, don't break, but fall through into the TA_RX_WAIT_DATA state to process it immediately.
    if(message == 0)
    {
        return -1;
    }

    return 0;
}


int64_t CalculateTof(instance_data_t *inst, uint64_t tagPollTxTime, uint64_t anchorRespRxTime, uint64_t tagFinalTxTime)
{
    int64_t Rb, Da, Ra, Db ;
    int64_t tof = INVALID_TOF;

    double RaRbxDaDb = 0;
    double RbyDb = 0;
    double RayDa = 0;

    // poll response round trip delay time is calculated as
    // (anchorRespRxTime - tagPollTxTime) - (anchorRespTxTime - tagPollRxTime)
    Ra = (int64_t)((anchorRespRxTime - tagPollTxTime) & MASK_40BIT);
    Db = (int64_t)((inst->tmAndDelays.txu.anchorRespTxTime - inst->tmAndDelays.tagPollRxTime) & MASK_40BIT);

    // response final round trip delay time is calculated as
    // (tagFinalRxTime - anchorRespTxTime) - (tagFinalTxTime - anchorRespRxTime)
    Rb = (int64_t)((inst->tmAndDelays.tagFinalRxTime - inst->tmAndDelays.txu.anchorRespTxTime) & MASK_40BIT);
    Da = (int64_t)((tagFinalTxTime - anchorRespRxTime) & MASK_40BIT);

    RaRbxDaDb = (((double)Ra))*(((double)Rb))
                - (((double)Da))*(((double)Db));

    RbyDb = ((double)Rb + (double)Db);

    RayDa = ((double)Ra + (double)Da);

    tof = (int64_t) ( RaRbxDaDb/(RbyDb + RayDa) );
    return tof;
}

int appState_TA_RX_WAIT_DATA(instance_data_t *inst, uint32_t *status, int message)
{
    *status = TA_RX_WAIT_DATA;
    if(ReceivedReply>0)
    {
        ReceivedReply--;
    }

    switch (message)
    {
        //if we have received a DWT_SIG_RX_OKAY event - this means that the message is IEEE data type - need to check frame control to know which addressing mode is used
        case DWT_SIG_RX_OKAY :
        {
          
            ReceivedReply=5000;
            event_data_t* dw_event = instGetEvent(15); //get and clear this event
            uint8_t  srcAddr[8] = {0,0,0,0,0,0,0,0};
            uint8_t  dstAddr[8] = {0,0,0,0,0,0,0,0};
            int fcode = 0;
            uint8_t tof_idx  = 0;
            uint8_t *messageData;

            monitor.startTick = dw_event->uTimeStamp;

            //short address, already checked in rxCallBack
            memcpy(&srcAddr[0], &(dw_event->msgUnion.rxMsg_ss.srcAddr[0]), ADDR_BYTE_SIZE_S);
            memcpy(&dstAddr[0], &(dw_event->msgUnion.rxMsg_ss.dstAddr[0]), ADDR_BYTE_SIZE_S);
            fcode = dw_event->msgUnion.rxMsg_ss.messageData[FCODE];
            messageData = &dw_event->msgUnion.rxMsg_ss.messageData[0];


            //process ranging messages
            switch(fcode)
            {
                case RTLS_DEMO_MSG_ANCH_POLL:
                    break;
                case RTLS_DEMO_MSG_TAG_POLL:
                {
                    tof_idx = srcAddr[0] | (srcAddr[1]<<8);

                    SaveTagStatus(tof_idx, messageData);

                    monitor.pollRecvTick = dw_event->uTimeStamp;

                    if(DWT_SIG_TX_PENDING == dw_event->typePend)
                    { //the response has been sent - await TX done event
                        inst->stateMachine.testAppState = TA_TX_WAIT_CONF;                // wait confirmation
                        inst->stateMachine.previousState = TA_TXRESPONSE_SENT_POLLRX ;    //wait for TX confirmation of sent response

                        monitor.startSendTick = dw_event->uTimeStamp;
                    }
                    else if (DWT_SIG_RX_PENDING == dw_event->typePend)
                    { //already re-enabled the receiver, stay in RX wait for next frame..., RX is already enabled...
                        inst->stateMachine.testAppState = TA_RX_WAIT_DATA ;              // wait for next frame
                    }
                    else //the DW1000 is idle (re-enable from the application level)
                    {
                        //stay in RX wait for next frame...
                        inst->stateMachine.testAppState = TA_RXE_WAIT ;              // wait for next frame
                    }
                    inst->stateMachine.done = INST_NOT_DONE_YET;
                } break; //RTLS_DEMO_MSG_TAG_POLL

                case RTLS_DEMO_MSG_ANCH_RESP2:
                    break;
                case RTLS_DEMO_MSG_ANCH_RESP:
                {
                    inst->stateMachine.done = INST_NOT_DONE_YET;
                    //tof_idx = srcAddr[0] & ANCHOR_LIST_INDEX_MASK;

                    //the response has been sent - await TX done event
                    if( DWT_SIG_TX_PENDING == dw_event->typePend ) //anchor received response from anchor ID - 1 so is sending it's response now back to tag
                    {
                        inst->stateMachine.testAppState = TA_TX_WAIT_CONF;                // wait confirmation
                        inst->stateMachine.previousState = TA_TXRESPONSE_SENT_RESPRX ;    //wait for TX confirmation of sent response
                        monitor.startSendTick = dw_event->uTimeStamp;
                    }
                    //already re-enabled the receiver
                    else if( DWT_SIG_RX_PENDING == dw_event->typePend )
                    {
                        // stay in TA_RX_WAIT_DATA - receiver is already enabled.
                    }
                        //DW1000 idle - send the final
                    else //if(dw_event->type_pend == DWT_SIG_DW_IDLE)
                    {
                        instBack2Anchor(inst);
                    }

                #if (ANC_RESP_TEST==1)
                    {
                        debugLen = sprintf(debugBuffer,"ANC_RESP RECVED:%d, %04x, %04x, %04x, %04x; count:%d,%d,%d,%d. T:%d\r\n",
                                           ancRespCount,ancRespRecords[0].address,ancRespRecords[1].address,
                                           ancRespRecords[2].address,ancRespRecords[3].address,
                                           instDebug.ancRespCounts[0],instDebug.ancRespCounts[1],
                                           instDebug.ancRespCounts[2],instDebug.ancRespCounts[3],
                                           portGetTickCnt());
                #if defined(REPORT2COM)
                        ReportMessage((uint8_t*)debugBuffer, debugLen);
                #elif defined(REPORT2WIFI)
                        SendMessageWifi((uint8_t*)debugBuffer, debugLen);
                #endif
                        SendMessage((uint8_t*)debugBuffer,debugLen);
                        SendMessageDbg((uint8_t*)debugBuffer,debugLen);
                    }
                #endif

                } break; //RTLS_DEMO_MSG_ANCH_RESP

                case RTLS_DEMO_MSG_ANCH_FINAL:
                    break;
                case RTLS_DEMO_MSG_TAG_FINAL:
                {
                    inst->stateMachine.done = INST_NOT_DONE_YET;
                    inst->stateMachine.wait4ack = 0; //clear the flag as the TX has failed the TRX is off
                    tof_idx = srcAddr[0] | (srcAddr[1]<<8);//tag address
                    if((RTLS_DEMO_MSG_TAG_FINAL == fcode) &&
                       (instGetTagRangeNum(tof_idx) != messageData[POLL_RNUM])) //Final's range number needs to match Poll's or else discard this message
                    {
                        inst->stateMachine.testAppState = TA_RXE_WAIT ;              // wait for next frame
                        break;
                    }

                    monitor.pollRecvTick = 0;//clear
                    monitor.startSendTick = 0;

                    uint32_t count = 0;
                    uint64_t tagFinalTxTime  = 0;
                    uint64_t tagPollTxTime  = 0;
                    uint64_t anchorRespRxTime  = 0;
                    uint64_t tof = INVALID_TOF;

                    //if we got the final, maybe the tag did not get our response, so
                    //we can use other anchors responses/ToF if there are any.. and output..
                    //but we cannot calculate new range
                    for(count = 0; count < NUM_EXPECTED_RESPONSES; count++)
                    {
                        //if(((validResp & (0x1<<(inst->setting.shortAdd_idx))) != 0))
                        if(tagFinalRecord.ancRespTimes[count].address == inst->setting.instanceAddress16)
                        {
                            inst->tmAndDelays.delayedReplyTime = 0 ;

                            // times measured at Tag extracted from the message buffer extract 40bit times
                            memcpy(&tagPollTxTime, (void const *)tagFinalRecord.tagPollTxTime, TIMESTAME_LENGTH);
                            memcpy(&anchorRespRxTime, (void const *)tagFinalRecord.ancRespTimes[count].ancRespRxTime, TIMESTAME_LENGTH);
                            memcpy(&tagFinalTxTime, (void const *)tagFinalRecord.tagFinalTxTime, TIMESTAME_LENGTH);

                            tof = (int32_t)CalculateTof(inst, tagPollTxTime, anchorRespRxTime, tagFinalTxTime);
                        }
                    }


                    inst->stateMachine.newRangeTime = dw_event->uTimeStamp ;
                    inst->stateMachine.newRangeAncAddress = inst->setting.instanceAddress16;
                    inst->stateMachine.newRangeTagAddress = tof_idx;

                    //time-of-flight
                    SetTagTofResult(inst->stateMachine.newRangeTagAddress, tof);

                    //we have our range - update the own mask entry...
                    if(tof != INVALID_TOF) //check the last ToF entry is valid and copy into the current array
                    {
                        //setTagDist(tof_idx & TAG_LIST_INDEX_MASK, inst->setting.shortAdd_idx); //copy distance from this anchor to the tag into array
                        SetTofResult(inst->setting.instanceAddress16, tof);
                        instDebug.tofSuccess++;
                    }

                    //calculate all tag - anchor ranges... and report
                    inst->stateMachine.newRange = instance_calcranges(inst->twrResult.tofArray, MAX_ANCHOR_LIST_SIZE, TOF_REPORT_T2A, status);
                    if(inst->stateMachine.newRange == TOF_REPORT_NUL)
                    {
                        instDebug.calculateFails++;
                    }

                    //PrintDebugMessage();
                    //ReportTagStatus(inst->stateMachine.newRangeTagAddress);

                    instSetAntennaDelays(); //this will update the antenna delay if it has changed
                    instSetTXPower(); // configure TX power if it has changed

                    inst->stateMachine.testAppState = TA_RXE_WAIT ;              // wait for next frame


                } break; //RTLS_DEMO_MSG_TAG_FINAL

                default:
                {
                    //only enable receiver when not using double buffering
                    inst->stateMachine.testAppState = TA_RXE_WAIT ;              // wait for next frame
                    dwt_setrxaftertxdelay(0);
                } break;
            } //end switch (fcode)

            if(dw_event->msgUnion.frame[0] & 0x20)
            {
                //as we only pass the received frame with the ACK request bit set after the ACK has been sent
                instGetEvent(16); //get and clear the ACK sent event
            }
        }
        break ; //end of DWT_SIG_RX_OKAY

        case DWT_SIG_RX_TIMEOUT :
        {
            event_data_t* dw_event = instGetEvent(17); //get and clear this event

            monitor.startTick = dw_event->uTimeStamp;

            if( DWT_SIG_TX_PENDING == dw_event->typePend )
            { // Sent resp in event DWT_SIG_RX_TIMEOUT of rxCallBack
                inst->stateMachine.testAppState = TA_TX_WAIT_CONF;    // wait confirmation
                inst->stateMachine.previousState = TA_TXRESPONSE_SENT_TORX ;    //wait for TX confirmation of sent response
                monitor.startSendTick = dw_event->uTimeStamp;
            }
            else if( DWT_SIG_DW_IDLE == dw_event->typePend ) //if timed out and back in receive then don't process as timeout
            {
                instProcessRXtimeout(inst);
            }
            inst->stateMachine.done = INST_NOT_DONE_YET;

            message = 0; //clear the message as we have processed the event
        }
        break ;

        case DWT_SIG_TX_AA_DONE: //ignore this event - just process the rx frame that was received before the ACK response
        case 0:
        default :
        {
            if(message) // == DWT_SIG_TX_DONE)
            {
                inst->stateMachine.done = INST_DONE_WAIT_FOR_NEXT_EVENT;
            }

            if(inst->stateMachine.done == INST_NOT_DONE_YET)
            {
                inst->stateMachine.done = INST_DONE_WAIT_FOR_NEXT_EVENT;
            }

            ///*status = dwt_read32bitoffsetreg(0x0F,0);//SYS_STATUS_ID
            ///(*status)++;
            uint32_t curTick = HAL_GetTick();
            if(monitor.pollRecvTick > 0)
            {
                if(monitor.pollRecvTick > curTick)
                {
                    monitor.pollRecvTick = curTick;
                }
                if(curTick - monitor.pollRecvTick > inst->setting.sframePeriod)// * 10)
                {
                    //dwt_forcetrxoff();
                    //dwt_rxreset();
                    monitor.startTick = curTick;
                    monitor.pollRecvTick = 0;
                }
            }
            else
            {
                if(monitor.startTick > curTick)
                {
                    monitor.startTick = curTick;
                }
                if(curTick - monitor.startTick > (3*1000))
                {
                    dwt_forcetrxoff();
                    dwt_rxreset();
                    instProcessRXtimeout(inst);
                    inst->stateMachine.gotTO = 0;
                    inst->stateMachine.wait4ack = 0 ; //if Resp timeout, clear this

                    monitor.startTick = curTick;

                    //sprintf((char*)debugBuffer,"Crash DWT_SIGNAL \r\n");
                    //DebugMessage(debugBuffer);
                }
            }
        }
        break;
    }
    return 0;
}

// -------------------------------------------------------------------------------------------------------------------
//
// the main instance state machine (all the instance modes Tag, Anchor or Listener use the same statemachine....)
//
// -------------------------------------------------------------------------------------------------------------------
//
int testAppRun(instance_data_t *inst, int message)
{
    uint32_t status=0;
    int rtnCode = 0;
    switch (inst->stateMachine.testAppState)
    {
        case TA_INIT :
            rtnCode = appState_TA_INIT(inst, &status, message);
        break; // end case TA_INIT
        case TA_TX_WAIT_CONF :
            rtnCode = appState_TA_TX_WAIT_CONF(inst, &status, message);
            if (rtnCode != 0) break;
            message = 0;
            //break ; // end case TA_TX_WAIT_CONF
        case TA_RXE_WAIT :
            rtnCode = appState_TA_RXE_WAIT(inst, &status, message);
            if (rtnCode != 0) break;
        case TA_RX_WAIT_DATA :                                                                     // Wait RX data
            rtnCode = appState_TA_RX_WAIT_DATA(inst, &status, message);
        break ; // end case TA_RX_WAIT_DATA
        default:
            //printf("\nERROR - invalid state %d - what is going on??\n", inst->testAppState) ;
        break;
    } // end switch on testAppState

    return inst->stateMachine.done;
} // end testapprun()

// -------------------------------------------------------------------------------------------------------------------
// function to set the fixed reply delay time (in us)
//
// This sets delay for RX to TX - Delayed Send, and for TX to RX delayed receive (wait for response) functionality,
// and the frame wait timeout value to use.  This is a function of data rate, preamble length, and PRF
void instSetReplyDelay(int delayus) //delay in us
{
    int margin = 3000; //2000 symbols
    int respframe = 0;
    int respframe_sy = 0;

	//configure the rx delay receive delay time, it is dependent on the message length
	float msgdatalen = 0;
	float preamblelen = 0;
	int sfdlen = 0;
	int x = 0;

	//Set the RX timeouts based on the longest expected message - the Final message
	//Poll = 13, Response = 20, Final = 44 bytes
	//msgdatalen = TAG_FINAL_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC;
	msgdatalen = ANCH_RESPONSE_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC;

	x = (int) ceil(msgdatalen*8/330.0f);

	msgdatalen = msgdatalen*8 + x*48;

	//add some margin so we don't timeout too soon
	margin = 0; //(TAG_FINAL_MSG_LEN - TAG_POLL_MSG_LEN);

	x = (int) ceil(margin*8/330.0f);

	margin = margin*8 + x*48;

	//assume PHR length is 172308ns for 110k and 21539ns for 850k/6.81M
	if(gInst->setting.configData.dataRate == DWT_BR_110K)
    {
		msgdatalen *= 8205.13f;
		msgdatalen += 172308; // PHR length in nanoseconds

		margin *= 8205.13f;

    }
	else if(gInst->setting.configData.dataRate == DWT_BR_850K)
    {
		msgdatalen *= 1025.64f;
		msgdatalen += 21539; // PHR length in nanoseconds

		margin *= 1025.64f;
    }
	else
    {
		msgdatalen *= 128.21f;
		msgdatalen += 21539; // PHR length in nanoseconds

		margin *= 128.21f;
	}

	//SFD length is 64 for 110k (always)
	//SFD length is 8 for 6.81M, and 16 for 850k, but can vary between 8 and 16 bytes
	sfdlen = dwnsSFDlen[gInst->setting.configData.dataRate];

	switch (gInst->setting.configData.txPreambLength)
    {
    case DWT_PLEN_4096 : preamblelen = 4096.0f; break;
    case DWT_PLEN_2048 : preamblelen = 2048.0f; break;
    case DWT_PLEN_1536 : preamblelen = 1536.0f; break;
    case DWT_PLEN_1024 : preamblelen = 1024.0f; break;
    case DWT_PLEN_512  : preamblelen = 512.0f;  break;
    case DWT_PLEN_256  : preamblelen = 256.0f;  break;
    case DWT_PLEN_128  : preamblelen = 128.0f;  break;
    case DWT_PLEN_64   : preamblelen = 64.0f;   break;
    }

	//preamble  = plen * (994 or 1018) depending on 16 or 64 PRF
	if(gInst->setting.configData.prf == DWT_PRF_16M)
	{
		preamblelen = (sfdlen + preamblelen) * 0.99359f;
	}
	else
	{
		preamblelen = (sfdlen + preamblelen) * 1.01763f;
	}

	//respframe_sy = (16 + (int)((preamblelen + ((msgdatalen + margin)/1000.0))/ 1.0256)) ;
     respframe_sy = (16 + (int)ceil((preamblelen + ((msgdatalen + margin)/1000.0))/ 1.0256)) ;

	//this is the delay used for the delayed transmit (when sending the response, and final messages)
    gInst->setting.pollTx2FinalTxDelay = convertMicroSec2DeviceTime (delayus);
	//the anchor to anchor ranging consist of A0 ranging to A1 and A2 and A1 ranging to A2
	//as there are less messages the ranging time is shorter (thus divide by 2)
    gInst->setting.pollTx2FinalTxDelayAnc = convertMicroSec2DeviceTime (delayus/2 + 100);

	//this is the delay the anchors 1, 2, etc.. will send the response back at...
	//anchor 2 will have the delay set to 2 * fixedReplyDelayAnc
	//andhor 3 will have the delay set to 3 * fixedReplyDelayAnc and so on...
	//this delay depends on how quickly the tag can receive and process the message from previous anchor
	//(and also the frame length of course)
	//respframe = (int)(preamblelen + (msgdatalen/1000.0)); //length of response frame (micro seconds)
    respframe = (int)ceil(preamblelen + (msgdatalen/1000.0)); //length of response frame (micro seconds)
	if(gInst->setting.configData.dataRate == DWT_BR_110K)
	{
		//set the frame wait timeout time - total time the frame takes in symbols
        gInst->setting.fwtoTime_sy = respframe_sy + RX_RESPONSE1_TURNAROUND_110K + 400; //add some margin because of the resp to resp RX turn on time
        //instance_data[instance].fwtoTime_sy = respframe_sy + RX_RESPONSE1_TURNAROUND_110K + 400 + 2000;

        gInst->setting.fwtoTimeAnc_sy = respframe_sy; //add some margin so we don't timeout too soon
        gInst->setting.fixedReplyDelayAnc = convertMicroSec2DeviceTime (respframe + RX_RESPONSE1_TURNAROUND_110K);
        gInst->setting.fixedReplyDelayAncP = (uint32_t) (((uint64_t) convertMicroSec2DeviceTime (preamblelen)) >> 8) + 16;

        gInst->setting.ancRespRxDelay = RX_RESPONSE1_TURNAROUND_110K ;
	}
	else
	{
		//set the frame wait timeout time - total time the frame takes in symbols
        gInst->setting.fwtoTime_sy = respframe_sy + RX_RESPONSE1_TURNAROUND_6M81; //add some margin because of the resp to resp RX turn on time

        gInst->setting.fwtoTimeAnc_sy =  respframe_sy;
        gInst->setting.fixedReplyDelayAnc = convertMicroSec2DeviceTime (respframe + RX_RESPONSE1_TURNAROUND_6M81);
        gInst->setting.fixedReplyDelayAncP = (uint32_t) (((uint64_t) convertMicroSec2DeviceTime (preamblelen)) >> 8) + 16;

        gInst->setting.ancRespRxDelay = RX_RESPONSE1_TURNAROUND_6M81;
        //gInst->setting.ancRespRxDelay = RX_RESPONSE1_TURNAROUND ;
	}
    gInst->setting.pollRx2FinalRxDelay = delayus + gInst->setting.fwtoTime_sy;
}

// -------------------------------------------------------------------------------------------------------------------
//
// Set Payload parameters for the instance
//
// -------------------------------------------------------------------------------------------------------------------
void instSetAddress(uint16_t address)
{
    gInst->setting.instanceAddress16 = address ;       // copy configurations
}

void instSetPanID(uint16_t id)
{
    gInst->setting.panID = id ;
}
void instUpdateFramePeroid(uint16_t period)
{
    gInst->setting.sframePeriod = period;

    gInst->setting.numSlots = gInst->setting.sframePeriod / (gInst->setting.slotPeriod * TA_SLOT_STEP);
}