/drivers/DW1000/v1/deca_instance_common.c - Annotate - AMiRo-OS - Research for Cognitive Interaction

amiro-lld / drivers / DW1000 / v1 / deca_instance_common.c @ 9466e34d

History | View | Annotate | Download (71.11 KB)

-a601a5
+/*! ----------------------------------------------------------------------------
  *  @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>
-e34d
+#include <module.h>
-a601a5
+#include <string.h>
 #include <math.h>
-e34d
+#include <deca_instance.h>
-f54213
+            Cung Sang
 /*! @brief Software defined configuration settings for RTLS applications */
 /*! Configuration for DecaRangeRTLS TREK Modes (4 default use cases selected by the switch S1 [2,3] on EVB1000, indexed 0 to 3 )*/
 instanceConfig_t chConfig[4] ={
                                //mode 1 - S1: 2 off, 3 off
+                               {
                                 .channelNumber = 2,             // channel
                                 .preambleCode = 4,              // preambleCode
                                 .pulseRepFreq = DWT_PRF_16M,    // prf
                                 .dataRate = DWT_BR_110K,        // datarate
                                 .preambleLen = DWT_PLEN_1024,   // preambleLength
                                 .pacSize = DWT_PAC32,           // pacSize
                                 .nsSFD = 1,                     // non-standard SFD
                                 .sfdTO = (1025 + 64 - 32)       // SFD timeout
                                },
                                //mode 2 - S1: 2 on, 3 off
+                               {
                                 .channelNumber = 2,            // channel
                                 .preambleCode = 4,             // preambleCode
                                 .pulseRepFreq = DWT_PRF_16M,   // prf
                                 .dataRate = DWT_BR_6M8,        // datarate
                                 .preambleLen = DWT_PLEN_128,   // preambleLength
                                 .pacSize = DWT_PAC8,           // pacSize
                                 .nsSFD = 0,                    // non-standard SFD
                                 .sfdTO = (129 + 8 - 8)        // SFD timeout
                                },
                                //mode 3 - S1: 2 off, 3 on
+                               {
                                 .channelNumber = 5,             // channel
                                 .preambleCode = 3,              // preambleCode
                                 .pulseRepFreq = DWT_PRF_16M,    // prf
                                 .dataRate = DWT_BR_110K,        // datarate
                                 .preambleLen = DWT_PLEN_1024,   // preambleLength
                                 .pacSize = DWT_PAC32,           // pacSize
                                 .nsSFD = 1,                     // non-standard SFD
                                 .sfdTO = (1025 + 64 - 32)       // SFD timeout
                                },
                                //mode 4 - S1: 2 on, 3 on
+                               {
                                 .channelNumber = 5,            // channel
                                 .preambleCode = 3,             // preambleCode
                                 .pulseRepFreq = DWT_PRF_16M,   // prf
                                 .dataRate = DWT_BR_6M8,        // datarate
                                 .preambleLen = DWT_PLEN_128,   // preambleLength
                                 .pacSize = DWT_PAC8,           // pacSize
                                 .nsSFD = 0,                    // non-standard SFD
                                 .sfdTO = (129 + 8 - 8)        // SFD timeout
+                               }
 };
-a601a5
+            Cung Sang
-f54213
+/*! Slot and Superframe Configuration for DecaRangeRTLS TREK Modes (4 default use cases selected by the switch S1 [2,3] on EVB1000, indexed 0 to 3 )*/
 sfConfig_t sfConfig[4] ={
                          //mode 1 - S1: 2 off, 3 off
+                         {
                           .slotPeriod = (28), //slot duration in milliseconds (NOTE: the ranging exchange must be able to complete in this time
                           //e.g. tag sends a poll, 4 anchors send responses and tag sends the final + processing time
                           .numSlots = (10),        //number of slots in the superframe (8 tag slots and 2 used for anchor to anchor ranging),
                           .sfPeriod = (10*28),  //in ms => 280ms frame means 3.57 Hz location rate
                           .pollSleepDly = (10*28), //tag period in ms (sleep time + ranging time)
                           .replyDly = (25000) //poll to final delay in microseconds (needs to be adjusted according to lengths of ranging frames)
                          },
 #if (DISCOVERY == 1)
                          //mode 2 - S1: 2 on, 3 off
+                         {
                           .slotPeriod = (10), //slot duration in milliseconds (NOTE: the ranging exchange must be able to complete in this time
                           //e.g. tag sends a poll, 4 anchors send responses and tag sends the final + processing time
                           .numSlots = (100),        //number of slots in the superframe (98 tag slots and 2 used for anchor to anchor ranging),
                           .sfPeriod = (10*100),  //in ms => 1000 ms frame means 1 Hz location rate
                           .pollSleepDly = (10*100), //tag period in ms (sleep time + ranging time)
                           .replyDly = (2500) //poll to final delay in microseconds (needs to be adjusted according to lengths of ranging frames)
                          },
 #else
                          //mode 2 - S1: 2 on, 3 off
+                         {
                           .slotPeriod = (10), //slot duration in milliseconds (NOTE: the ranging exchange must be able to complete in this time
                           //e.g. tag sends a poll, 4 anchors send responses and tag sends the final + processing time
                           .numSlots = (10),        //number of slots in the superframe (8 tag slots and 2 used for anchor to anchor ranging),
                           .sfPeriod = (10*10),  //in ms => 100 ms frame means 10 Hz location rate
                           .pollSleepDly = (10*10), //tag period in ms (sleep time + ranging time)
                           .replyDly = (2500) //poll to final delay in microseconds (needs to be adjusted according to lengths of ranging frames)
                          },
 #endif
                          //mode 3 - S1: 2 off, 3 on
+                         {
                           .slotPeriod = (28), //slot duration in milliseconds (NOTE: the ranging exchange must be able to complete in this time
                           //e.g. tag sends a poll, 4 anchors send responses and tag sends the final + processing time
                           .numSlots = (10),        //number of slots in the superframe (8 tag slots and 2 used for anchor to anchor ranging),
                           .sfPeriod = (10*28),  //in ms => 280ms frame means 3.57 Hz location rate
                           .pollSleepDly = (10*28), //tag period in ms (sleep time + ranging time)
                           .replyDly = (20000) //poll to final delay in microseconds (needs to be adjusted according to lengths of ranging frames)
                          },
                          //mode 4 - S1: 2 on, 3 on
+                         {
                           .slotPeriod = (10), //slot duration in milliseconds (NOTE: the ranging exchange must be able to complete in this time
                           //e.g. tag sends a poll, 4 anchors send responses and tag sends the final + processing time
                           .numSlots = (10),        //number of slots in the superframe (8 tag slots and 2 used for anchor to anchor ranging),
                           .sfPeriod = (10*10),  //in ms => 100 ms frame means 10 Hz location rate
                           .pollSleepDly = (10*10), //tag period in ms (sleep time + ranging time)
                           .replyDly = (2500) //poll to final delay in microseconds (needs to be adjusted according to lengths of ranging frames)
+                         }
 };
-a601a5
+            Cung Sang
 // -------------------------------------------------------------------------------------------------------------------
 //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
+    {
 x0,   //0
+            {
 x0, //0
 x0 //0
+            }
     },
     //Channel 1
+    {
 xc9,   //PG_DELAY
+            {
 x15355575, //16M prf power
 x07274767 //64M prf power
+            }
     },
     //Channel 2
+    {
 xc2,   //PG_DELAY
+            {
 x15355575, //16M prf power
 x07274767 //64M prf power
+            }
     },
     //Channel 3
+    {
 xc5,   //PG_DELAY
+            {
 x0f2f4f6f, //16M prf power
 x2b4b6b8b //64M prf power
+            }
     },
     //Channel 4
+    {
 x95,   //PG_DELAY
+            {
 x1f1f3f5f, //16M prf power
 x3a5a7a9a //64M prf power
+            }
     },
     //Channel 5
+    {
 xc0,   //PG_DELAY
+            {
 x0E082848, //16M prf power
 x25456585 //64M prf power
+            }
     },
     //Channel 6 ----- this is just a place holder so the next array element is channel 7
+    {
 x0,   //0
+            {
 x0, //0
 x0 //0
+            }
     },
     //Channel 7
+    {
 x93,   //PG_DELAY
+            {
 x32527292, //16M prf power
 x5171B1d1 //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] = {
-dead12
+        (uint16_t) (((double)DWT_PRF_16M_RFDLY/ 2.0) * 1e-9 / DWT_TIME_UNITS),//PRF 16
         (uint16_t) (((double)DWT_PRF_64M_RFDLY/ 2.0) * 1e-9 / DWT_TIME_UNITS)
-a601a5
+};
 //these are default TREK Tag/Anchor antenna delays
 const uint16_t rfDelaysTREK[2] = {
-dead12
+        (uint16_t) (((double)514.83f/ 2.0) * 1e-9 / DWT_TIME_UNITS),//channel 2
         (uint16_t) (((double)514.65f/ 2.0) * 1e-9 / DWT_TIME_UNITS) //channel 5
-a601a5
+};
 // -------------------------------------------------------------------------------------------------------------------
 //      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] ;
 instance_data_t instance_data[NUM_INST] ;
-dead12
+typedef struct __attribute__((packed))
-a601a5
+            Cung Sang
     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;
-dead12
+  dtime = (long double)((microsecu / (double) DWT_TIME_UNITS) / 1e6);
-a601a5
+            Cung Sang
   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 ;
-dead12
+  int64_t tofi ;
-a601a5
+            Cung Sang
   // 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)
-dead12
+  tof = convertdevicetimetosec((int32_t)tofi) ;          //this is divided by 4 to get single time of flight
-a601a5
+  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
-dead12
+  instance_data[0].mode =  (INST_MODE)inst_mode;                   // set the role
-a601a5
+            Cung Sang
 int instancegetrole(void){
-dead12
+  return (int)instance_data[0].mode;
-a601a5
+            Cung Sang
 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;
+}
-dead12
+uint32_t instancenewrangetim(void){
-a601a5
+  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
-f54213
+int instance_init(DW1000Driver* drv){
-a601a5
+  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
-dead12
+  result = dwt_initialise(DWT_LOADUCODE, drv);
-a601a5
+            Cung Sang
   //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
 //
-c48d
+void instance_config(instanceConfig_t *config, sfConfig_t *sfConfig, DW1000Driver* drv){
-a601a5
+  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
-c48d
+    setHighSpeed_SPI(FALSE, drv);
-a601a5
+    dwt_loadopsettabfromotp(0);
     //increase SPI to max
-c48d
+    setHighSpeed_SPI(TRUE, drv);
-a601a5
+            Cung Sang
   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();
-dead12
+  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);
-a601a5
+            Cung Sang
-dead12
+  instance_data[instance].stopTimer = 0;
-a601a5
+            Cung Sang
-dead12
+  dw_event.rxLength = instance_data[instance].psduLength;
   dw_event.type =  0;
   dw_event.type_pend =  0;
   dw_event.type_save = DWT_SIG_TX_DONE;
-a601a5
+            Cung Sang
-dead12
+  memcpy((uint8_t *)&dw_event.msgu.frame[0], (uint8_t *)&instance_data[instance].msg_f, instance_data[instance].psduLength);
-a601a5
+            Cung Sang
-dead12
+  instance_putevent(dw_event, DWT_SIG_TX_DONE);
-a601a5
+            Cung Sang
-dead12
+  instance_data[instance].txMsgCount++;
-a601a5
+            Cung Sang
   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
-dead12
+      dwt_setrxtimeout((uint16_t)(instance_data[instance].fwtoTime_sy * instance_data[instance].responseTO)); //reconfigure the timeout
-a601a5
+      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)
-dead12
+  dwt_setrxaftertxdelay((uint32_t)instance_data[instance].ancRespRxDelay);  //units are 1.0256us - wait for wait4respTIM before RX on (delay RX)