AMiRo-OS

/*! ----------------------------------------------------------------------------

 *  @file    instance_common.c

 *  @brief   DecaWave application level common instance functions

 *

 * @attention

 *

 * Copyright 2015 (c) DecaWave Ltd, Dublin, Ireland.

 *

 * All rights reserved.

 *

 * @author DecaWave

 */

#include <alld_DW1000.h>

#if (defined(AMIROLLD_CFG_DW1000) && (AMIROLLD_CFG_DW1000 == 1)) || defined(__DOXYGEN__)

#include "module.h"

#include <string.h>

#include <math.h>

extern double dwt_getrangebias(uint8_t chan, float range, uint8_t prf);

extern const uint16_t rfDelays[2];

extern const uint16_t rfDelaysTREK[2];

extern const tx_struct txSpectrumConfig[8];

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

// Deca Calibration Values

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

#define DWT_PRF_64M_RFDLY   (514.462f)

#define DWT_PRF_16M_RFDLY   (513.9067f)

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

//The table below specifies the default TX spectrum configuration parameters... this has been tuned for DW EVK hardware units

//the table is set for smart power - see below in the instance_config function how this is used when not using smart power

const tx_struct txSpectrumConfig[8] =

{

    //Channel 0 ----- this is just a place holder so the next array element is channel 1

    {

            0x0,   //0

            {

                    0x0, //0

                    0x0 //0

            }

    },

    //Channel 1

    {

            0xc9,   //PG_DELAY

            {

                    0x15355575, //16M prf power

                    0x07274767 //64M prf power

            }

    },

    //Channel 2

    {

            0xc2,   //PG_DELAY

            {

                    0x15355575, //16M prf power

                    0x07274767 //64M prf power

            }

    },

    //Channel 3

    {

            0xc5,   //PG_DELAY

            {

                    0x0f2f4f6f, //16M prf power

                    0x2b4b6b8b //64M prf power

            }

    },

    //Channel 4

    {

            0x95,   //PG_DELAY

            {

                    0x1f1f3f5f, //16M prf power

                    0x3a5a7a9a //64M prf power

            }

    },

    //Channel 5

    {

            0xc0,   //PG_DELAY

            {

                    0x0E082848, //16M prf power

                    0x25456585 //64M prf power

            }

    },

    //Channel 6 ----- this is just a place holder so the next array element is channel 7

    {

            0x0,   //0

            {

                    0x0, //0

                    0x0 //0

            }

    },

    //Channel 7

    {

            0x93,   //PG_DELAY

            {

                    0x32527292, //16M prf power

                    0x5171B1d1 //64M prf power

            }

    }

};

//these are default antenna delays for EVB1000, these can be used if there is no calibration data in the DW1000,

//or instead of the calibration data

const uint16_t rfDelays[2] = {

        (uint16_t) ((DWT_PRF_16M_RFDLY/ 2.0) * 1e-9 / DWT_TIME_UNITS),//PRF 16

        (uint16_t) ((DWT_PRF_64M_RFDLY/ 2.0) * 1e-9 / DWT_TIME_UNITS)

};

//these are default TREK Tag/Anchor antenna delays

const uint16_t rfDelaysTREK[2] = {

        (uint16_t) ((514.83f/ 2.0) * 1e-9 / DWT_TIME_UNITS),//channel 2

        (uint16_t) ((514.65f/ 2.0) * 1e-9 / DWT_TIME_UNITS) //channel 5

};

//int instance_starttxtest(int framePeriod)

//{

//    //define some test data for the tx buffer

//    uint8 msg[127] = "The quick brown fox jumps over the lazy dog. The quick brown fox jumps over the lazy dog. The quick brown fox jumps over the l";

//    //NOTE: SPI frequency must be < 3MHz

//    port_set_dw1000_slowrate();  //max SPI before PLLs configured is ~4M

//    // the value here 0x1000 gives a period of 32.82 µs

//    //this is setting 0x1000 as frame period (125MHz clock cycles) (time from Tx en - to next - Tx en)

//    dwt_configcontinuousframemode(framePeriod);

//    dwt_writetxdata(127, (uint8 *)  msg, 0) ;

//    dwt_writetxfctrl(127, 0, 0);

//    //to start the first frame - set TXSTRT

//    dwt_starttx(DWT_START_TX_IMMEDIATE);

//    //measure the power

//    //Spectrum Analyser set:

//    //FREQ to be channel default e.g. 3.9936 GHz for channel 2

//    //SPAN to 1GHz

//    //SWEEP TIME 1s

//    //RBW and VBW 1MHz

//    //measure channel power

//    return DWT_SUCCESS ;

//}

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

//      Data Definitions

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

instance_data_t instance_data[NUM_INST] ;

static double inst_tdist[MAX_TAG_LIST_SIZE] ;

static double inst_idist[MAX_ANCHOR_LIST_SIZE] ;

static double inst_idistraw[MAX_ANCHOR_LIST_SIZE] ;

// TODO: Instance_data declaration in .c. Not on .h (extern only)

instance_data_t instance_data[NUM_INST] ;

/* TODO: Custom data declarations */

typedef struct

{

    uint32_t      deviceID ;

    uint8_t       chan;               // added chan here - used in the reading of acc

    uint16_t      rfrxDly;            // rf delay (delay though the RF blocks before the signal comes out of the antenna i.e. "antenna delay")

    uint16_t      rftxDly;            // rf delay (delay though the RF blocks before the signal comes out of the antenna i.e. "antenna delay")

    uint32_t      antennaDly;         // antenna delay read from OTP 64 PRF value is in high 16 bits and 16M PRF in low 16 bits

    uint32_t      antennaCals[4];     // antenna delays for the TREKs (Anchor high 16-bits, Tag low 16-bits)

    uint32_t      txPowCfg[12];       // stores the Tx power configuration read from OTP (6 channels consecutively with PRF16 then 64, e.g. Ch 1 PRF16 is index 0 and 64 index 1)

    uint32_t      states[3] ;         //MP workaround debug states register

    uint8_t       statescount ;

    int           prfIndex ;

    uint32_t      ldoTune ;           //low 32 bits of LDO tune value

} platform_local_data_t ;

static platform_local_data_t platformLocalData ; // Static local device data

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

// Functions

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

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

// convert microseconds to device time

uint64_t convertmicrosectodevicetimeu (double microsecu){

  uint64_t dt;

  long double dtime;

  dtime = (microsecu / (double) DWT_TIME_UNITS) / 1e6 ;

  dt =  (uint64_t) (dtime) ;

  return dt;

}

double convertdevicetimetosec(int32_t dt){

  double f = 0;

  f =  dt * DWT_TIME_UNITS ;  // seconds #define TIME_UNITS          (1.0/499.2e6/128.0) = 15.65e-12

  return f ;

}

#pragma GCC optimize ("O3")

int reportTOF(int idx, uint32_t tofx){

  double distance ;

  double distance_to_correct;

  double tof ;

  int32_t tofi ;

  // check for negative results and accept them making them proper negative integers

  tofi = (int32_t) tofx ; // make it signed

  if (tofi > 0x7FFFFFFF){  // close up TOF may be negative

    tofi -= 0x80000000 ;  //

  }

  // convert to seconds (as floating point)

  tof = convertdevicetimetosec(tofi) ;          //this is divided by 4 to get single time of flight

  inst_idistraw[idx] = distance = tof * SPEED_OF_LIGHT;

#if (CORRECT_RANGE_BIAS == 1)

  //for the 6.81Mb data rate we assume gating gain of 6dB is used,

  //thus a different range bias needs to be applied

  //if(inst->configData.dataRate == DWT_BR_6M8)

  if(instance_data[0].configData.smartPowerEn){

    //1.31 for channel 2 and 1.51 for channel 5

    if(instance_data[0].configData.chan == 5){

      distance_to_correct = distance/1.51;

    }

    else{   //channel 2

      distance_to_correct = distance/1.31;

    }

  }

  else{

    distance_to_correct = distance;

  }

  distance = distance - dwt_getrangebias(instance_data[0].configData.chan, (float) distance_to_correct, instance_data[0].configData.prf);

#endif

  if ((distance < 0) || (distance > 20000.000))    // discard any results less than <0 cm or >20 km

    return 0;

  inst_idist[idx] = distance;

  instance_data[0].longTermRangeCount++ ;                          // for computing a long term average

  return 1;

}// end of reportTOF

void setTagDist(int tidx, int aidx){

  inst_tdist[tidx] = inst_idist[aidx];

}

double getTagDist(int idx){

  return inst_tdist[idx];

}

void clearDistTable(int idx){

  inst_idistraw[idx] = 0;

  inst_idist[idx] = 0;

}

void instancecleardisttableall(void){

  int i;

  for(i=0; i<MAX_ANCHOR_LIST_SIZE; i++)  {

    inst_idistraw[i] = 0xffff;

    inst_idist[i] = 0xffff;

  }

}

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

#if NUM_INST != 1

#error These functions assume one instance only

#else

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

// Set this instance role as the Tag, Anchor or Listener

void instancesetrole(int inst_mode){

  // assume instance 0, for this

  instance_data[0].mode =  inst_mode;                   // set the role

}

int instancegetrole(void){

  return instance_data[0].mode;

}

int instancenewrange(void){

  int x = instance_data[0].newRange;

  instance_data[0].newRange = TOF_REPORT_NUL;

  return x;

}

int instancenewrangeancadd(void){

  return instance_data[0].newRangeAncAddress;

}

int instancenewrangetagadd(void){

  return instance_data[0].newRangeTagAddress;

}

int instancenewrangetim(void){

  return instance_data[0].newRangeTime;

}

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

// function to clear counts/averages/range values

//

void instanceclearcounts(void){

  int instance = 0 ;

  int i= 0 ;

  instance_data[instance].rxTimeouts = 0 ;

  dwt_configeventcounters(1); //enable and clear - NOTE: the counters are not preserved when in DEEP SLEEP

  instance_data[instance].frameSN = 0;

  instance_data[instance].longTermRangeCount  = 0;

  for(i=0; i<MAX_ANCHOR_LIST_SIZE; i++){

    instance_data[instance].tofArray[i] = INVALID_TOF;

  }

  for(i=0; i<MAX_TAG_LIST_SIZE; i++){

    instance_data[instance].tof[i] = INVALID_TOF;

  }

} // end instanceclearcounts()

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

// function to initialise instance structures

//

// Returns 0 on success and -1 on error

int instance_init(void){

  int instance = 0 ;

  int i;

  int result;

  instance_data[instance].mode =  ANCHOR;                                // assume listener,

  instance_data[instance].testAppState = TA_INIT ;

  instance_data[instance].instToSleep = FALSE;

  // Reset the IC (might be needed if not getting here from POWER ON)

  // ARM code: Remove soft reset here as using hard reset in the inittestapplication() in the main.c file

  //dwt_softreset();

  //this initialises DW1000 and uses specified configurations from OTP/ROM

  result = dwt_initialise(DWT_LOADUCODE) ;

  //this is platform dependent - only program if DW EVK/EVB

  dwt_setleds(3) ; //configure the GPIOs which control the leds on EVBs

  if (DWT_SUCCESS != result){

    return (-1) ;   // device initialise has failed

  }

  instanceclearcounts() ;

  instance_data[instance].panID = 0xdeca ;

  instance_data[instance].wait4ack = 0;

  instance_data[instance].stopTimer = 0;

  instance_data[instance].instanceTimerEn = 0;

  instance_clearevents();

  //dwt_geteui(instance_data[instance].eui64);

  memset(instance_data[instance].eui64, 0, ADDR_BYTE_SIZE_L);

  instance_data[instance].tagSleepCorrection = 0;

//  dwt_setautorxreenable(0); //disable auto RX re-enable

  dwt_setdblrxbuffmode(0); //disable double RX buffer

  // if using auto CRC check (DWT_INT_RFCG and DWT_INT_RFCE) are used instead of DWT_INT_RDFR flag

  // other errors which need to be checked (as they disable receiver) are

  //dwt_setinterrupt(DWT_INT_TFRS | DWT_INT_RFCG | (DWT_INT_SFDT | DWT_INT_RFTO /*| DWT_INT_RXPTO*/), 1);

  dwt_setinterrupt(DWT_INT_TFRS | DWT_INT_RFCG | (DWT_INT_ARFE | DWT_INT_RFSL | DWT_INT_SFDT | DWT_INT_RPHE | DWT_INT_RFCE | DWT_INT_RFTO | DWT_INT_RXPTO), 1);

  dwt_setcallbacks(instance_txcallback, instance_rxcallback, instance_rxtimeoutcallback,instance_rxerrorcallback );

  instance_data[instance].monitor = 0;

  instance_data[instance].lateTX = 0;

  instance_data[instance].lateRX = 0;

  instance_data[instance].responseTO = -1; //initialise

  for(i=0; i<256; i++)  {

    instance_data[instance].rxResps[i] = -10;

  }

  instance_data[instance].delayedReplyTime = 0;

  return 0 ;

}

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

//

// Return the Device ID register value, enables higher level validation of physical device presence

//

uint32_t instancereaddeviceid(void){

  return dwt_readdevid() ;

}

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

//

// function to allow application configuration be passed into instance and affect underlying device operation

//

void instance_config(instanceConfig_t *config, sfConfig_t *sfConfig){

  int instance = 0 ;

  uint32_t power = 0;

  uint8_t otprev ;

  instance_data[instance].configData.chan = config->channelNumber ;

  instance_data[instance].configData.rxCode =  config->preambleCode ;

  instance_data[instance].configData.txCode = config->preambleCode ;

  instance_data[instance].configData.prf = config->pulseRepFreq ;

  instance_data[instance].configData.dataRate = config->dataRate ;

  instance_data[instance].configData.txPreambLength = config->preambleLen ;

  instance_data[instance].configData.rxPAC = config->pacSize ;

  instance_data[instance].configData.nsSFD = config->nsSFD ;

  instance_data[instance].configData.phrMode = DWT_PHRMODE_STD ;

  instance_data[instance].configData.sfdTO = config->sfdTO;

  //the DW1000 will automatically use gating gain for frames < 1ms duration (i.e. 6.81Mbps data rate)

  //smartPowerEn should be set based on the frame length, but we can also use dtaa rate.

  if(instance_data[instance].configData.dataRate == DWT_BR_6M8)  {

    instance_data[instance].configData.smartPowerEn = 1;

  }

  else{

    instance_data[instance].configData.smartPowerEn = 0;

  }

  //configure the channel parameters

  dwt_configure(&instance_data[instance].configData) ;

  instance_data[instance].configTX.PGdly = txSpectrumConfig[config->channelNumber].PGdelay ;    //Default

  //firstly check if there are calibrated TX power value in the DW1000 OTP

  power = dwt_getotptxpower(config->pulseRepFreq, instance_data[instance].configData.chan);

  if((power == 0x0) || (power == 0xFFFFFFFF)) {    //if there are no calibrated values... need to use defaults

    power = txSpectrumConfig[config->channelNumber].txPwr[config->pulseRepFreq- DWT_PRF_16M];

  }

  //Configure TX power

  instance_data[instance].configTX.power = power;

  //configure the tx spectrum parameters (power and PG delay)

  dwt_configuretxrf(&instance_data[instance].configTX);

  otprev = dwt_otprevision() ;  // this revision tells us how OTP is programmed.

  if ((2 == otprev) || (3 == otprev))  {  // board is calibrated with TREK1000 with antenna delays set for each use case)

    uint8_t mode = (instance_data[instance].mode == ANCHOR ? 1 : 0);

    uint8_t chanindex = 0;

    instance_data[instance].txAntennaDelay

    = dwt_getTREKOTPantennadelay(mode,

                                 instance_data[instance].configData.chan,

                                 instance_data[instance].configData.dataRate) ;

    // if nothing was actually programmed then set a reasonable value anyway

    if ((instance_data[instance].txAntennaDelay == 0)

        || (instance_data[instance].txAntennaDelay == 0xffff)){

      if(instance_data[instance].configData.chan == 5){

        chanindex = 1;

      }

      instance_data[instance].txAntennaDelay = rfDelaysTREK[chanindex];

    }

  }

  else {  // assume it is older EVK1000 programming.

    //get the antenna delay that was read from the OTP calibration area

    instance_data[instance].txAntennaDelay = dwt_readantennadelay(config->pulseRepFreq) >> 1;

    // if nothing was actually programmed then set a reasonable value anyway

    if ((instance_data[instance].txAntennaDelay == 0)

        || (instance_data[instance].txAntennaDelay == 0xffff)){

      instance_data[instance].txAntennaDelay = rfDelays[config->pulseRepFreq - DWT_PRF_16M];

    }

  }

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

  // set the antenna delay, we assume that the RX is the same as TX.

  dwt_setrxantennadelay(instance_data[instance].txAntennaDelay);

  dwt_settxantennadelay(instance_data[instance].txAntennaDelay);

  instance_data[instance].rxAntennaDelay = instance_data[instance].txAntennaDelay;

  if(config->preambleLen == DWT_PLEN_64)  {  //if preamble length is 64

    //reduce SPI to < 3MHz

    setHighSpeed_SPI(FALSE);  //TODO: workaround using flag to change speed

    dwt_loadopsettabfromotp(0);

    //increase SPI to max

    setHighSpeed_SPI(TRUE);  //TODO: workaround using flag to change speed

  }

  instancesettagsleepdelay(sfConfig->pollSleepDly); //set the Tag sleep time

  instance_data[instance].sframePeriod = sfConfig->sfPeriod;

  instance_data[instance].slotPeriod = sfConfig->slotPeriod;

  instance_data[instance].tagSleepRnd = sfConfig->slotPeriod;

  instance_data[instance].numSlots = sfConfig->numSlots;

  //last two slots are used for anchor to anchor ranging

  instance_data[instance].a0SlotTime = (instance_data[instance].numSlots-2) * instance_data[instance].slotPeriod;

  //set the default response delays

  instancesetreplydelay(sfConfig->replyDly);

}

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

// function to set the tag sleep time (in ms)

//

void instancesettagsleepdelay(int sleepdelay) {  //sleep in ms

  int instance = 0 ;

  instance_data[instance].tagSleepTime_ms = sleepdelay; //subtract the micro system delays (time it takes to switch states etc.)

}

int instancegetrnum(void) {  //get ranging number

  return instance_data[0].rangeNum;

}

int instancegetrnuma(int idx){  //get ranging number

  return instance_data[0].rangeNumA[idx];

}

int instancegetrnumanc(int idx){  //get ranging number

  return instance_data[0].rangeNumAAnc[idx];

}

int instancegetlcount(void) {  //get count of ranges used for calculation of lt avg

  int x = instance_data[0].longTermRangeCount;

  return (x);

}

double instancegetidist(int idx) {  //get instantaneous range

  double x ;

  idx &= (MAX_ANCHOR_LIST_SIZE - 1);

  x = inst_idist[idx];

  return (x);

}

double instancegetidistraw(int idx) { //get instantaneous range (uncorrected)

  double x ;

  idx &= (MAX_ANCHOR_LIST_SIZE - 1);

  x = inst_idistraw[idx];

  return (x);

}

int instancegetidist_mm(int idx) {  //get instantaneous range

  int x ;

  idx &= (MAX_ANCHOR_LIST_SIZE - 1);

  x = (int)(inst_idist[idx]*1000);

  return (x);

}

int instancegetidistraw_mm(int idx) {  //get instantaneous range (uncorrected)

  int x ;

  idx &= (MAX_ANCHOR_LIST_SIZE - 1);

  x = (int)(inst_idistraw[idx]*1000);

  return (x);

}

void instance_backtoanchor(instance_data_t *inst){

  //stay in RX and behave as anchor

  inst->testAppState = TA_RXE_WAIT ;

  inst->mode = ANCHOR ;

  dwt_setrxtimeout(0);

  dwt_setpreambledetecttimeout(0);

  dwt_setrxaftertxdelay(0);

}

#pragma GCC optimize ("O3")

void inst_processrxtimeout(instance_data_t *inst){

  //inst->responseTimeouts ++ ;

  inst->rxTimeouts ++ ;

  inst->done = INST_NOT_DONE_YET;

  if(inst->mode == ANCHOR) { //we did not receive the final - wait for next poll

    //only enable receiver when not using double buffering

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

    dwt_setrxtimeout(0);

  }

  else if(inst->mode == TAG) {

    //if tag times out - no response (check if we are to send a final)

    //send the final only if it has received response from anchor 0

    if((inst->previousState == TA_TXPOLL_WAIT_SEND) && ((inst->rxResponseMask & 0x1) == 0)) {

      inst->instToSleep = TRUE ; //set sleep to TRUE so that tag will go to DEEP SLEEP before next ranging attempt

      inst->testAppState = TA_TXE_WAIT ;

      inst->nextState = TA_TXPOLL_WAIT_SEND ;

    }

    else if (inst->previousState == TA_TXFINAL_WAIT_SEND) {  //got here from main (error sending final - handle as timeout)

      dwt_forcetrxoff();        //this will clear all events

      inst->instToSleep = TRUE ;

      // initiate the re-transmission of the poll that was not responded to

      inst->testAppState = TA_TXE_WAIT ;

      inst->nextState = TA_TXPOLL_WAIT_SEND ;

    }

    else {  //send the final

      // initiate the re-transmission of the poll that was not responded to

      inst->testAppState = TA_TXE_WAIT ;

      inst->nextState = TA_TXFINAL_WAIT_SEND ;

    }

  }

  else {  //ANCHOR_RNG

    //no Response form the other anchor

    if(

        ((inst->previousState == TA_TXPOLL_WAIT_SEND)

            && ((A1_ANCHOR_ADDR == inst->instanceAddress16) && ((inst->rxResponseMaskAnc & 0x4) == 0)))

            ||

            ((inst->previousState == TA_TXPOLL_WAIT_SEND)

                && ((GATEWAY_ANCHOR_ADDR == inst->instanceAddress16) && ((inst->rxResponseMaskAnc & 0x2) == 0)))

    )  {

      instance_backtoanchor(inst);

    }

    else if (inst->previousState == TA_TXFINAL_WAIT_SEND) {  //got here from main (error ending final - handle as timeout)

      instance_backtoanchor(inst);

    }

    else {  //send the final

      // initiate the re-transmission of the poll that was not responded to

      inst->testAppState = TA_TXE_WAIT ;

      inst->nextState = TA_TXFINAL_WAIT_SEND ;

    }

  }

  //timeout - disable the radio (if using SW timeout the rx will not be off)

  //dwt_forcetrxoff() ;

}

//

// NB: This function is called from the (TX) interrupt handler

//

#pragma GCC optimize ("O3")

void instance_txcallback(const dwt_cb_data_t *txd){

  (void) txd;

  int instance = 0;

  uint8_t txTimeStamp[5] = {0, 0, 0, 0, 0};

//  uint8 txevent = txd->event;

  event_data_t dw_event;

  dw_event.uTimeStamp = portGetTickCnt();

//  chprintf((BaseSequentialStream *)&SD2, " TX callback! \r\n");

//  if(txevent == DWT_SIG_TX_DONE) {

    //uint32_t txtimestamp = 0;

    //NOTE - we can only get TX good (done) while here

    //dwt_readtxtimestamp((uint8*) &instance_data[instance].txu.txTimeStamp);

    dwt_readtxtimestamp(txTimeStamp) ;

    dw_event.timeStamp32l = (uint32_t)txTimeStamp[0] + ((uint32_t)txTimeStamp[1] << 8) + ((uint32_t)txTimeStamp[2] << 16) + ((uint32_t)txTimeStamp[3] << 24);

    dw_event.timeStamp = txTimeStamp[4];

    dw_event.timeStamp <<= 32;

    dw_event.timeStamp += dw_event.timeStamp32l;

    dw_event.timeStamp32h = ((uint32_t)txTimeStamp[4] << 24) + (dw_event.timeStamp32l >> 8);

    instance_data[instance].stopTimer = 0;

    dw_event.rxLength = instance_data[instance].psduLength;

    dw_event.type =  0;

    dw_event.type_pend =  0;

    dw_event.type_save = DWT_SIG_TX_DONE;

    memcpy((uint8_t *)&dw_event.msgu.frame[0], (uint8_t *)&instance_data[instance].msg_f, instance_data[instance].psduLength);

    instance_putevent(dw_event, DWT_SIG_TX_DONE);

    instance_data[instance].txMsgCount++;

//  }

//  else if(txevent == DWT_SIG_TX_AA_DONE)  {

//    //auto ACK confirmation

//    dw_event.rxLength = 0;

//    dw_event.type =  0;

//    dw_event.type_save = DWT_SIG_TX_AA_DONE;

//

//    instance_putevent(dw_event, DWT_SIG_TX_AA_DONE);

//

//    //printf("TX AA time %f ecount %d\n",convertdevicetimetosecu(instance_data[instance].txu.txTimeStamp), instance_data[instance].dweventCnt);

//  }

  instance_data[instance].monitor = 0;

}

/**

 * @brief function to re-enable the receiver and also adjust the timeout before sending the final message

 * if it is time so send the final message, the callback will notify the application, else the receiver is

 * automatically re-enabled

 *

 * this function is only used for tag when ranging to other anchors

 */

uint8_t tagrxreenable(uint16_t sourceAddress){

  uint8_t type_pend = DWT_SIG_DW_IDLE;

  uint8_t anc = sourceAddress & 0x3;

  int instance = 0;

  switch(anc){

  //if we got Response from anchor 3 - this is the last expected response - send the final

  case 3:

    type_pend = DWT_SIG_DW_IDLE;

    break;

    //if we got Response from anchor 0, 1, or 2 - go back to wait for next anchor's response

  case 0:

  case 1:

  case 2:

  default:

    if(instance_data[instance].responseTO > 0) {  //can get here as result of error frame so need to check

      dwt_setrxtimeout((uint16_t)instance_data[instance].fwtoTime_sy * instance_data[instance].responseTO); //reconfigure the timeout

      dwt_rxenable(DWT_START_RX_IMMEDIATE) ;

      type_pend = DWT_SIG_RX_PENDING ;

    }

    else //last response was not received (got error/frame was corrupt)

    {

      type_pend = DWT_SIG_DW_IDLE; //report timeout - send the final

    }

    break;

  }

  return type_pend;

}

/**

 * @brief function to re-enable the receiver and also adjust the timeout before sending the final message

 * if it is time so send the final message, the callback will notify the application, else the receiver is

 * automatically re-enabled

 *

 * this function is only used for anchors (having a role of ANCHOR_RNG) when ranging to other anchors

 */

uint8_t ancsendfinalorrxreenable(uint16_t sourceAddress){

  uint8_t type_pend = DWT_SIG_DW_IDLE;

  uint8_t anc = sourceAddress & 0x3;

  int instance = 0;

  if(instance_data[instance].instanceAddress16 == GATEWAY_ANCHOR_ADDR) {

    switch(anc) {

    //if we got Response from anchor 1 - go back to wait for next anchor's response

    case 1:

      dwt_setrxtimeout((uint16_t)instance_data[instance].fwtoTime_sy); //reconfigure the timeout

      dwt_rxenable(DWT_START_RX_IMMEDIATE) ;

      type_pend = DWT_SIG_RX_PENDING ;

      break;

      //if we got Response from anchor 2 - this is the last expected response - send the final

    case 2:

    default:

      type_pend = DWT_SIG_DW_IDLE;

      break;

    }

  }

  if(instance_data[instance].instanceAddress16 == A1_ANCHOR_ADDR){

    switch(anc)

    {

    //if we got Response from anchor 2 - this is the last expected response - send the final

    case 2:

    default:

      type_pend = DWT_SIG_DW_IDLE;

      break;

    }

  }

  return type_pend;

}

/**

 * @brief this function either enables the receiver (delayed)

 *

 **/

void ancenablerx(void){

  int instance = 0;

  //subtract preamble length

  dwt_setdelayedtrxtime(instance_data[instance].delayedReplyTime - instance_data[instance].fixedReplyDelayAncP) ;

  if(dwt_rxenable(DWT_START_RX_DELAYED)) {  //delayed rx

    //if the delayed RX failed - time has passed - do immediate enable

    //led_on(LED_PC9);

    dwt_setrxtimeout((uint16_t)instance_data[instance].fwtoTimeAnc_sy*2); //reconfigure the timeout before enable

    //longer timeout as we cannot do delayed receive... so receiver needs to stay on for longer

    dwt_rxenable(DWT_START_RX_IMMEDIATE);

    dwt_setrxtimeout((uint16_t)instance_data[instance].fwtoTimeAnc_sy); //restore the timeout for next RX enable

    instance_data[instance].lateRX++;

    //led_off(LED_PC9);

  }

}

/**

 * @brief this function either re-enables the receiver (delayed or immediate) or transmits the response frame

 *

 * @param the sourceAddress is the address of the sender of the current received frame

 * @param ancToAncTWR == 1 means that the anchor is ranging to another anchor, if == 0 then ranging to a tag

 *

 */

#pragma GCC optimize ("O0")

uint8_t anctxorrxreenable(uint16_t sourceAddress, int ancToAncTWR){

  uint8_t type_pend = DWT_SIG_DW_IDLE;

  int sendResp = 0;

  int instance = 0;

  if(instance_data[instance].responseTO == 0) {  //go back to RX without TO - ranging has finished. (wait for Final but no TO)

    dwt_setrxtimeout(0); //reconfigure the timeout

    dwt_setpreambledetecttimeout(0);

  }

  if((ancToAncTWR & 1) == 1)  {

    if(instance_data[instance].responseTO == 1) {  //if one response left to receive (send a response now)

      sendResp = 1;

    }

    //if A0 or A3 go back to RX as they do not send any responses when Anchor to Anchor ranging

    if((instance_data[instance].gatewayAnchor)

        || (instance_data[instance].shortAdd_idx == 3)) { //if this is anchor ID 3 do not reply to anchor poll

      dwt_setrxtimeout(0);

      dwt_rxenable(DWT_START_RX_IMMEDIATE);

      return DWT_SIG_RX_PENDING ;

    }

  }

  //configure delayed reply time (this is incremented for each received frame) it is timed from Poll rx time

  instance_data[instance].delayedReplyTime += (instance_data[instance].fixedReplyDelayAnc >> 8);

  //this checks if to send a frame

  if((((ancToAncTWR & 1) == 0) && ((instance_data[instance].responseTO + instance_data[instance].shortAdd_idx) == NUM_EXPECTED_RESPONSES)) //it's our turn to tx

      || (sendResp == 1)) {

    //led_on(LED_PC9);

    //response is expected

    instance_data[instance].wait4ack = DWT_RESPONSE_EXPECTED; //re has/will be re-enabled

    dwt_setdelayedtrxtime(instance_data[instance].delayedReplyTime) ;

    if(dwt_starttx(DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED)) {

      //if TX has failed - we need to re-enable RX for the next response or final reception...

      dwt_setrxaftertxdelay(0);

      instance_data[instance].wait4ack = 0; //clear the flag as the TX has failed the TRX is off

      instance_data[instance].lateTX++;

      instance_data[instance].delayedReplyTime += 2*(instance_data[instance].fixedReplyDelayAnc >> 8); //to take into account W4R

      ancenablerx();

      type_pend = DWT_SIG_RX_PENDING ;

    }

    else {

      instance_data[instance].delayedReplyTime += (instance_data[instance].fixedReplyDelayAnc >> 8); //to take into account W4R

      type_pend = DWT_SIG_TX_PENDING ; // exit this interrupt and notify the application/instance that TX is in progress.

      instance_data[instance].timeofTx = portGetTickCnt();

      instance_data[instance].monitor = 1;

    }

    //led_off(LED_PC9);

  }

  else {  //stay in receive

    if(sourceAddress == 0) { //we got here after RX error, as we don't need to TX, we just enable RX

      dwt_setrxtimeout(0);

      dwt_rxenable(DWT_START_RX_IMMEDIATE);

    }

    else{

      //led_on(LED_PC9);

      ancenablerx();

      //led_off(LED_PC9);

    }

    type_pend = DWT_SIG_RX_PENDING ;

  }

  //if time to send a response

  return type_pend;

}

/**

 * @brief this function handles frame error event, it will either signal TO or re-enable the receiver

 */

void handle_error_unknownframe(event_data_t dw_event){

  int instance = 0;

  //re-enable the receiver (after error frames as we are not using auto re-enable

  //for ranging application rx error frame is same as TO - as we are not going to get the expected frame

  if(instance_data[instance].mode == ANCHOR){

    //if we are participating in the ranging (i.e. Poll was received)

    //and we get an rx error (in one of the responses)

    //need to consider this as a timeout as we could be sending our response next and

    //the applications needs to know to change the state

    //

    if(instance_data[instance].responseTO > 0){

      instance_data[instance].responseTO--;

      //send a response or re-enable rx

      dw_event.type_pend = anctxorrxreenable(0, 0);

      dw_event.type = 0;

      dw_event.type_save = 0x40 | DWT_SIG_RX_TIMEOUT;

      dw_event.rxLength = 0;

      instance_putevent(dw_event, DWT_SIG_RX_TIMEOUT);

    }

    else{

      dwt_setrxtimeout(0); //reconfigure the timeout

      dwt_rxenable(DWT_START_RX_IMMEDIATE) ;

    }

  }

  else if(instance_data[instance].mode == LISTENER){

    dwt_rxenable(DWT_START_RX_IMMEDIATE) ;

  }

  else{

    instance_data[instance].responseTO--; //got something (need to reduce timeout (for remaining responses))

    dw_event.type_pend = tagrxreenable(0); //check if receiver will be re-enabled or it's time to send the final

    dw_event.type = 0;

    dw_event.type_save = 0x40 | DWT_SIG_RX_TIMEOUT;

    dw_event.rxLength = 0;

    instance_putevent(dw_event, DWT_SIG_RX_TIMEOUT);

  }

}

/**

 * @brief this function prepares and writes the anchor to anchor response frame into the TX buffer

 * it is called after anchor receives a Poll from an anchor

 */

void ancprepareresponse2(uint16_t sourceAddress, uint8_t srcAddr_index, uint8_t fcode_index, uint8_t *frame){

  uint16_t frameLength = 0;

  uint8_t tof_idx = (sourceAddress) & 0x3 ;

  int instance = 0;

  instance_data[instance].psduLength = frameLength = ANCH_RESPONSE_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC;

  //set the destination address (copy source as this is a reply)

  memcpy(&instance_data[instance].msg_f.destAddr[0], &frame[srcAddr_index], ADDR_BYTE_SIZE_S); //remember who to send the reply to (set destination address)

  instance_data[instance].msg_f.sourceAddr[0] = instance_data[instance].eui64[0];

  instance_data[instance].msg_f.sourceAddr[1] = instance_data[instance].eui64[1];

  // Write calculated TOF into response message (get the previous ToF+range number from that anchor)

  memcpy(&(instance_data[instance].msg_f.messageData[TOFR]), &instance_data[instance].tofAnc[tof_idx], 4);

  instance_data[instance].msg_f.messageData[TOFRN] = instance_data[instance].rangeNumAAnc[tof_idx]; //get the previous range number

  instance_data[instance].rangeNumAAnc[tof_idx] = 0; //clear the entry

  instance_data[instance].rangeNumAnc = frame[POLL_RNUM + fcode_index] ;

  instance_data[instance].msg_f.seqNum = instance_data[instance].frameSN++;

  //set the delayed rx on time (the final message will be sent after this delay)

  dwt_setrxaftertxdelay(instance_data[instance].ancRespRxDelay);  //units are 1.0256us - wait for wait4respTIM before RX on (delay RX)

  instance_data[instance].tagSleepCorrection = 0;

  instance_data[instance].msg_f.messageData[RES_TAG_SLP0] = 0 ;

  instance_data[instance].msg_f.messageData[RES_TAG_SLP1] = 0 ;

  instance_data[instance].msg_f.messageData[FCODE] = RTLS_DEMO_MSG_ANCH_RESP2; //message function code (specifies if message is a poll, response or other...)

  //write the TX data

  dwt_writetxfctrl(frameLength, 0, 1);

  dwt_writetxdata(frameLength, (uint8_t *)  &instance_data[instance].msg_f, 0) ;        // write the frame data

}

/**

 * @brief this function prepares and writes the anchor to tag response frame into the TX buffer

 * it is called after anchor receives a Poll from a tag

 */

void ancprepareresponse(uint16_t sourceAddress, uint8_t srcAddr_index, uint8_t fcode_index, uint8_t *frame, uint32_t uTimeStamp){

  uint16_t frameLength = 0;

  uint8_t tof_idx = (sourceAddress) & 0x7 ;

  int instance = 0;

  instance_data[instance].psduLength = frameLength = ANCH_RESPONSE_MSG_LEN + FRAME_CRTL_AND_ADDRESS_S + FRAME_CRC;

  memcpy(&instance_data[instance].msg_f.destAddr[0], &frame[srcAddr_index], ADDR_BYTE_SIZE_S); //remember who to send the reply to (set destination address)

  instance_data[instance].msg_f.sourceAddr[0] = instance_data[instance].eui64[0];

  instance_data[instance].msg_f.sourceAddr[1] = instance_data[instance].eui64[1];

  // Write calculated TOF into response message (get the previous ToF+range number from that tag)

  memcpy(&(instance_data[instance].msg_f.messageData[TOFR]), &instance_data[instance].tof[tof_idx], 4);

  instance_data[instance].msg_f.messageData[TOFRN] = instance_data[instance].rangeNumA[tof_idx]; //get the previous range number

  instance_data[instance].rangeNumA[tof_idx] = 0; //clear after copy above...

  instance_data[instance].rangeNum = frame[POLL_RNUM+fcode_index] ;

  instance_data[instance].msg_f.seqNum = instance_data[instance].frameSN++;

  //we have our range - update the own mask entry...

  if(instance_data[instance].tof[tof_idx] != INVALID_TOF) { //check the last ToF entry is valid and copy into the current array

    instance_data[instance].rxResponseMask = (0x1 << instance_data[instance].shortAdd_idx);

    instance_data[instance].tofArray[instance_data[instance].shortAdd_idx] = instance_data[instance].tof[tof_idx];

  }

  else        {  //reset response mask

    instance_data[instance].tofArray[instance_data[instance].shortAdd_idx] = INVALID_TOF ;

    instance_data[instance].rxResponseMask = 0;        //reset the mask of received responses when rx poll

  }

  //set the delayed rx on time (the final message will be sent after this delay)