Alejandro Ungria Hirte / GA-Test_copy

Dependencies:   mbed-dev

Files at this revision

API Documentation at this revision

Comitter:
aungriah
Date:
Wed Dec 06 21:35:45 2017 +0000
Commit message:
test

Changed in this revision

DecaWave/DecaWave.cpp Show annotated file Show diff for this revision Revisions of this file
DecaWave/DecaWave.h Show annotated file Show diff for this revision Revisions of this file
DecaWave/SMConfig.cpp Show annotated file Show diff for this revision Revisions of this file
DecaWave/SMConfig.h Show annotated file Show diff for this revision Revisions of this file
PC/PC.cpp Show annotated file Show diff for this revision Revisions of this file
PC/PC.h Show annotated file Show diff for this revision Revisions of this file
Watchdog/Watchdog.cpp Show annotated file Show diff for this revision Revisions of this file
Watchdog/Watchdog.h Show annotated file Show diff for this revision Revisions of this file
decadriver/deca_device.c Show annotated file Show diff for this revision Revisions of this file
decadriver/deca_device_api.h Show annotated file Show diff for this revision Revisions of this file
decadriver/deca_param_types.h Show annotated file Show diff for this revision Revisions of this file
decadriver/deca_params_init.c Show annotated file Show diff for this revision Revisions of this file
decadriver/deca_regs.h Show annotated file Show diff for this revision Revisions of this file
decadriver/deca_types.h Show annotated file Show diff for this revision Revisions of this file
decadriver/deca_version.h Show annotated file Show diff for this revision Revisions of this file
globals.h Show annotated file Show diff for this revision Revisions of this file
main_minimal.cpp Show annotated file Show diff for this revision Revisions of this file
mbed-dev.lib Show annotated file Show diff for this revision Revisions of this file
nodes/frames.h Show annotated file Show diff for this revision Revisions of this file
nodes/nodes.cpp Show annotated file Show diff for this revision Revisions of this file
nodes/nodes.h Show annotated file Show diff for this revision Revisions of this file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DecaWave/DecaWave.cpp	Wed Dec 06 21:35:45 2017 +0000
@@ -0,0 +1,1100 @@
+/*
+ * DecaWave.cpp
+ *
+ *  Created on: 04.11.2015
+ *      Author: kauf
+ */
+
+#include "DecaWave.h"
+//#include "states.h"
+
+extern "C" {
+// TODO: create dedicated struct instead of void pointer
+#pragma Otime
+int writetospi(uint16 headerLength, const uint8 *headerBuffer,
+    uint32 bodyLength, const uint8 *bodyBuffer) {
+  uint32_t i = 0;
+  decaIrqStatus_t stat;
+
+  stat = decamutexon();
+
+  // chip select
+  decaWaveCs = 0;                 // set Cable Select pin low to start transmission
+  for (i = 0; i < headerLength; i++) {
+    decaWaveSpi.write(headerBuffer[i]);
+  }
+  for (i = 0; i < bodyLength; i++) {
+    decaWaveSpi.write(bodyBuffer[i]);
+  }
+  decaWaveCs = 1;
+
+  decamutexoff(stat);
+
+  return 0;
+}
+
+#pragma Otime
+int readfromspi(uint16 headerLength, const uint8 *headerBuffer,
+    uint32 readLength, uint8 *readBuffer) {
+  uint32_t i = 0;
+
+  decaIrqStatus_t stat;
+
+  stat = decamutexon();
+
+  /* Wait for SPIx Tx buffer empty */
+  //while (port_SPIx_busy_sending());
+  decaWaveCs = 0;
+  for (i = 0; i < headerLength; i++) {
+    decaWaveSpi.write(headerBuffer[i]);
+  }
+  for (i = 0; i < readLength; i++) {
+    readBuffer[i] = decaWaveSpi.write(0x00); //port_SPIx_receive_data(); //this clears RXNE bit
+  }
+  decaWaveCs = 1;
+
+  decamutexoff(stat);
+
+  return 0;
+}
+
+//#pragma Otime
+decaIrqStatus_t decamutexon() {
+  decaWaveIrq.disable_irq();
+  return 0;
+}
+
+//#pragma Otime
+void decamutexoff(decaIrqStatus_t s) {
+  decaWaveIrq.enable_irq();
+}
+
+void deca_sleep(unsigned int time_ms) {
+  wait_ms(time_ms);
+}
+
+}
+
+DecaWave::DecaWave()
+  {
+
+  decaWaveCs = 1;               // deselect chip
+ // decaWaveRst = 1;               // make sure that reset pin is high !!!!!!!!!!TODO (haven't the pin definition of the reset pin available
+  decaWaveIrq.enable_irq();
+  decaWaveSpi.format(8, 0); // Setup the spi for standard 8 bit data and SPI-Mode 0 (GPIO5, GPIO6 open circuit or ground on DW1000)
+  decaWaveSpi.frequency(MIN_SPI_FREQ);   // during init phase, only clock at 1 MHz
+
+  decaWaveIrq.rise(dwt_isr); // attach interrupt handler to rising edge of interrupt pin from DW1000
+
+  hardreset();
+  dwt_softreset();
+
+  _sequenceNumber = 0;
+}
+
+DecaWave::~DecaWave() {
+  // TODO Auto-generated destructor stub
+}
+
+void DecaWave::setup(dwt_config_t configdw, dwt_txconfig_t configdw_tx,
+    uint32_t delay, void (*txcallback)(const dwt_cb_data_t *),
+    void (*rxcallback)(const dwt_cb_data_t *)) {
+
+  _deca_config = configdw;
+  _antennadelay = delay;
+  // disable interrupts
+  decamutexon();
+
+  // inittestapplication
+  // setup slow spi
+  decaWaveSpi.frequency(MIN_SPI_FREQ);   // during init phase, only clock at 1 MHz
+
+  // instance init
+  dwt_initialise(DWT_LOADUCODE);
+  dwt_geteui(_euid);
+
+  // setinterrupt, callbacks
+  dwt_setinterrupt(DWT_INT_TFRS | DWT_INT_RFCG, 1);
+  dwt_setcallbacks(txcallback, rxcallback, NULL, NULL);
+
+  // inst config
+  dwt_configure(&configdw);
+
+  //Configure TX power
+  uint32_t power = configdw_tx.power;
+  _configTX.PGdly = configdw_tx.PGdly;
+
+  //if smart power is used then the value as read from OTP is used directly
+  //if smart power is used the user needs to make sure to transmit only one frame per 1ms or TX spectrum power will be violated
+  if (configdw.dataRate == DWT_BR_6M8) {
+    _configTX.power = power;
+    dwt_setsmarttxpower(1);
+  } else { //if the smart power is not used, then the low byte value (repeated) is used for the whole TX power register
+    uint8 pow = power & 0xFF;
+    _configTX.power = (pow | (pow << 8) | (pow << 16) | (pow << 24));
+    dwt_setsmarttxpower(0);
+  }
+
+  //configure the tx spectrum parameters (power and PG delay)
+  dwt_configuretxrf(&_configTX);
+  
+  _antennadelay += getAntennaDelayOffset(dwt_getpartid());
+  dwt_setrxantennadelay(_antennadelay);
+  dwt_settxantennadelay(_antennadelay);
+
+  if (configdw.txPreambLength == DWT_PLEN_64) { //if preamble length is 64
+    decaWaveSpi.frequency(MIN_SPI_FREQ); //reduce SPI to < 3MHz
+    dwt_loadopsettabfromotp(0);
+    decaWaveSpi.frequency(MAX_SPI_FREQ);  //increase SPI to max
+  }
+  wait_ms(10);
+
+  autoreenable();
+
+  // enable event counter & clear
+  dwt_configeventcounters(1);
+
+  decaWaveSpi.frequency(MAX_SPI_FREQ);
+
+  // enable interrupts
+  decamutexoff(0);
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * Function: autoreenable()
+ *
+ * Set the auto-reenable flag
+ *
+ * arguments:
+ * returns:
+ */
+void DecaWave::autoreenable() {
+  uint8 byte = 0;
+  dwt_readfromdevice(SYS_CFG_ID, 3, 1, &byte);
+
+  byte |= (SYS_CFG_RXAUTR >> 24);
+
+  dwt_writetodevice(SYS_CFG_ID, 3, 1, &byte) ;
+}
+
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * Function: sendFrame()
+ *
+ * Send a pre-composed frame:
+ * - Write uint8_t array of with length bytes to register
+ * - set length in register
+ * - if it is scheduled to be sent at a given timestamp, write 40bit delay in timestamps units
+ * - send TX command
+ * - if the receiver turns on after a delay, write this delay in us
+ * - enable (potentially delay) receiver
+ *
+ * arguments: pointer to message (uint8 array) with specified length, dtime transmission time (truncated 32bit deca time), delay (us) until RX turn on
+ * returns DWT_SUCCESS if process was successful, or DWT_ERROR if TX failed.
+ */
+#pragma Otime
+int8_t DecaWave::sendFrame(uint8_t* message, uint16_t length, uint32_t dtime,
+    uint32_t delay) {
+  length += 2;      // include 2 byte crc in the frame length
+
+  dwt_writetxdata(length, message, 0);
+
+  dwt_writetxfctrl(length, 0, 1);
+
+  if (dtime > 0) {
+    dwt_setdelayedtrxtime(dtime);
+  }
+
+  if (_state == DW_RECEIVE) {
+    uint8_t temp = (uint8)SYS_CTRL_TRXOFF ; // This assumes the bit is in the lowest byte
+    dwt_writetodevice(SYS_CTRL_ID,0,1,&temp) ; // Disable the radio
+  }
+
+  uint8_t mode = (dtime>0)*DWT_START_TX_DELAYED + 0*DWT_RESPONSE_EXPECTED;
+  _state = DW_TRANSMIT;
+  return dwt_starttx(mode);
+}
+
+uint32_t DecaWave::getStatus() {
+  uint32_t status;
+//  status = dwt_read32bitreg(SYS_STATUS_ID, this);
+  dwt_readfromdevice(SYS_STATUS_ID, 0x0, 4, (uint8_t*) &status);
+
+  uint32_t temp = SYS_STATUS_MASK_32;
+  dwt_writetodevice(SYS_STATUS_ID, 0x00, 4, (uint8_t*)&temp);
+  return status;
+}
+
+uint16_t DecaWave::computeFrameLength_us() {
+
+  //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;
+
+  msgdatalen = 16;//sizeof(FrameHeader_t); //TODO add size of header!
+  
+
+  x = (int) ceil(msgdatalen * 8 / 330.0f);
+
+  msgdatalen = msgdatalen * 8 + x * 48;
+
+  //assume PHR length is 172308us for 110k and 21539us for 850k/6.81M
+  if (_deca_config.dataRate == DWT_BR_110K) {
+    msgdatalen *= 8205.13f;
+    msgdatalen += 172308;
+
+  } else if (_deca_config.dataRate == DWT_BR_850K) {
+    msgdatalen *= 1025.64f;
+    msgdatalen += 21539;
+  } else {
+    msgdatalen *= 128.21f;
+    msgdatalen += 21539;
+  }
+
+  //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[_deca_config.dataRate];
+
+  switch (_deca_config.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 (_deca_config.prf == DWT_PRF_16M) {
+    preamblelen = (sfdlen + preamblelen) * 0.99359f;
+  } else {
+    preamblelen = (sfdlen + preamblelen) * 1.01763f;
+  }
+
+  //set the frame wait timeout time - total time the frame takes in symbols
+  return uint16_t(
+      16 + (int) ((preamblelen + (msgdatalen / 1000.0f)) / 1.0256f));
+
+}
+
+float DecaWave::getRXLevel(dwt_rxdiag_t *diagnostics) {
+  float A; // 115.72 if PRF 16 MHz, 121.74 if PRF 64 MHz
+  if (_deca_config.prf == DWT_PRF_16M) {
+    A = 115.72f;
+  } else {
+    A = 121.74f;
+  }
+  // RXLevel (dBm) is calculated as P = 10 * log10( C * 2^17 ) / N^2 ) - A
+  // use magic number: 10*log10(2^17)=51.175
+  return 10 * log10(float(diagnostics->maxGrowthCIR)) + 51.175f - 20 * log10(float(diagnostics->rxPreamCount)) - A; // user manual 4.7.2 p.45
+}
+
+float DecaWave::getFPLevel() {
+
+  uint32_t frameInfo = dwt_read32bitreg(RX_FINFO_ID);
+
+  uint64_t frameQuality;
+  dwt_readfromdevice(RX_FQUAL_ID, 0, 8, (uint8_t*) &frameQuality);
+
+  uint16_t F1;
+  dwt_readfromdevice(RX_TIME_ID, 7, 2, (uint8_t*) &F1);
+  uint16_t F2 = (frameQuality >> 16) & 0xFFFF;
+  uint16_t F3 = (frameQuality >> 32) & 0xFFFF;
+
+  float A; // 115.72 if PRF 16 MHz, 121.74 if PRF 64 MHz
+  if (_deca_config.prf == DWT_PRF_16M) {
+    A = 115.72f;
+  } else {
+    A = 121.74f;
+  }
+  uint32_t N = (frameInfo >> 20) & 0x7FF;
+
+  // First Path Power Level (dBm) is calculated as P = 10 * log10( F1^2+F2^2+F3^2) / N^2 ) - A
+
+  return 10
+      * log10(
+          float(F1) * float(F1) + float(F2) * float(F2)
+              + float(F3) * float(F3)) - 20 * log10(float(N)) - A; // user manual 4.7.1 p.44
+}
+
+void DecaWave::reset() {
+  decaWaveSpi.frequency(MIN_SPI_FREQ);
+  dwt_softreset();
+  decaWaveSpi.frequency(MAX_SPI_FREQ);
+}
+
+void DecaWave::hardreset() {//TODO: Check where the reset-pin is and add it!
+  //decaWaveRst = 0;
+  wait_ms(10);
+ // decaWaveRst = 1;
+}
+
+int8_t DecaWave::turnonrx() {
+  int8_t result = dwt_rxenable(0);
+  if (result != DWT_ERROR) {
+    _state = DW_RECEIVE;
+  } else {
+    _state = DW_IDLE;
+  }
+  return result;
+}
+
+void DecaWave::turnoffrx() {
+  uint8_t temp = (uint8)SYS_CTRL_TRXOFF ; // This assumes the bit is in the lowest byte
+  dwt_writetodevice(SYS_CTRL_ID,0,1,&temp) ; // Disable the radio
+  _state = DW_IDLE;
+}
+
+uint8_t DecaWave::getNextSequenceNumber() {
+  return ++_sequenceNumber;
+}
+
+uint16_t DecaWave::getAntennaDelay() {
+  return _antennadelay;
+}
+
+
+uint8_t DecaWave::getCHAN() {
+  return _deca_config.chan;
+}
+
+uint8_t DecaWave::getPRF() {
+  return _deca_config.prf;
+}
+
+#define NUM_16M_OFFSET  (37)
+#define NUM_16M_OFFSETWB  (68)
+#define NUM_64M_OFFSET  (26)
+#define NUM_64M_OFFSETWB  (59)
+
+const uint8 chan_idxnb[NUM_CH_SUPPORTED] = {0, 0, 1, 2, 0, 3, 0, 0}; // Only channels 1,2,3 and 5 are in the narrow band tables
+const uint8 chan_idxwb[NUM_CH_SUPPORTED] = {0, 0, 0, 0, 0, 0, 0, 1}; // Only channels 4 and 7 are in in the wide band tables
+
+//---------------------------------------------------------------------------------------------------------------------------
+// Range Bias Correction TABLES of range values in integer units of 25 CM, for 8-bit unsigned storage, MUST END IN 255 !!!!!!
+//---------------------------------------------------------------------------------------------------------------------------
+
+// offsets to nearest centimetre for index 0, all rest are +1 cm per value
+
+#define CM_OFFSET_16M_NB    (-23)   // For normal band channels at 16 MHz PRF
+#define CM_OFFSET_16M_WB    (-28)   // For wider  band channels at 16 MHz PRF
+#define CM_OFFSET_64M_NB    (-17)   // For normal band channels at 64 MHz PRF
+#define CM_OFFSET_64M_WB    (-30)   // For wider  band channels at 64 MHz PRF
+
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * Function: getAntennaDelayOffset()
+ *
+ * Get the Offset for the antenna delay
+ *
+ * arguments:
+ * returns:
+ */
+int16_t DecaWave::getAntennaDelayOffset(uint32_t board_id) {
+    switch(board_id){
+       // Calibrated on 23.03. outside at 15C, windy. 8m side length
+        case 268436898: return -91;        
+        case 268445167: return -101;
+        case 268445155: return -106;
+        case 268445158: return -107;
+        case 268436604: return -106;
+        case 268437112: return -102;
+        case 268445165: return -100;
+        case 268444882: return -102;
+        case 268437817: return -87;
+        case 268445163: return -104;
+        case 268436897: return -99;
+        case 268437656: return -106;
+        case 268445154: return -102;
+        case 268436603: return -98;
+        case 268444886: return -112;
+        case 268437847: return -119;
+        case 268437825: return -111;
+        
+        default: return -106;
+    }
+}
+
+//---------------------------------------------------------------------------------------------------------------------------
+// range25cm16PRFnb: Range Bias Correction table for narrow band channels at 16 MHz PRF, NB: !!!! each MUST END IN 255 !!!!
+//---------------------------------------------------------------------------------------------------------------------------
+
+const uint8 range25cm16PRFnb[4][NUM_16M_OFFSET] =
+{
+    // Ch 1 - range25cm16PRFnb
+    {
+           1,
+           3,
+           4,
+           5,
+           7,
+           9,
+          11,
+          12,
+          13,
+          15,
+          18,
+          20,
+          23,
+          25,
+          28,
+          30,
+          33,
+          36,
+          40,
+          43,
+          47,
+          50,
+          54,
+          58,
+          63,
+          66,
+          71,
+          76,
+          82,
+          89,
+          98,
+         109,
+         127,
+         155,
+         222,
+         255,
+         255
+    },
+
+    // Ch 2 - range25cm16PRFnb
+    {
+           1,
+           2,
+           4,
+           5,
+           6,
+           8,
+           9,
+          10,
+          12,
+          13,
+          15,
+          18,
+          20,
+          22,
+          24,
+          27,
+          29,
+          32,
+          35,
+          38,
+          41,
+          44,
+          47,
+          51,
+          55,
+          58,
+          62,
+          66,
+          71,
+          78,
+          85,
+          96,
+         111,
+         135,
+         194,
+         240,
+         255
+    },
+
+    // Ch 3 - range25cm16PRFnb
+    {
+           1,
+           2,
+           3,
+           4,
+           5,
+           7,
+           8,
+           9,
+          10,
+          12,
+          14,
+          16,
+          18,
+          20,
+          22,
+          24,
+          26,
+          28,
+          31,
+          33,
+          36,
+          39,
+          42,
+          45,
+          49,
+          52,
+          55,
+          59,
+          63,
+          69,
+          76,
+          85,
+          98,
+         120,
+         173,
+         213,
+         255
+    },
+
+    // Ch 5 - range25cm16PRFnb
+    {
+           1,
+           1,
+           2,
+           3,
+           4,
+           5,
+           6,
+           6,
+           7,
+           8,
+           9,
+          11,
+          12,
+          14,
+          15,
+          16,
+          18,
+          20,
+          21,
+          23,
+          25,
+          27,
+          29,
+          31,
+          34,
+          36,
+          38,
+          41,
+          44,
+          48,
+          53,
+          59,
+          68,
+          83,
+         120,
+         148,
+         255
+    }
+}; // end range25cm16PRFnb
+
+
+//---------------------------------------------------------------------------------------------------------------------------
+// range25cm16PRFwb: Range Bias Correction table for wide band channels at 16 MHz PRF, NB: !!!! each MUST END IN 255 !!!!
+//---------------------------------------------------------------------------------------------------------------------------
+
+const uint8 range25cm16PRFwb[2][NUM_16M_OFFSETWB] =
+{
+    // Ch 4 - range25cm16PRFwb
+    {
+           7,
+           7,
+           8,
+           9,
+           9,
+          10,
+          11,
+          11,
+          12,
+          13,
+          14,
+          15,
+          16,
+          17,
+          18,
+          19,
+          20,
+          21,
+          22,
+          23,
+          24,
+          26,
+          27,
+          28,
+          30,
+          31,
+          32,
+          34,
+          36,
+          38,
+          40,
+          42,
+          44,
+          46,
+          48,
+          50,
+          52,
+          55,
+          57,
+          59,
+          61,
+          63,
+          66,
+          68,
+          71,
+          74,
+          78,
+          81,
+          85,
+          89,
+          94,
+          99,
+         104,
+         110,
+         116,
+         123,
+         130,
+         139,
+         150,
+         164,
+         182,
+         207,
+         238,
+         255,
+         255,
+         255,
+         255,
+         255
+    },
+
+    // Ch 7 - range25cm16PRFwb
+    {
+           4,
+           5,
+           5,
+           5,
+           6,
+           6,
+           7,
+           7,
+           7,
+           8,
+           9,
+           9,
+          10,
+          10,
+          11,
+          11,
+          12,
+          13,
+          13,
+          14,
+          15,
+          16,
+          17,
+          17,
+          18,
+          19,
+          20,
+          21,
+          22,
+          23,
+          25,
+          26,
+          27,
+          29,
+          30,
+          31,
+          32,
+          34,
+          35,
+          36,
+          38,
+          39,
+          40,
+          42,
+          44,
+          46,
+          48,
+          50,
+          52,
+          55,
+          58,
+          61,
+          64,
+          68,
+          72,
+          75,
+          80,
+          85,
+          92,
+         101,
+         112,
+         127,
+         147,
+         168,
+         182,
+         194,
+         205,
+         255
+    }
+}; // end range25cm16PRFwb
+
+//---------------------------------------------------------------------------------------------------------------------------
+// range25cm64PRFnb: Range Bias Correction table for narrow band channels at 64 MHz PRF, NB: !!!! each MUST END IN 255 !!!!
+//---------------------------------------------------------------------------------------------------------------------------
+
+const uint8 range25cm64PRFnb[4][NUM_64M_OFFSET] =
+{
+    // Ch 1 - range25cm64PRFnb
+    {
+           1,
+           2,
+           2,
+           3,
+           4,
+           5,
+           7,
+          10,
+          13,
+          16,
+          19,
+          22,
+          24,
+          27,
+          30,
+          32,
+          35,
+          38,
+          43,
+          48,
+          56,
+          78,
+         101,
+         120,
+         157,
+         255
+    },
+
+    // Ch 2 - range25cm64PRFnb
+    {
+           1,
+           2,
+           2,
+           3,
+           4,
+           4,
+           6,
+           9,
+          12,
+          14,
+          17,
+          19,
+          21,
+          24,
+          26,
+          28,
+          31,
+          33,
+          37,
+          42,
+          49,
+          68,
+          89,
+         105,
+         138,
+         255
+    },
+
+    // Ch 3 - range25cm64PRFnb
+    {
+           1,
+           1,
+           2,
+           3,
+           3,
+           4,
+           5,
+           8,
+          10,
+          13,
+          15,
+          17,
+          19,
+          21,
+          23,
+          25,
+          27,
+          30,
+          33,
+          37,
+          44,
+          60,
+          79,
+          93,
+         122,
+         255
+    },
+
+    // Ch 5 - range25cm64PRFnb
+    {
+           1,
+           1,
+           1,
+           2,
+           2,
+           3,
+           4,
+           6,
+           7,
+           9,
+          10,
+          12,
+          13,
+          15,
+          16,
+          17,
+          19,
+          21,
+          23,
+          26,
+          30,
+          42,
+          55,
+          65,
+          85,
+         255
+    }
+}; // end range25cm64PRFnb
+
+//---------------------------------------------------------------------------------------------------------------------------
+// range25cm64PRFwb: Range Bias Correction table for wide band channels at 64 MHz PRF, NB: !!!! each MUST END IN 255 !!!!
+//---------------------------------------------------------------------------------------------------------------------------
+
+const uint8 range25cm64PRFwb[2][NUM_64M_OFFSETWB] =
+{
+    // Ch 4 - range25cm64PRFwb
+    {
+           7,
+           8,
+           8,
+           9,
+           9,
+          10,
+          11,
+          12,
+          13,
+          13,
+          14,
+          15,
+          16,
+          16,
+          17,
+          18,
+          19,
+          19,
+          20,
+          21,
+          22,
+          24,
+          25,
+          27,
+          28,
+          29,
+          30,
+          32,
+          33,
+          34,
+          35,
+          37,
+          39,
+          41,
+          43,
+          45,
+          48,
+          50,
+          53,
+          56,
+          60,
+          64,
+          68,
+          74,
+          81,
+          89,
+          98,
+         109,
+         122,
+         136,
+         146,
+         154,
+         162,
+         178,
+         220,
+         249,
+         255,
+         255,
+         255
+    },
+
+    // Ch 7 - range25cm64PRFwb
+    {
+           4,
+           5,
+           5,
+           5,
+           6,
+           6,
+           7,
+           7,
+           8,
+           8,
+           9,
+           9,
+          10,
+          10,
+          10,
+          11,
+          11,
+          12,
+          13,
+          13,
+          14,
+          15,
+          16,
+          16,
+          17,
+          18,
+          19,
+          19,
+          20,
+          21,
+          22,
+          23,
+          24,
+          25,
+          26,
+          28,
+          29,
+          31,
+          33,
+          35,
+          37,
+          39,
+          42,
+          46,
+          50,
+          54,
+          60,
+          67,
+          75,
+          83,
+          90,
+          95,
+         100,
+         110,
+         135,
+         153,
+         172,
+         192,
+         255
+    }
+}; // end range25cm64PRFwb
+
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_getrangebias()
+ *
+ * @brief This function is used to return the range bias correction need for TWR with DW1000 units.
+ *
+ * input parameters:
+ * @param chan  - specifies the operating channel (e.g. 1, 2, 3, 4, 5, 6 or 7)
+ * @param range - the calculated distance before correction
+ * @param prf  - this is the PRF e.g. DWT_PRF_16M or DWT_PRF_64M
+ *
+ * output parameters
+ *
+ * returns correction needed in meters
+ */
+double dwt_getrangebias(uint8 chan, float range, uint8 prf)
+{
+    // First get the lookup index that corresponds to given range for a particular channel at 16M PRF
+    int i = 0 ;
+    int chanIdx ;
+    int cmoffseti ; // Integer number of CM offset
+
+    double mOffset ; // Final offset result in metres
+
+    // NB: note we may get some small negitive values e.g. up to -50 cm.
+
+    int rangeint25cm = (int) (range * 4.00f) ; // Convert range to integer number of 25cm values.
+
+    if (rangeint25cm > 255) rangeint25cm = 255 ; // Make sure it matches largest value in table (all tables end in 255 !!!!)
+
+    if (prf == DWT_PRF_16M)
+    {
+        switch(chan)
+        {
+            case 4:
+            case 7:
+            {
+                chanIdx = chan_idxwb[chan];
+                while (rangeint25cm > range25cm16PRFwb[chanIdx][i]) i++ ; // Find index in table corresponding to range
+                cmoffseti = i + CM_OFFSET_16M_WB ;                        // Nearest centimetre correction
+            }
+            break;
+            default:
+            {
+                chanIdx = chan_idxnb[chan];
+                while (rangeint25cm > range25cm16PRFnb[chanIdx][i]) i++ ; // Find index in table corresponding to range
+                cmoffseti = i + CM_OFFSET_16M_NB ;                        // Nearest centimetre correction
+            }
+        }//end of switch
+    }
+    else // 64M PRF
+    {
+        switch(chan)
+        {
+            case 4:
+            case 7:
+            {
+                chanIdx = chan_idxwb[chan];
+                while (rangeint25cm > range25cm64PRFwb[chanIdx][i]) i++ ; // Find index in table corresponding to range
+                cmoffseti = i + CM_OFFSET_64M_WB ;                        // Nearest centimetre correction
+            }
+            break;
+            default:
+            {
+                chanIdx = chan_idxnb[chan];
+                while (rangeint25cm > range25cm64PRFnb[chanIdx][i]) i++ ; // Find index in table corresponding to range
+                cmoffseti = i + CM_OFFSET_64M_NB ;                        // Nearest centimetre correction
+            }
+        }//end of switch
+    } // end else
+
+
+    mOffset = (float) cmoffseti ; // Offset result in centimetres
+
+    mOffset *= 0.01 ; // Convert to metres
+
+    return (mOffset) ;
+}
+
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DecaWave/DecaWave.h	Wed Dec 06 21:35:45 2017 +0000
@@ -0,0 +1,86 @@
+/*
+ * DecaWave.h
+ *
+ *  Created on: 04.11.2015
+ *      Author: kauf
+ */
+
+#ifndef _DECAWAVE_H_
+#define _DECAWAVE_H_
+
+#include <stddef.h>
+#include "deca_device_api.h"
+#include "deca_param_types.h"
+#include "deca_regs.h"
+#include "mbed.h"
+#include "PC.h"
+
+#include "globals.h"
+//#include "frames.h"
+
+#ifndef uint64
+#ifndef _DECA_INT64_
+#define _DECA_INT64_
+typedef uint64_t uint64;
+#endif
+#endif
+
+#define TIMEUNITS_TO_US     (1/(128*499.2))             // conversion between the decawave timeunits (ca 15.65ps) to microseconds.
+#define US_TO_TIMEUNITS     (128*499.2)                 // conversion between microseconds to the decawave timeunits (ca 15.65ps).
+#define MASK_40BIT          (0x00FFFFFFFFFF)            // MP counter is 40 bits
+#define MASK_TXDTS          (0x00FFFFFFFE00)            // TX timestamp will snap to 8 ns resolution - mask lower 9 bits.
+
+#define SPEED_OF_LIGHT      (299702547.0f)              // in m/s in air
+#define MIN_SPI_FREQ         1000000
+#define MAX_SPI_FREQ        20000000
+
+enum dw_state { DW_RECEIVE, DW_TRANSMIT, DW_IDLE };
+double dwt_getrangebias(uint8 chan, float range, uint8 prf);
+
+
+class DecaWave {
+public:
+
+  DecaWave();
+
+  virtual ~DecaWave();
+
+  void setup(dwt_config_t config, dwt_txconfig_t configdw_tx, uint32_t delay,
+      void (*txcallback)(const dwt_cb_data_t *),
+      void (*rxcallback)(const dwt_cb_data_t *));
+    
+  int8_t sendFrame(uint8_t* message, uint16_t length, uint32_t dtime, 
+      uint32_t delay);
+  int8_t turnonrx();
+  void turnoffrx();
+
+  uint16_t computeFrameLength_us();
+  float getRXLevel(dwt_rxdiag_t *diagnostics);
+  float getFPLevel();
+
+  uint32_t getStatus();
+  uint32_t readdevid();
+
+  uint8_t   getNextSequenceNumber();
+  uint16_t  getAntennaDelay();
+  uint8_t   getCHAN();
+  uint8_t   getPRF();
+
+
+protected:
+
+  void reset();
+  void hardreset();
+  void autoreenable();
+  int16_t getAntennaDelayOffset(uint32_t board_id);
+
+  uint8_t _sequenceNumber;
+  dwt_config_t _deca_config;
+  uint8_t _euid[8];
+  uint16_t _antennadelay;
+  dwt_txconfig_t _configTX;
+  dw_state _state;
+};
+
+#endif /* _DECAWAVE_H_ */
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DecaWave/SMConfig.cpp	Wed Dec 06 21:35:45 2017 +0000
@@ -0,0 +1,238 @@
+#include "SMConfig.h"
+
+void SMsetconfig(uint8_t dr_mode, dwt_config_t *dw_config,
+    dwt_txconfig_t *dw_configtx) {
+
+  dw_config->chan = chConfig[dr_mode].channel;
+  dw_config->prf = chConfig[dr_mode].prf;
+  dw_config->txPreambLength = chConfig[dr_mode].preambleLength;
+  dw_config->rxPAC = chConfig[dr_mode].pacSize;
+  dw_config->txCode = chConfig[dr_mode].preambleCode;
+  dw_config->rxCode = chConfig[dr_mode].preambleCode;
+  dw_config->nsSFD = chConfig[dr_mode].nsSFD;
+  dw_config->dataRate = chConfig[dr_mode].datarate;
+  dw_config->sfdTO = chConfig[dr_mode].sfdTO;
+  dw_config->phrMode = DWT_PHRMODE_STD;
+  //enable gating gain for 6.81Mbps data rate
+  /*if (dw_config->dataRate == DWT_BR_6M8)
+    dw_config->smartPowerEn = 1;
+  else
+    dw_config->smartPowerEn = 0;*/
+
+  //dwt_setsmarttxpower(0);
+  dw_configtx->power = txSpectrumConfig[dw_config->chan].txPwr[dw_config->prf
+      - DWT_PRF_16M];
+  dw_configtx->PGdly = txSpectrumConfig[dw_config->chan].PGdelay;
+
+}
+
+uint8_t SMpower(float gain) {
+   if (gain <= 0.0f) {
+     return 0xc0;
+   } else if (gain > 18.0f+15.5f) {
+     return 0x1f;
+   } else {
+     int gain_coarse_3;  // coarse gain in multiples of 3 dB
+     int gain_fine_0_5;  // gine gain in multiples of 0.5 dB
+
+     // set coarse gain first
+     if (gain > 18) {
+       gain_coarse_3 = 6;
+     } else {
+       gain_coarse_3 = (int)(gain/3);
+     }
+
+     // then set the fine gain
+     gain_fine_0_5 = 2*(gain - gain_coarse_3);
+
+     uint8_t result = ((6-gain_coarse_3)<<5) + gain_fine_0_5;
+
+     return result;
+   }
+}
+
+
+float SMgain(uint8_t power) {
+  float gain_coarse = 3.0*(6-(power>>5));
+  float gain_fine = 0.5*(power & 0x1f);
+  return gain_coarse + gain_fine;
+}
+
+
+const chConfig_t chConfig[NUM_CH_SUPPORTED] = {
+    { 1,             // channel // TKA was 2
+        DWT_PRF_16M,    // prf
+        DWT_BR_110K,    // datarate
+        3,             // preambleCode
+        DWT_PLEN_1024,  // preambleLength
+        DWT_PAC32,      // pacSize
+        1,       // non-standard SFD
+        (1025 + 64 - 32) //SFD timeout
+    },
+    //mode 2
+    { 2,              // channel
+        DWT_PRF_16M,    // prf
+        DWT_BR_6M8,    // datarate
+        3,             // preambleCode
+        DWT_PLEN_128,   // preambleLength
+        DWT_PAC8,       // pacSize
+        0,       // non-standard SFD
+        (129 + 8 - 8) //SFD timeout
+    },
+    //mode 3
+    { 2,              // channel
+        DWT_PRF_64M,    // prf
+        DWT_BR_110K,    // datarate
+        9,             // preambleCode
+        DWT_PLEN_1024,  // preambleLength
+        DWT_PAC32,      // pacSize
+        1,       // non-standard SFD
+        (1025 + 64 - 32) //SFD timeout
+    },
+    //mode 4
+    { 2,              // channel
+        DWT_PRF_64M,    // prf
+        DWT_BR_6M8,    // datarate
+        9,             // preambleCode
+        DWT_PLEN_128,   // preambleLength
+        DWT_PAC8,       // pacSize
+        0,       // non-standard SFD
+        (129 + 8 - 8) //SFD timeout
+    },
+    //mode 5
+    { 5,              // channel
+        DWT_PRF_16M,    // prf
+        DWT_BR_110K,    // datarate
+        3,             // preambleCode
+        DWT_PLEN_1024,  // preambleLength
+        DWT_PAC32,      // pacSize
+        1,       // non-standard SFD
+        (1025 + 64 - 32) //SFD timeout
+    },
+    //mode 6
+    { 5,              // channel
+        DWT_PRF_16M,    // prf
+        DWT_BR_6M8,    // datarate
+        3,             // preambleCode
+        DWT_PLEN_128,   // preambleLength
+        DWT_PAC8,       // pacSize
+        0,       // non-standard SFD
+        (129 + 8 - 8) //SFD timeout
+    },
+    //mode 6
+    { 5,              // channel
+        DWT_PRF_64M,    // prf
+        DWT_BR_110K,     // datarate
+        9,             // preambleCode
+        DWT_PLEN_1024,  // preambleLength
+        DWT_PAC32,      // pacSize
+        1,       // non-standard SFD
+        (1025 + 64 - 32) //SFD timeout
+    },
+    //mode 7
+    { 7,              // channel
+        DWT_PRF_64M,    // prf
+        DWT_BR_6M8,    // datarate
+        9,             // preambleCode
+        DWT_PLEN_128,   // preambleLength
+        DWT_PAC8,       // pacSize
+        0,       // non-standard SFD
+        (129 + 8 - 8) //SFD timeout
+    } };
+
+const float ChannelFrequency[NUM_CH_SUPPORTED] = { 0.0, 3.49944, 3.9936, 4.4928,
+    3.9936, 6.4896, 0, 6.4986 };
+const float ChannelBandwidth[NUM_CH_SUPPORTED] = { 0.0, 0.4992, 0.4992, 0.4992,
+    1.3312, 0.4992, 0, 1.0816 };
+const char* ChannelBitrate[3] = {"110 kbits/s", "850 kbits/s", "6.8 Mbits/s"};
+const char* ChannelPRF[3] = {"", "16 MHz", "64 MHz"};
+const uint8_t ChannelPAC[4] = {8, 16, 32, 64};
+
+uint16_t ChannelPLEN(uint8_t PLEN) {
+  switch (PLEN) {
+    case  DWT_PLEN_4096:
+      return (uint16_t)4096;
+
+    case DWT_PLEN_2048:
+      return (uint16_t)2048;
+
+    case DWT_PLEN_1536:
+      return (uint16_t)1536;
+
+    case DWT_PLEN_1024:
+      return (uint16_t)1024;
+
+    case DWT_PLEN_512:
+      return (uint16_t)512;
+
+    case DWT_PLEN_256:
+      return (uint16_t)256;
+
+    case DWT_PLEN_128:
+      return (uint16_t)128;
+
+    case DWT_PLEN_64:
+      return (uint16_t)64;
+
+    default:
+      return 0;
+  };
+
+}
+
+
+
+
+//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[NUM_CH_SUPPORTED] = {
+//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,//0x51515151, //0xc0c0c0c0,//0x1f1f1f1f,//0x32527292, //16M prf power
+           0x1f1f1f1f//0x5171B1d1//0x51515151//0xc0c0c0c0//0x1f1f1f1f// 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[NUM_PRF] = { (uint16_t) ((DWT_PRF_16M_RFDLY / 2.0f)
+    * 1.0e-9f / DWT_TIME_UNITS), //PRF 16
+    (uint16_t) ((DWT_PRF_64M_RFDLY / 2.0f) * 1.0e-9f / DWT_TIME_UNITS) //PRF 64
+    };
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/DecaWave/SMConfig.h	Wed Dec 06 21:35:45 2017 +0000
@@ -0,0 +1,107 @@
+#ifndef _SMCONFIG_H_
+#define _SMCONFIG_H_
+
+#include "mbed.h"
+#include "deca_device_api.h"
+#include "deca_param_types.h"
+//#include "frames.h"
+
+#define DWT_PRF_64M_RFDLY   (515.2f)
+#define DWT_PRF_16M_RFDLY   (514.7f)
+
+
+struct ppsConfig_s {
+  /*PPS_Role*/ int Role;
+
+  PinName SpiMosi;
+  PinName SpiMiso;
+  PinName SpiClk;
+
+  PinName DwCs;
+  PinName DwIrq;
+  PinName DwRst;
+
+  PinName LedGreen;
+  PinName LedRed;
+
+  PinName BatIn;
+  PinName Buzzer;
+  PinName Vibra;
+
+  PinName CanRx;
+  PinName CanTx;
+  PinName CanStandby;
+
+  PinName I2cSda;
+  PinName I2cScl;
+
+  PinName DipSwitch0;
+  PinName DipSwitch1;
+  PinName DipSwitch2;
+  PinName DipSwitch3;
+  PinName DipSwitch4;
+  PinName DipSwitch5;
+  PinName DipSwitch6;
+  PinName DipSwitch7;
+};
+
+typedef ppsConfig_s ppsConfig_t;
+
+struct chConfig_t {
+    uint8_t channel ;
+    uint8_t prf ;
+    uint8_t datarate ;
+    uint8_t preambleCode ;
+    uint8_t preambleLength ;
+    uint8_t pacSize ;
+    uint8_t nsSFD ;
+    uint16_t sfdTO ;
+};
+
+struct tx_struct {
+    uint8_t PGdelay;
+
+    //TX POWER
+    //31:24     BOOST_0.125ms_PWR
+    //23:16     BOOST_0.25ms_PWR-TX_SHR_PWR
+    //15:8      BOOST_0.5ms_PWR-TX_PHR_PWR
+    //7:0       DEFAULT_PWR-TX_DATA_PWR
+    uint32_t txPwr[2]; //
+};
+
+void SMsetconfig(uint8_t dr_mode, dwt_config_t *dw_config, dwt_txconfig_t *dw_configtx);
+uint8_t SMpower(float gain);
+float SMgain(uint8_t power);
+
+extern const agc_cfg_struct agc_config ;
+
+//SFD threshold settings for 110k, 850k, 6.8Mb standard and non-standard
+extern const uint16_t sftsh[NUM_BR][NUM_SFD];
+
+extern const uint16_t dtune1[NUM_PRF];
+
+#define XMLPARAMS_VERSION  (1.17f)
+
+extern const uint8_t pll2_config[NUM_CH][5];
+extern const uint8_t pll2calcfg;
+extern const uint8_t rx_config[NUM_BW];
+//extern const uint32_t tx_config[NUM_CH];
+extern const uint8_t dwnsSFDlen[NUM_BR];        //length of SFD for each of the bitrates
+//extern const uint32_t digital_bb_config[NUM_PRF][NUM_PACS];
+extern const uint8_t chan_idx[NUM_CH_SUPPORTED];
+
+extern const uint16_t lde_replicaCoeff[PCODES];
+extern const tx_struct txSpectrumConfig[NUM_CH_SUPPORTED];
+extern const uint16_t rfDelays[NUM_PRF];
+
+extern const chConfig_t chConfig[NUM_CH_SUPPORTED];
+extern const float ChannelFrequency[NUM_CH_SUPPORTED];
+extern const float ChannelBandwidth[NUM_CH_SUPPORTED];
+extern const char* ChannelBitrate[3];
+extern const char* ChannelPRF[3];
+extern const uint8_t ChannelPAC[4];
+extern uint16_t ChannelPLEN(uint8_t PLEN);
+
+extern const tx_struct txSpectrumConfig[NUM_CH_SUPPORTED];
+#endif
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PC/PC.cpp	Wed Dec 06 21:35:45 2017 +0000
@@ -0,0 +1,63 @@
+#include "PC.h"
+#include "mbed.h"
+
+PC::PC(PinName tx, PinName rx, int baudrate) : Serial(tx, rx) 
+{
+    baud(baudrate);
+    cls();
+    
+    command[0] = '\0';
+    command_char_count = 0;
+}
+
+
+void PC::cls() 
+{
+    printf("\x1B[2J");
+}
+
+
+void PC::locate(int Spalte, int Zeile) 
+{
+    printf("\x1B[%d;%dH", Zeile + 1, Spalte + 1);
+}
+
+void PC::readcommand(void (*executer)(char*))
+{
+    
+/*
+    char input = getc();             // get the character from serial bus
+    //printf("\x1B[1K");
+    //printf("-");
+    if(input == '\r') {                 // if return was pressed, the command must be executed
+        command[command_char_count] = '\0';
+        executer(&command[0]);
+        printf("Debug Point 5");
+        //printf("1");
+        command_char_count = 0;                 // reset command
+        command[command_char_count] = '\0';
+       // printf("2");
+    } else if (command_char_count < COMMAND_MAX_LENGHT) {
+        printf("Debug Point 4");
+        command[command_char_count] = input;
+        command_char_count++;
+    }
+*/
+
+
+
+    while (1)
+    {
+        if (readable())
+        {
+            scanf( "%s" , command );
+            break;
+        }
+    }
+    
+   
+    
+    executer(&command[0]);
+
+
+}
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PC/PC.h	Wed Dec 06 21:35:45 2017 +0000
@@ -0,0 +1,21 @@
+#include "mbed.h"
+
+#ifndef PC_H
+#define PC_H
+
+#define COMMAND_MAX_LENGHT 300
+
+class PC : public Serial 
+{
+    public:
+        PC(PinName tx, PinName rx, int baud);
+        void cls();                                                                        // to clear the display
+        void locate(int column, int row);                                                  // to relocate the cursor
+        void readcommand(void (*executer)(char*));                  // to read a char from the pc to the command string
+        
+        char command[COMMAND_MAX_LENGHT];
+    private:
+        int command_char_count;
+};
+#endif
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Watchdog/Watchdog.cpp	Wed Dec 06 21:35:45 2017 +0000
@@ -0,0 +1,25 @@
+#include "Watchdog.h"
+
+
+Watchdog::Watchdog() {
+}
+
+// Load timeout value in watchdog timer and enable
+void Watchdog::kick(float s) {
+    
+    uint32_t clk = SystemCoreClock / 3;    // found by trying. TODO: exakt values
+    hiwdg.Instance = IWDG;
+    hiwdg.Init.Prescaler = IWDG_PRESCALER_16;
+    hiwdg.Init.Reload = s * (float)clk;
+    if (HAL_IWDG_Init(&hiwdg) != HAL_OK)
+    {
+      // Error Handler
+    }    
+}
+    
+// "kick" or "feed" the dog - reset the watchdog timer
+// by writing this required bit pattern
+void Watchdog::kick() {
+    HAL_IWDG_Refresh(&hiwdg);
+}
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Watchdog/Watchdog.h	Wed Dec 06 21:35:45 2017 +0000
@@ -0,0 +1,23 @@
+#ifndef _WATCHDOG_H_
+#define _WATCHDOG_H_
+
+#include "globals.h"
+
+#include "mbed.h"
+#include "stm32f4xx.h"
+
+class Watchdog {
+public:
+    Watchdog();
+    void kick(float s);
+    void kick();
+private:
+    IWDG_HandleTypeDef hiwdg;
+};
+
+
+
+
+
+
+#endif /* _WATCHDOG_H_ */
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/decadriver/deca_device.c	Wed Dec 06 21:35:45 2017 +0000
@@ -0,0 +1,3245 @@
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @file    deca_device.c
+ * @brief   Decawave device configuration and control functions
+ *
+ * @attention
+ *
+ * Copyright 2013 (c) Decawave Ltd, Dublin, Ireland.
+ *
+ * All rights reserved.
+ *
+ */
+
+#include <assert.h>
+
+#include "deca_types.h"
+#include "deca_param_types.h"
+#include "deca_regs.h"
+#include "deca_device_api.h"
+
+// Defines for enable_clocks function
+#define FORCE_SYS_XTI  0
+#define ENABLE_ALL_SEQ 1
+#define FORCE_SYS_PLL  2
+#define READ_ACC_ON    7
+#define READ_ACC_OFF   8
+#define FORCE_OTP_ON   11
+#define FORCE_OTP_OFF  12
+#define FORCE_TX_PLL   13
+#define FORCE_LDE      14
+
+// Defines for ACK request bitmask in DATA and MAC COMMAND frame control (first byte) - Used to detect AAT bit wrongly set.
+#define FCTRL_ACK_REQ_MASK 0x20
+// Frame control maximum length in bytes.
+#define FCTRL_LEN_MAX 2
+
+// #define DWT_API_ERROR_CHECK     // define so API checks config input parameters
+
+// -------------------------------------------------------------------------------------------------------------------
+//
+// Internal functions for controlling and configuring the device
+//
+// -------------------------------------------------------------------------------------------------------------------
+
+// Enable and Configure specified clocks
+void _dwt_enableclocks(int clocks) ;
+// Configure the ucode (FP algorithm) parameters
+void _dwt_configlde(int prf);
+// Load ucode from OTP/ROM
+void _dwt_loaducodefromrom(void);
+// Read non-volatile memory
+uint32 _dwt_otpread(uint32 address);
+// Program the non-volatile memory
+uint32 _dwt_otpprogword32(uint32 data, uint16 address);
+// Upload the device configuration into always on memory
+void _dwt_aonarrayupload(void);
+// -------------------------------------------------------------------------------------------------------------------
+
+/*!
+ * Static data for DW1000 DecaWave Transceiver control
+ */
+
+// -------------------------------------------------------------------------------------------------------------------
+// Structure to hold device data
+typedef struct
+{
+    uint32      partID ;            // IC Part ID - read during initialisation
+    uint32      lotID ;             // IC Lot ID - read during initialisation
+    uint8       longFrames ;        // Flag in non-standard long frame mode
+    uint8       otprev ;            // OTP revision number (read during initialisation)
+    uint32      txFCTRL ;           // Keep TX_FCTRL register config
+    uint8       init_xtrim;         // initial XTAL trim value read from OTP (or defaulted to mid-range if OTP not programmed)
+    uint8       dblbuffon;          // Double RX buffer mode flag
+    uint32      sysCFGreg ;         // Local copy of system config register
+    uint16      sleep_mode;         // Used for automatic reloading of LDO tune and microcode at wake-up
+    uint8       wait4resp ;         // wait4response was set with last TX start command
+    dwt_cb_data_t cbData;           // Callback data structure
+    dwt_cb_t    cbTxDone;           // Callback for TX confirmation event
+    dwt_cb_t    cbRxOk;             // Callback for RX good frame event
+    dwt_cb_t    cbRxTo;             // Callback for RX timeout events
+    dwt_cb_t    cbRxErr;            // Callback for RX error events
+} dwt_local_data_t ;
+
+static dwt_local_data_t dw1000local ; // Static local device data
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_initialise()
+ *
+ * @brief This function initiates communications with the DW1000 transceiver
+ * and reads its DEV_ID register (address 0x00) to verify the IC is one supported
+ * by this software (e.g. DW1000 32-bit device ID value is 0xDECA0130).  Then it
+ * does any initial once only device configurations needed for use and initialises
+ * as necessary any static data items belonging to this low-level driver.
+ *
+ * NOTES:
+ * 1.this function needs to be run before dwt_configuresleep, also the SPI frequency has to be < 3MHz
+ * 2.it also reads and applies LDO tune and crystal trim values from OTP memory
+ *
+ * input parameters
+ * @param config    -   specifies what configuration to load
+ *                  DWT_LOADUCODE     0x1 - load the LDE microcode from ROM - enabled accurate RX timestamp
+ *                  DWT_LOADNONE      0x0 - do not load any values from OTP memory
+ *
+ * output parameters
+ *
+ * returns DWT_SUCCESS for success, or DWT_ERROR for error
+ */
+// OTP addresses definitions
+#define LDOTUNE_ADDRESS (0x04)
+#define PARTID_ADDRESS (0x06)
+#define LOTID_ADDRESS  (0x07)
+#define VBAT_ADDRESS   (0x08)
+#define VTEMP_ADDRESS  (0x09)
+#define XTRIM_ADDRESS  (0x1E)
+
+int dwt_initialise(uint16 config)
+{
+    uint16 otp_addr = 0;
+    uint32 ldo_tune = 0;
+
+    dw1000local.dblbuffon = 0; // Double buffer mode off by default
+    dw1000local.wait4resp = 0;
+    dw1000local.sleep_mode = 0;
+
+    dw1000local.cbTxDone = NULL;
+    dw1000local.cbRxOk = NULL;
+    dw1000local.cbRxTo = NULL;
+    dw1000local.cbRxErr = NULL;
+
+    // Read and validate device ID return -1 if not recognised
+    if (DWT_DEVICE_ID != dwt_readdevid()) // MP IC ONLY (i.e. DW1000) FOR THIS CODE
+    {
+        return DWT_ERROR ;
+    }
+
+    // Make sure the device is completely reset before starting initialisation
+    dwt_softreset();
+
+    _dwt_enableclocks(FORCE_SYS_XTI); // NOTE: set system clock to XTI - this is necessary to make sure the values read by _dwt_otpread are reliable
+
+    // Configure the CPLL lock detect
+    dwt_write8bitoffsetreg(EXT_SYNC_ID, EC_CTRL_OFFSET, EC_CTRL_PLLLCK);
+
+    // Read OTP revision number
+    otp_addr = _dwt_otpread(XTRIM_ADDRESS) & 0xffff;        // Read 32 bit value, XTAL trim val is in low octet-0 (5 bits)
+    dw1000local.otprev = (otp_addr >> 8) & 0xff;            // OTP revision is next byte
+
+    // Load LDO tune from OTP and kick it if there is a value actually programmed.
+    ldo_tune = _dwt_otpread(LDOTUNE_ADDRESS);
+    if((ldo_tune & 0xFF) != 0)
+    {
+        // Kick LDO tune
+        dwt_write8bitoffsetreg(OTP_IF_ID, OTP_SF, OTP_SF_LDO_KICK); // Set load LDE kick bit
+        dw1000local.sleep_mode |= AON_WCFG_ONW_LLDO; // LDO tune must be kicked at wake-up
+    }
+
+    // Load Part and Lot ID from OTP
+    dw1000local.partID = _dwt_otpread(PARTID_ADDRESS);
+    dw1000local.lotID = _dwt_otpread(LOTID_ADDRESS);
+
+    // XTAL trim value is set in OTP for DW1000 module and EVK/TREK boards but that might not be the case in a custom design
+    dw1000local.init_xtrim = otp_addr & 0x1F;
+    if (!dw1000local.init_xtrim) // A value of 0 means that the crystal has not been trimmed
+    {
+        dw1000local.init_xtrim = FS_XTALT_MIDRANGE ; // Set to mid-range if no calibration value inside
+    }
+    // Configure XTAL trim
+    dwt_setxtaltrim(dw1000local.init_xtrim);
+
+    // Load leading edge detect code
+    if(config & DWT_LOADUCODE)
+    {
+        _dwt_loaducodefromrom();
+        dw1000local.sleep_mode |= AON_WCFG_ONW_LLDE; // microcode must be loaded at wake-up
+    }
+    else // Should disable the LDERUN enable bit in 0x36, 0x4
+    {
+        uint16 rega = dwt_read16bitoffsetreg(PMSC_ID, PMSC_CTRL1_OFFSET+1) ;
+        rega &= 0xFDFF ; // Clear LDERUN bit
+        dwt_write16bitoffsetreg(PMSC_ID, PMSC_CTRL1_OFFSET+1, rega) ;
+    }
+
+    _dwt_enableclocks(ENABLE_ALL_SEQ); // Enable clocks for sequencing
+
+    // The 3 bits in AON CFG1 register must be cleared to ensure proper operation of the DW1000 in DEEPSLEEP mode.
+    dwt_write8bitoffsetreg(AON_ID, AON_CFG1_OFFSET, 0x00);
+
+    // Read system register / store local copy
+    dw1000local.sysCFGreg = dwt_read32bitreg(SYS_CFG_ID) ; // Read sysconfig register
+
+    return DWT_SUCCESS ;
+
+} // end dwt_initialise()
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_otprevision()
+ *
+ * @brief This is used to return the read OTP revision
+ *
+ * NOTE: dwt_initialise() must be called prior to this function so that it can return a relevant value.
+ *
+ * input parameters
+ *
+ * output parameters
+ *
+ * returns the read OTP revision value
+ */
+uint8 dwt_otprevision(void)
+{
+    return dw1000local.otprev ;
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_setfinegraintxseq()
+ *
+ * @brief This function enables/disables the fine grain TX sequencing (enabled by default).
+ *
+ * input parameters
+ * @param enable - 1 to enable fine grain TX sequencing, 0 to disable it.
+ *
+ * output parameters none
+ *
+ * no return value
+ */
+void dwt_setfinegraintxseq(int enable)
+{
+    if (enable)
+    {
+        dwt_write16bitoffsetreg(PMSC_ID, PMSC_TXFINESEQ_OFFSET, PMSC_TXFINESEQ_ENABLE);
+    }
+    else
+    {
+        dwt_write16bitoffsetreg(PMSC_ID, PMSC_TXFINESEQ_OFFSET, PMSC_TXFINESEQ_DISABLE);
+    }
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_setlnapamode()
+ *
+ * @brief This is used to enable GPIO for external LNA or PA functionality - HW dependent, consult the DW1000 User Manual.
+ *        This can also be used for debug as enabling TX and RX GPIOs is quite handy to monitor DW1000's activity.
+ *
+ * NOTE: Enabling PA functionality requires that fine grain TX sequencing is deactivated. This can be done using
+ *       dwt_setfinegraintxseq().
+ *
+ * input parameters
+ * @param lna - 1 to enable LNA functionality, 0 to disable it
+ * @param pa - 1 to enable PA functionality, 0 to disable it
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_setlnapamode(int lna, int pa)
+{
+    uint32 gpio_mode = dwt_read32bitoffsetreg(GPIO_CTRL_ID, GPIO_MODE_OFFSET);
+    gpio_mode &= ~(GPIO_MSGP4_MASK | GPIO_MSGP5_MASK | GPIO_MSGP6_MASK);
+    if (lna)
+    {
+        gpio_mode |= GPIO_PIN6_EXTRXE;
+    }
+    if (pa)
+    {
+        gpio_mode |= (GPIO_PIN5_EXTTXE | GPIO_PIN4_EXTPA);
+    }
+    dwt_write32bitoffsetreg(GPIO_CTRL_ID, GPIO_MODE_OFFSET, gpio_mode);
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_setgpiodirection()
+ *
+ * @brief This is used to set GPIO direction as an input (1) or output (0)
+ *
+ * input parameters
+ * @param gpioNum    -   this is the GPIO to configure - see GxM0... GxM8 in the deca_regs.h file
+ * @param direction  -   this sets the GPIO direction - see GxP0... GxP8 in the deca_regs.h file
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_setgpiodirection(uint32 gpioNum, uint32 direction)
+{
+    uint8 buf[GPIO_DIR_LEN];
+    uint32 command = direction | gpioNum;
+
+    buf[0] = command & 0xff;
+    buf[1] = (command >> 8) & 0xff;
+    buf[2] = (command >> 16) & 0xff;
+
+    dwt_writetodevice(GPIO_CTRL_ID, GPIO_DIR_OFFSET, GPIO_DIR_LEN, buf);
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_setgpiovalue()
+ *
+ * @brief This is used to set GPIO value as (1) or (0) only applies if the GPIO is configured as output
+ *
+ * input parameters
+ * @param gpioNum    -   this is the GPIO to configure - see GxM0... GxM8 in the deca_regs.h file
+ * @param value  -   this sets the GPIO value - see GDP0... GDP8 in the deca_regs.h file
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_setgpiovalue(uint32 gpioNum, uint32 value)
+{
+    uint8 buf[GPIO_DOUT_LEN];
+    uint32 command = value | gpioNum;
+
+    buf[0] = command & 0xff;
+    buf[1] = (command >> 8) & 0xff;
+    buf[2] = (command >> 16) & 0xff;
+
+    dwt_writetodevice(GPIO_CTRL_ID, GPIO_DOUT_OFFSET, GPIO_DOUT_LEN, buf);
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_getpartid()
+ *
+ * @brief This is used to return the read part ID of the device
+ *
+ * NOTE: dwt_initialise() must be called prior to this function so that it can return a relevant value.
+ *
+ * input parameters
+ *
+ * output parameters
+ *
+ * returns the 32 bit part ID value as programmed in the factory
+ */
+uint32 dwt_getpartid(void)
+{
+    return dw1000local.partID;
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_getlotid()
+ *
+ * @brief This is used to return the read lot ID of the device
+ *
+ * NOTE: dwt_initialise() must be called prior to this function so that it can return a relevant value.
+ *
+ * input parameters
+ *
+ * output parameters
+ *
+ * returns the 32 bit lot ID value as programmed in the factory
+ */
+uint32 dwt_getlotid(void)
+{
+    return dw1000local.lotID;
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_readdevid()
+ *
+ * @brief This is used to return the read device type and revision information of the DW1000 device (MP part is 0xDECA0130)
+ *
+ * input parameters
+ *
+ * output parameters
+ *
+ * returns the read value which for DW1000 is 0xDECA0130
+ */
+uint32 dwt_readdevid(void)
+{
+    return dwt_read32bitoffsetreg(DEV_ID_ID,0);
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_configuretxrf()
+ *
+ * @brief This function provides the API for the configuration of the TX spectrum
+ * including the power and pulse generator delay. The input is a pointer to the data structure
+ * of type dwt_txconfig_t that holds all the configurable items.
+ *
+ * input parameters
+ * @param config    -   pointer to the txrf configuration structure, which contains the tx rf config data
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_configuretxrf(dwt_txconfig_t *config)
+{
+
+    // Configure RF TX PG_DELAY
+    dwt_write8bitoffsetreg(TX_CAL_ID, TC_PGDELAY_OFFSET, config->PGdly);
+
+    // Configure TX power
+    dwt_write32bitreg(TX_POWER_ID, config->power);
+
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_configure()
+ *
+ * @brief This function provides the main API for the configuration of the
+ * DW1000 and this low-level driver.  The input is a pointer to the data structure
+ * of type dwt_config_t that holds all the configurable items.
+ * The dwt_config_t structure shows which ones are supported
+ *
+ * input parameters
+ * @param config    -   pointer to the configuration structure, which contains the device configuration data.
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_configure(dwt_config_t *config)
+{
+    uint8 chan = config->chan ;
+    uint32 regval ;
+    uint16 reg16 = lde_replicaCoeff[config->rxCode];
+    uint8 prfIndex = config->prf - DWT_PRF_16M;
+    uint8 bw = ((chan == 4) || (chan == 7)) ? 1 : 0 ; // Select wide or narrow band
+
+#ifdef DWT_API_ERROR_CHECK
+    assert(config->dataRate <= DWT_BR_6M8);
+    assert(config->rxPAC <= DWT_PAC64);
+    assert((chan >= 1) && (chan <= 7) && (chan != 6));
+    assert(((config->prf == DWT_PRF_64M) && (config->txCode >= 9) && (config->txCode <= 24))
+           || ((config->prf == DWT_PRF_16M) && (config->txCode >= 1) && (config->txCode <= 8)));
+    assert(((config->prf == DWT_PRF_64M) && (config->rxCode >= 9) && (config->rxCode <= 24))
+           || ((config->prf == DWT_PRF_16M) && (config->rxCode >= 1) && (config->rxCode <= 8)));
+    assert((config->txPreambLength == DWT_PLEN_64) || (config->txPreambLength == DWT_PLEN_128) || (config->txPreambLength == DWT_PLEN_256)
+           || (config->txPreambLength == DWT_PLEN_512) || (config->txPreambLength == DWT_PLEN_1024) || (config->txPreambLength == DWT_PLEN_1536)
+           || (config->txPreambLength == DWT_PLEN_2048) || (config->txPreambLength == DWT_PLEN_4096));
+    assert((config->phrMode == DWT_PHRMODE_STD) || (config->phrMode == DWT_PHRMODE_EXT));
+#endif
+
+    // For 110 kbps we need a special setup
+    if(DWT_BR_110K == config->dataRate)
+    {
+        dw1000local.sysCFGreg |= SYS_CFG_RXM110K ;
+        reg16 >>= 3; // lde_replicaCoeff must be divided by 8
+    }
+    else
+    {
+        dw1000local.sysCFGreg &= (~SYS_CFG_RXM110K) ;
+    }
+
+    dw1000local.longFrames = config->phrMode ;
+
+    dw1000local.sysCFGreg &= ~SYS_CFG_PHR_MODE_11;
+    dw1000local.sysCFGreg |= (SYS_CFG_PHR_MODE_11 & (config->phrMode << SYS_CFG_PHR_MODE_SHFT));
+
+    dwt_write32bitreg(SYS_CFG_ID,dw1000local.sysCFGreg) ;
+    // Set the lde_replicaCoeff
+    dwt_write16bitoffsetreg(LDE_IF_ID, LDE_REPC_OFFSET, reg16) ;
+
+    _dwt_configlde(prfIndex);
+
+    // Configure PLL2/RF PLL block CFG/TUNE (for a given channel)
+    dwt_write32bitoffsetreg(FS_CTRL_ID, FS_PLLCFG_OFFSET, fs_pll_cfg[chan_idx[chan]]);
+    dwt_write8bitoffsetreg(FS_CTRL_ID, FS_PLLTUNE_OFFSET, fs_pll_tune[chan_idx[chan]]);
+
+    // Configure RF RX blocks (for specified channel/bandwidth)
+    dwt_write8bitoffsetreg(RF_CONF_ID, RF_RXCTRLH_OFFSET, rx_config[bw]);
+
+    // Configure RF TX blocks (for specified channel and PRF)
+    // Configure RF TX control
+    dwt_write32bitoffsetreg(RF_CONF_ID, RF_TXCTRL_OFFSET, tx_config[chan_idx[chan]]);
+
+    // Configure the baseband parameters (for specified PRF, bit rate, PAC, and SFD settings)
+    // DTUNE0
+    dwt_write16bitoffsetreg(DRX_CONF_ID, DRX_TUNE0b_OFFSET, sftsh[config->dataRate][config->nsSFD]);
+
+    // DTUNE1
+    dwt_write16bitoffsetreg(DRX_CONF_ID, DRX_TUNE1a_OFFSET, dtune1[prfIndex]);
+
+    if(config->dataRate == DWT_BR_110K)
+    {
+        dwt_write16bitoffsetreg(DRX_CONF_ID, DRX_TUNE1b_OFFSET, DRX_TUNE1b_110K);
+    }
+    else
+    {
+        if(config->txPreambLength == DWT_PLEN_64)
+        {
+            dwt_write16bitoffsetreg(DRX_CONF_ID, DRX_TUNE1b_OFFSET, DRX_TUNE1b_6M8_PRE64);
+            dwt_write8bitoffsetreg(DRX_CONF_ID, DRX_TUNE4H_OFFSET, DRX_TUNE4H_PRE64);
+        }
+        else
+        {
+            dwt_write16bitoffsetreg(DRX_CONF_ID, DRX_TUNE1b_OFFSET, DRX_TUNE1b_850K_6M8);
+            dwt_write8bitoffsetreg(DRX_CONF_ID, DRX_TUNE4H_OFFSET, DRX_TUNE4H_PRE128PLUS);
+        }
+    }
+
+    // DTUNE2
+    dwt_write32bitoffsetreg(DRX_CONF_ID, DRX_TUNE2_OFFSET, digital_bb_config[prfIndex][config->rxPAC]);
+
+    // DTUNE3 (SFD timeout)
+    // Don't allow 0 - SFD timeout will always be enabled
+    if(config->sfdTO == 0)
+    {
+        config->sfdTO = DWT_SFDTOC_DEF;
+    }
+    dwt_write16bitoffsetreg(DRX_CONF_ID, DRX_SFDTOC_OFFSET, config->sfdTO);
+
+    // Configure AGC parameters
+    dwt_write32bitoffsetreg( AGC_CFG_STS_ID, 0xC, agc_config.lo32);
+    dwt_write16bitoffsetreg( AGC_CFG_STS_ID, 0x4, agc_config.target[prfIndex]);
+
+    // Set (non-standard) user SFD for improved performance,
+    if(config->nsSFD)
+    {
+        // Write non standard (DW) SFD length
+        dwt_write8bitoffsetreg(USR_SFD_ID, 0x00, dwnsSFDlen[config->dataRate]);
+    }
+    regval =  (CHAN_CTRL_TX_CHAN_MASK & (chan << CHAN_CTRL_TX_CHAN_SHIFT)) | // Transmit Channel
+              (CHAN_CTRL_RX_CHAN_MASK & (chan << CHAN_CTRL_RX_CHAN_SHIFT)) | // Receive Channel
+              (CHAN_CTRL_RXFPRF_MASK & (config->prf << CHAN_CTRL_RXFPRF_SHIFT)) | // RX PRF
+              (CHAN_CTRL_DWSFD & (config->nsSFD << CHAN_CTRL_DWSFD_SHIFT)) | // Use DW nsSFD
+              (CHAN_CTRL_TX_PCOD_MASK & (config->txCode << CHAN_CTRL_TX_PCOD_SHIFT)) | // TX Preamble Code
+              (CHAN_CTRL_RX_PCOD_MASK & (config->rxCode << CHAN_CTRL_RX_PCOD_SHIFT)) ; // RX Preamble Code
+
+    dwt_write32bitreg(CHAN_CTRL_ID,regval) ;
+
+    // Set up TX Preamble Size, PRF and Data Rate
+    dw1000local.txFCTRL = ((config->txPreambLength | config->prf) << TX_FCTRL_TXPRF_SHFT) | (config->dataRate << TX_FCTRL_TXBR_SHFT);
+    dwt_write32bitreg(TX_FCTRL_ID, dw1000local.txFCTRL);
+
+    // The SFD transmit pattern is initialised by the DW1000 upon a user TX request, but (due to an IC issue) it is not done for an auto-ACK TX. The
+    // SYS_CTRL write below works around this issue, by simultaneously initiating and aborting a transmission, which correctly initialises the SFD
+    // after its configuration or reconfiguration.
+    // This issue is not documented at the time of writing this code. It should be in next release of DW1000 User Manual (v2.09, from July 2016).
+    dwt_write8bitoffsetreg(SYS_CTRL_ID, SYS_CTRL_OFFSET, SYS_CTRL_TXSTRT | SYS_CTRL_TRXOFF); // Request TX start and TRX off at the same time
+} // end dwt_configure()
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_setrxantennadelay()
+ *
+ * @brief This API function writes the antenna delay (in time units) to RX registers
+ *
+ * input parameters:
+ * @param rxDelay - this is the total (RX) antenna delay value, which
+ *                          will be programmed into the RX register
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_setrxantennadelay(uint16 rxDelay)
+{
+    // Set the RX antenna delay for auto TX timestamp adjustment
+    dwt_write16bitoffsetreg(LDE_IF_ID, LDE_RXANTD_OFFSET, rxDelay);
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_settxantennadelay()
+ *
+ * @brief This API function writes the antenna delay (in time units) to TX registers
+ *
+ * input parameters:
+ * @param txDelay - this is the total (TX) antenna delay value, which
+ *                          will be programmed into the TX delay register
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_settxantennadelay(uint16 txDelay)
+{
+    // Set the TX antenna delay for auto TX timestamp adjustment
+    dwt_write16bitoffsetreg(TX_ANTD_ID, TX_ANTD_OFFSET, txDelay);
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_writetxdata()
+ *
+ * @brief This API function writes the supplied TX data into the DW1000's
+ * TX buffer.  The input parameters are the data length in bytes and a pointer
+ * to those data bytes.
+ *
+ * input parameters
+ * @param txFrameLength  - This is the total frame length, including the two byte CRC.
+ *                         Note: this is the length of TX message (including the 2 byte CRC) - max is 1023
+ *                         standard PHR mode allows up to 127 bytes
+ *                         if > 127 is programmed, DWT_PHRMODE_EXT needs to be set in the phrMode configuration
+ *                         see dwt_configure function
+ * @param txFrameBytes   - Pointer to the user’s buffer containing the data to send.
+ * @param txBufferOffset - This specifies an offset in the DW1000’s TX Buffer at which to start writing data.
+ *
+ * output parameters
+ *
+ * returns DWT_SUCCESS for success, or DWT_ERROR for error
+ */
+int dwt_writetxdata(uint16 txFrameLength, uint8 *txFrameBytes, uint16 txBufferOffset)
+{
+#ifdef DWT_API_ERROR_CHECK
+    assert(txFrameLength >= 2);
+    assert((dw1000local.longFrames && (txFrameLength <= 1023)) || (txFrameLength <= 127));
+    assert((txBufferOffset + txFrameLength) <= 1024);
+#endif
+
+    if ((txBufferOffset + txFrameLength) <= 1024)
+    {
+        // Write the data to the IC TX buffer, (-2 bytes for auto generated CRC)
+        dwt_writetodevice( TX_BUFFER_ID, txBufferOffset, txFrameLength-2, txFrameBytes);
+        return DWT_SUCCESS;
+    }
+    else
+    {
+        return DWT_ERROR;
+    }
+} // end dwt_writetxdata()
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_writetxfctrl()
+ *
+ * @brief This API function configures the TX frame control register before the transmission of a frame
+ *
+ * input parameters:
+ * @param txFrameLength - this is the length of TX message (including the 2 byte CRC) - max is 1023
+ *                              NOTE: standard PHR mode allows up to 127 bytes
+ *                              if > 127 is programmed, DWT_PHRMODE_EXT needs to be set in the phrMode configuration
+ *                              see dwt_configure function
+ * @param txBufferOffset - the offset in the tx buffer to start writing the data
+ * @param ranging - 1 if this is a ranging frame, else 0
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_writetxfctrl(uint16 txFrameLength, uint16 txBufferOffset, int ranging)
+{
+
+#ifdef DWT_API_ERROR_CHECK
+    assert((dw1000local.longFrames && (txFrameLength <= 1023)) || (txFrameLength <= 127));
+#endif
+
+    // Write the frame length to the TX frame control register
+    // dw1000local.txFCTRL has kept configured bit rate information
+    uint32 reg32 = dw1000local.txFCTRL | txFrameLength | (txBufferOffset << TX_FCTRL_TXBOFFS_SHFT) | (ranging << TX_FCTRL_TR_SHFT);
+    dwt_write32bitreg(TX_FCTRL_ID, reg32);
+} // end dwt_writetxfctrl()
+
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_readrxdata()
+ *
+ * @brief This is used to read the data from the RX buffer, from an offset location give by offset parameter
+ *
+ * input parameters
+ * @param buffer - the buffer into which the data will be read
+ * @param length - the length of data to read (in bytes)
+ * @param rxBufferOffset - the offset in the rx buffer from which to read the data
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_readrxdata(uint8 *buffer, uint16 length, uint16 rxBufferOffset)
+{
+    dwt_readfromdevice(RX_BUFFER_ID,rxBufferOffset,length,buffer) ;
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_readaccdata()
+ *
+ * @brief This is used to read the data from the Accumulator buffer, from an offset location give by offset parameter
+ *
+ * NOTE: Because of an internal memory access delay when reading the accumulator the first octet output is a dummy octet
+ *       that should be discarded. This is true no matter what sub-index the read begins at.
+ *
+ * input parameters
+ * @param buffer - the buffer into which the data will be read
+ * @param length - the length of data to read (in bytes)
+ * @param accOffset - the offset in the acc buffer from which to read the data
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_readaccdata(uint8 *buffer, uint16 len, uint16 accOffset)
+{
+    // Force on the ACC clocks if we are sequenced
+    _dwt_enableclocks(READ_ACC_ON);
+
+    dwt_readfromdevice(ACC_MEM_ID,accOffset,len,buffer) ;
+
+    _dwt_enableclocks(READ_ACC_OFF); // Revert clocks back
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_readdiagnostics()
+ *
+ * @brief this function reads the RX signal quality diagnostic data
+ *
+ * input parameters
+ * @param diagnostics - diagnostic structure pointer, this will contain the diagnostic data read from the DW1000
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_readdiagnostics(dwt_rxdiag_t *diagnostics)
+{
+    // Read the HW FP index
+    diagnostics->firstPath = dwt_read16bitoffsetreg(RX_TIME_ID, RX_TIME_FP_INDEX_OFFSET);
+
+    // LDE diagnostic data
+    diagnostics->maxNoise = dwt_read16bitoffsetreg(LDE_IF_ID, LDE_THRESH_OFFSET);
+
+    // Read all 8 bytes in one SPI transaction
+    dwt_readfromdevice(RX_FQUAL_ID, 0x0, 8, (uint8*)&diagnostics->stdNoise);
+
+    diagnostics->firstPathAmp1 = dwt_read16bitoffsetreg(RX_TIME_ID, RX_TIME_FP_AMPL1_OFFSET);
+
+    diagnostics->rxPreamCount = (dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXPACC_MASK) >> RX_FINFO_RXPACC_SHIFT  ;
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_readtxtimestamp()
+ *
+ * @brief This is used to read the TX timestamp (adjusted with the programmed antenna delay)
+ *
+ * input parameters
+ * @param timestamp - a pointer to a 5-byte buffer which will store the read TX timestamp time
+ *
+ * output parameters - the timestamp buffer will contain the value after the function call
+ *
+ * no return value
+ */
+void dwt_readtxtimestamp(uint8 * timestamp)
+{
+    dwt_readfromdevice(TX_TIME_ID, TX_TIME_TX_STAMP_OFFSET, TX_TIME_TX_STAMP_LEN, timestamp) ; // Read bytes directly into buffer
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_readtxtimestamphi32()
+ *
+ * @brief This is used to read the high 32-bits of the TX timestamp (adjusted with the programmed antenna delay)
+ *
+ * input parameters
+ *
+ * output parameters
+ *
+ * returns high 32-bits of TX timestamp
+ */
+uint32 dwt_readtxtimestamphi32(void)
+{
+    return dwt_read32bitoffsetreg(TX_TIME_ID, 1); // Offset is 1 to get the 4 upper bytes out of 5
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_readtxtimestamplo32()
+ *
+ * @brief This is used to read the low 32-bits of the TX timestamp (adjusted with the programmed antenna delay)
+ *
+ * input parameters
+ *
+ * output parameters
+ *
+ * returns low 32-bits of TX timestamp
+ */
+uint32 dwt_readtxtimestamplo32(void)
+{
+    return dwt_read32bitreg(TX_TIME_ID); // Read TX TIME as a 32-bit register to get the 4 lower bytes out of 5
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_readrxtimestamp()
+ *
+ * @brief This is used to read the RX timestamp (adjusted time of arrival)
+ *
+ * input parameters
+ * @param timestamp - a pointer to a 5-byte buffer which will store the read RX timestamp time
+ *
+ * output parameters - the timestamp buffer will contain the value after the function call
+ *
+ * no return value
+ */
+void dwt_readrxtimestamp(uint8 * timestamp)
+{
+    dwt_readfromdevice(RX_TIME_ID, RX_TIME_RX_STAMP_OFFSET, RX_TIME_RX_STAMP_LEN, timestamp) ; // Get the adjusted time of arrival
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_readrxtimestamphi32()
+ *
+ * @brief This is used to read the high 32-bits of the RX timestamp (adjusted with the programmed antenna delay)
+ *
+ * input parameters
+ *
+ * output parameters
+ *
+ * returns high 32-bits of RX timestamp
+ */
+uint32 dwt_readrxtimestamphi32(void)
+{
+    return dwt_read32bitoffsetreg(RX_TIME_ID, 1); // Offset is 1 to get the 4 upper bytes out of 5
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_readrxtimestamplo32()
+ *
+ * @brief This is used to read the low 32-bits of the RX timestamp (adjusted with the programmed antenna delay)
+ *
+ * input parameters
+ *
+ * output parameters
+ *
+ * returns low 32-bits of RX timestamp
+ */
+uint32 dwt_readrxtimestamplo32(void)
+{
+    return dwt_read32bitreg(RX_TIME_ID); // Read RX TIME as a 32-bit register to get the 4 lower bytes out of 5
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_readsystimestamphi32()
+ *
+ * @brief This is used to read the high 32-bits of the system time
+ *
+ * input parameters
+ *
+ * output parameters
+ *
+ * returns high 32-bits of system time timestamp
+ */
+uint32 dwt_readsystimestamphi32(void)
+{
+    return dwt_read32bitoffsetreg(SYS_TIME_ID, 1); // Offset is 1 to get the 4 upper bytes out of 5
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_readsystime()
+ *
+ * @brief This is used to read the system time
+ *
+ * input parameters
+ * @param timestamp - a pointer to a 5-byte buffer which will store the read system time
+ *
+ * output parameters
+ * @param timestamp - the timestamp buffer will contain the value after the function call
+ *
+ * no return value
+ */
+void dwt_readsystime(uint8 * timestamp)
+{
+    dwt_readfromdevice(SYS_TIME_ID, SYS_TIME_OFFSET, SYS_TIME_LEN, timestamp) ;
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_writetodevice()
+ *
+ * @brief  this function is used to write to the DW1000 device registers
+ * Notes:
+ *        1. Firstly we create a header (the first byte is a header byte)
+ *        a. check if sub index is used, if subindexing is used - set bit-6 to 1 to signify that the sub-index address follows the register index byte
+ *        b. set bit-7 (or with 0x80) for write operation
+ *        c. if extended sub address index is used (i.e. if index > 127) set bit-7 of the first sub-index byte following the first header byte
+ *
+ *        2. Write the header followed by the data bytes to the DW1000 device
+ *
+ *
+ * input parameters:
+ * @param recordNumber  - ID of register file or buffer being accessed
+ * @param index         - byte index into register file or buffer being accessed
+ * @param length        - number of bytes being written
+ * @param buffer        - pointer to buffer containing the 'length' bytes to be written
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_writetodevice
+(
+    uint16      recordNumber,
+    uint16      index,
+    uint32      length,
+    const uint8 *buffer
+)
+{
+    uint8 header[3] ; // Buffer to compose header in
+    int   cnt = 0; // Counter for length of header
+#ifdef DWT_API_ERROR_CHECK
+    assert(recordNumber <= 0x3F); // Record number is limited to 6-bits.
+#endif
+
+    // Write message header selecting WRITE operation and addresses as appropriate (this is one to three bytes long)
+    if (index == 0) // For index of 0, no sub-index is required
+    {
+        header[cnt++] = 0x80 | recordNumber ; // Bit-7 is WRITE operation, bit-6 zero=NO sub-addressing, bits 5-0 is reg file id
+    }
+    else
+    {
+#ifdef DWT_API_ERROR_CHECK
+        assert((index <= 0x7FFF) && ((index + length) <= 0x7FFF)); // Index and sub-addressable area are limited to 15-bits.
+#endif
+        header[cnt++] = 0xC0 | recordNumber ; // Bit-7 is WRITE operation, bit-6 one=sub-address follows, bits 5-0 is reg file id
+
+        if (index <= 127) // For non-zero index < 127, just a single sub-index byte is required
+        {
+            header[cnt++] = (uint8)index ; // Bit-7 zero means no extension, bits 6-0 is index.
+        }
+        else
+        {
+            header[cnt++] = 0x80 | (uint8)(index) ; // Bit-7 one means extended index, bits 6-0 is low seven bits of index.
+            header[cnt++] =  (uint8) (index >> 7) ; // 8-bit value = high eight bits of index.
+        }
+    }
+
+    // Write it to the SPI
+    writetospi(cnt,header,length,buffer);
+} // end dwt_writetodevice()
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_readfromdevice()
+ *
+ * @brief  this function is used to read from the DW1000 device registers
+ * Notes:
+ *        1. Firstly we create a header (the first byte is a header byte)
+ *        a. check if sub index is used, if subindexing is used - set bit-6 to 1 to signify that the sub-index address follows the register index byte
+ *        b. set bit-7 (or with 0x80) for write operation
+ *        c. if extended sub address index is used (i.e. if index > 127) set bit-7 of the first sub-index byte following the first header byte
+ *
+ *        2. Write the header followed by the data bytes to the DW1000 device
+ *        3. Store the read data in the input buffer
+ *
+ * input parameters:
+ * @param recordNumber  - ID of register file or buffer being accessed
+ * @param index         - byte index into register file or buffer being accessed
+ * @param length        - number of bytes being read
+ * @param buffer        - pointer to buffer in which to return the read data.
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_readfromdevice
+(
+    uint16  recordNumber,
+    uint16  index,
+    uint32  length,
+    uint8   *buffer
+)
+{
+    uint8 header[3] ; // Buffer to compose header in
+    int   cnt = 0; // Counter for length of header
+#ifdef DWT_API_ERROR_CHECK
+    assert(recordNumber <= 0x3F); // Record number is limited to 6-bits.
+#endif
+
+    // Write message header selecting READ operation and addresses as appropriate (this is one to three bytes long)
+    if (index == 0) // For index of 0, no sub-index is required
+    {
+        header[cnt++] = (uint8) recordNumber ; // Bit-7 zero is READ operation, bit-6 zero=NO sub-addressing, bits 5-0 is reg file id
+    }
+    else
+    {
+#ifdef DWT_API_ERROR_CHECK
+        assert((index <= 0x7FFF) && ((index + length) <= 0x7FFF)); // Index and sub-addressable area are limited to 15-bits.
+#endif
+        header[cnt++] = (uint8)(0x40 | recordNumber) ; // Bit-7 zero is READ operation, bit-6 one=sub-address follows, bits 5-0 is reg file id
+
+        if (index <= 127) // For non-zero index < 127, just a single sub-index byte is required
+        {
+            header[cnt++] = (uint8) index ; // Bit-7 zero means no extension, bits 6-0 is index.
+        }
+        else
+        {
+            header[cnt++] = 0x80 | (uint8)(index) ; // Bit-7 one means extended index, bits 6-0 is low seven bits of index.
+            header[cnt++] =  (uint8) (index >> 7) ; // 8-bit value = high eight bits of index.
+        }
+    }
+
+    // Do the read from the SPI
+    readfromspi(cnt, header, length, buffer);  // result is stored in the buffer
+} // end dwt_readfromdevice()
+
+
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_read32bitoffsetreg()
+ *
+ * @brief  this function is used to read 32-bit value from the DW1000 device registers
+ *
+ * input parameters:
+ * @param regFileID - ID of register file or buffer being accessed
+ * @param regOffset - the index into register file or buffer being accessed
+ *
+ * output parameters
+ *
+ * returns 32 bit register value
+ */
+uint32 dwt_read32bitoffsetreg(int regFileID,int regOffset)
+{
+    uint32  regval = 0 ;
+    int     j ;
+    uint8   buffer[4] ;
+
+    dwt_readfromdevice(regFileID,regOffset,4,buffer); // Read 4 bytes (32-bits) register into buffer
+
+    for (j = 3 ; j >= 0 ; j --)
+    {
+        regval = (regval << 8) + buffer[j] ;
+    }
+    return regval ;
+
+} // end dwt_read32bitoffsetreg()
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_read16bitoffsetreg()
+ *
+ * @brief  this function is used to read 16-bit value from the DW1000 device registers
+ *
+ * input parameters:
+ * @param regFileID - ID of register file or buffer being accessed
+ * @param regOffset - the index into register file or buffer being accessed
+ *
+ * output parameters
+ *
+ * returns 16 bit register value
+ */
+uint16 dwt_read16bitoffsetreg(int regFileID,int regOffset)
+{
+    uint16  regval = 0 ;
+    uint8   buffer[2] ;
+
+    dwt_readfromdevice(regFileID,regOffset,2,buffer); // Read 2 bytes (16-bits) register into buffer
+
+    regval = (buffer[1] << 8) + buffer[0] ;
+    return regval ;
+
+} // end dwt_read16bitoffsetreg()
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_read8bitoffsetreg()
+ *
+ * @brief  this function is used to read an 8-bit value from the DW1000 device registers
+ *
+ * input parameters:
+ * @param regFileID - ID of register file or buffer being accessed
+ * @param regOffset - the index into register file or buffer being accessed
+ *
+ * output parameters
+ *
+ * returns 8-bit register value
+ */
+uint8 dwt_read8bitoffsetreg(int regFileID, int regOffset)
+{
+    uint8 regval;
+
+    dwt_readfromdevice(regFileID, regOffset, 1, &regval);
+
+    return regval ;
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_write8bitoffsetreg()
+ *
+ * @brief  this function is used to write an 8-bit value to the DW1000 device registers
+ *
+ * input parameters:
+ * @param regFileID - ID of register file or buffer being accessed
+ * @param regOffset - the index into register file or buffer being accessed
+ * @param regval    - the value to write
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_write8bitoffsetreg(int regFileID, int regOffset, uint8 regval)
+{
+    dwt_writetodevice(regFileID, regOffset, 1, &regval);
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_write16bitoffsetreg()
+ *
+ * @brief  this function is used to write 16-bit value to the DW1000 device registers
+ *
+ * input parameters:
+ * @param regFileID - ID of register file or buffer being accessed
+ * @param regOffset - the index into register file or buffer being accessed
+ * @param regval    - the value to write
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_write16bitoffsetreg(int regFileID,int regOffset,uint16 regval)
+{
+    uint8   buffer[2] ;
+
+    buffer[0] = regval & 0xFF;
+    buffer[1] = regval >> 8 ;
+
+    dwt_writetodevice(regFileID,regOffset,2,buffer);
+} // end dwt_write16bitoffsetreg()
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_write32bitoffsetreg()
+ *
+ * @brief  this function is used to write 32-bit value to the DW1000 device registers
+ *
+ * input parameters:
+ * @param regFileID - ID of register file or buffer being accessed
+ * @param regOffset - the index into register file or buffer being accessed
+ * @param regval    - the value to write
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_write32bitoffsetreg(int regFileID,int regOffset,uint32 regval)
+{
+    int     j ;
+    uint8   buffer[4] ;
+
+    for ( j = 0 ; j < 4 ; j++ )
+    {
+        buffer[j] = regval & 0xff ;
+        regval >>= 8 ;
+    }
+
+    dwt_writetodevice(regFileID,regOffset,4,buffer);
+} // end dwt_write32bitoffsetreg()
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_enableframefilter()
+ *
+ * @brief This is used to enable the frame filtering - (the default option is to
+ * accept any data and ACK frames with correct destination address
+ *
+ * input parameters
+ * @param - bitmask - enables/disables the frame filtering options according to
+ *      DWT_FF_NOTYPE_EN        0x000   no frame types allowed
+ *      DWT_FF_COORD_EN         0x002   behave as coordinator (can receive frames with no destination address (PAN ID has to match))
+ *      DWT_FF_BEACON_EN        0x004   beacon frames allowed
+ *      DWT_FF_DATA_EN          0x008   data frames allowed
+ *      DWT_FF_ACK_EN           0x010   ack frames allowed
+ *      DWT_FF_MAC_EN           0x020   mac control frames allowed
+ *      DWT_FF_RSVD_EN          0x040   reserved frame types allowed
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_enableframefilter(uint16 enable)
+{
+    uint32 sysconfig = SYS_CFG_MASK & dwt_read32bitreg(SYS_CFG_ID) ; // Read sysconfig register
+
+    if(enable)
+    {
+        // Enable frame filtering and configure frame types
+        sysconfig &= ~(SYS_CFG_FF_ALL_EN); // Clear all
+        sysconfig |= (enable & SYS_CFG_FF_ALL_EN) | SYS_CFG_FFE;
+    }
+    else
+    {
+        sysconfig &= ~(SYS_CFG_FFE);
+    }
+
+    dw1000local.sysCFGreg = sysconfig ;
+    dwt_write32bitreg(SYS_CFG_ID,sysconfig) ;
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_setpanid()
+ *
+ * @brief This is used to set the PAN ID
+ *
+ * input parameters
+ * @param panID - this is the PAN ID
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_setpanid(uint16 panID)
+{
+    // PAN ID is high 16 bits of register
+    dwt_write16bitoffsetreg(PANADR_ID, PANADR_PAN_ID_OFFSET, panID);
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_setaddress16()
+ *
+ * @brief This is used to set 16-bit (short) address
+ *
+ * input parameters
+ * @param shortAddress - this sets the 16 bit short address
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_setaddress16(uint16 shortAddress)
+{
+    // Short address into low 16 bits
+    dwt_write16bitoffsetreg(PANADR_ID, PANADR_SHORT_ADDR_OFFSET, shortAddress);
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_seteui()
+ *
+ * @brief This is used to set the EUI 64-bit (long) address
+ *
+ * input parameters
+ * @param eui64 - this is the pointer to a buffer that contains the 64bit address
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_seteui(uint8 *eui64)
+{
+    dwt_writetodevice(EUI_64_ID, EUI_64_OFFSET, EUI_64_LEN, eui64);
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_geteui()
+ *
+ * @brief This is used to get the EUI 64-bit from the DW1000
+ *
+ * input parameters
+ * @param eui64 - this is the pointer to a buffer that will contain the read 64-bit EUI value
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_geteui(uint8 *eui64)
+{
+    dwt_readfromdevice(EUI_64_ID, EUI_64_OFFSET, EUI_64_LEN, eui64);
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_otpread()
+ *
+ * @brief This is used to read the OTP data from given address into provided array
+ *
+ * input parameters
+ * @param address - this is the OTP address to read from
+ * @param array - this is the pointer to the array into which to read the data
+ * @param length - this is the number of 32 bit words to read (array needs to be at least this length)
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_otpread(uint32 address, uint32 *array, uint8 length)
+{
+    int i;
+
+    _dwt_enableclocks(FORCE_SYS_XTI); // NOTE: Set system clock to XTAL - this is necessary to make sure the values read by _dwt_otpread are reliable
+
+    for(i=0; i<length; i++)
+    {
+        array[i] = _dwt_otpread(address + i) ;
+    }
+
+    _dwt_enableclocks(ENABLE_ALL_SEQ); // Restore system clock to PLL
+
+    return ;
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn _dwt_otpread()
+ *
+ * @brief function to read the OTP memory. Ensure that MR,MRa,MRb are reset to 0.
+ *
+ * input parameters
+ * @param address - address to read at
+ *
+ * output parameters
+ *
+ * returns the 32bit of read data
+ */
+uint32 _dwt_otpread(uint32 address)
+{
+    uint32 ret_data;
+
+    // Write the address
+    dwt_write16bitoffsetreg(OTP_IF_ID, OTP_ADDR, address);
+
+    // Perform OTP Read - Manual read mode has to be set
+    dwt_write8bitoffsetreg(OTP_IF_ID, OTP_CTRL, OTP_CTRL_OTPREAD | OTP_CTRL_OTPRDEN);
+    dwt_write8bitoffsetreg(OTP_IF_ID, OTP_CTRL, 0x00); // OTPREAD is self clearing but OTPRDEN is not
+
+    // Read read data, available 40ns after rising edge of OTP_READ
+    ret_data = dwt_read32bitoffsetreg(OTP_IF_ID, OTP_RDAT);
+
+    // Return the 32bit of read data
+    return ret_data;
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn _dwt_otpsetmrregs()
+ *
+ * @brief Configure the MR registers for initial programming (enable charge pump).
+ * Read margin is used to stress the read back from the
+ * programmed bit. In normal operation this is relaxed.
+ *
+ * input parameters
+ * @param mode - "0" : Reset all to 0x0:           MRA=0x0000, MRB=0x0000, MR=0x0000
+ *               "1" : Set for inital programming: MRA=0x9220, MRB=0x000E, MR=0x1024
+ *               "2" : Set for soak programming:   MRA=0x9220, MRB=0x0003, MR=0x1824
+ *               "3" : High Vpp:                   MRA=0x9220, MRB=0x004E, MR=0x1824
+ *               "4" : Low Read Margin:            MRA=0x0000, MRB=0x0003, MR=0x0000
+ *               "5" : Array Clean:                MRA=0x0049, MRB=0x0003, MR=0x0024
+ *               "4" : Very Low Read Margin:       MRA=0x0000, MRB=0x0003, MR=0x0000
+ *
+ * output parameters
+ *
+ * returns DWT_SUCCESS for success, or DWT_ERROR for error
+ */
+uint32 _dwt_otpsetmrregs(int mode)
+{
+    uint8 rd_buf[4];
+    uint8 wr_buf[4];
+    uint32 mra=0,mrb=0,mr=0;
+
+    // PROGRAMME MRA
+    // Set MRA, MODE_SEL
+    wr_buf[0] = 0x03;
+    dwt_writetodevice(OTP_IF_ID, OTP_CTRL+1,1,wr_buf);
+
+    // Load data
+    switch(mode&0x0f) {
+    case 0x0 :
+        mr =0x0000;
+        mra=0x0000;
+        mrb=0x0000;
+        break;
+    case 0x1 :
+        mr =0x1024;
+        mra=0x9220; // Enable CPP mon
+        mrb=0x000e;
+        break;
+    case 0x2 :
+        mr =0x1824;
+        mra=0x9220;
+        mrb=0x0003;
+        break;
+    case 0x3 :
+        mr =0x1824;
+        mra=0x9220;
+        mrb=0x004e;
+        break;
+    case 0x4 :
+        mr =0x0000;
+        mra=0x0000;
+        mrb=0x0003;
+        break;
+    case 0x5 :
+        mr =0x0024;
+        mra=0x0000;
+        mrb=0x0003;
+        break;
+    default :
+        return DWT_ERROR;
+    }
+
+    wr_buf[0] = mra & 0x00ff;
+    wr_buf[1] = (mra & 0xff00)>>8;
+    dwt_writetodevice(OTP_IF_ID, OTP_WDAT,2,wr_buf);
+
+
+    // Set WRITE_MR
+    wr_buf[0] = 0x08;
+    dwt_writetodevice(OTP_IF_ID, OTP_CTRL,1,wr_buf);
+
+    // Wait?
+
+    // Set Clear Mode sel
+    wr_buf[0] = 0x02;
+    dwt_writetodevice(OTP_IF_ID,OTP_CTRL+1,1,wr_buf);
+
+    // Set AUX update, write MR
+    wr_buf[0] = 0x88;
+    dwt_writetodevice(OTP_IF_ID, OTP_CTRL,1,wr_buf);
+    // Clear write MR
+    wr_buf[0] = 0x80;
+    dwt_writetodevice(OTP_IF_ID, OTP_CTRL,1,wr_buf);
+    // Clear AUX update
+    wr_buf[0] = 0x00;
+    dwt_writetodevice(OTP_IF_ID, OTP_CTRL,1,wr_buf);
+
+    ///////////////////////////////////////////
+    // PROGRAM MRB
+    // Set SLOW, MRB, MODE_SEL
+    wr_buf[0] = 0x05;
+    dwt_writetodevice(OTP_IF_ID,OTP_CTRL+1,1,wr_buf);
+
+    wr_buf[0] = mrb & 0x00ff;
+    wr_buf[1] = (mrb & 0xff00)>>8;
+    dwt_writetodevice(OTP_IF_ID, OTP_WDAT,2,wr_buf);
+
+    // Set WRITE_MR
+    wr_buf[0] = 0x08;
+    dwt_writetodevice(OTP_IF_ID, OTP_CTRL,1,wr_buf);
+
+    // Wait?
+
+    // Set Clear Mode sel
+    wr_buf[0] = 0x04;
+    dwt_writetodevice(OTP_IF_ID,OTP_CTRL+1,1,wr_buf);
+
+    // Set AUX update, write MR
+    wr_buf[0] = 0x88;
+    dwt_writetodevice(OTP_IF_ID, OTP_CTRL,1,wr_buf);
+    // Clear write MR
+    wr_buf[0] = 0x80;
+    dwt_writetodevice(OTP_IF_ID, OTP_CTRL,1,wr_buf);
+    // Clear AUX update
+    wr_buf[0] = 0x00;
+    dwt_writetodevice(OTP_IF_ID, OTP_CTRL,1,wr_buf);
+
+    ///////////////////////////////////////////
+    // PROGRAM MR
+    // Set SLOW, MODE_SEL
+    wr_buf[0] = 0x01;
+    dwt_writetodevice(OTP_IF_ID,OTP_CTRL+1,1,wr_buf);
+    // Load data
+
+    wr_buf[0] = mr & 0x00ff;
+    wr_buf[1] = (mr & 0xff00)>>8;
+    dwt_writetodevice(OTP_IF_ID, OTP_WDAT,2,wr_buf);
+
+    // Set WRITE_MR
+    wr_buf[0] = 0x08;
+    dwt_writetodevice(OTP_IF_ID, OTP_CTRL,1,wr_buf);
+
+    // Wait?
+    deca_sleep(10);
+    // Set Clear Mode sel
+    wr_buf[0] = 0x00;
+    dwt_writetodevice(OTP_IF_ID,OTP_CTRL+1,1,wr_buf);
+
+    // Read confirm mode writes.
+    // Set man override, MRA_SEL
+    wr_buf[0] = OTP_CTRL_OTPRDEN;
+    dwt_writetodevice(OTP_IF_ID, OTP_CTRL,1,wr_buf);
+    wr_buf[0] = 0x02;
+    dwt_writetodevice(OTP_IF_ID,OTP_CTRL+1,1,wr_buf);
+    // MRB_SEL
+    wr_buf[0] = 0x04;
+    dwt_writetodevice(OTP_IF_ID,OTP_CTRL+1,1,wr_buf);
+    deca_sleep(100);
+
+    // Clear mode sel
+    wr_buf[0] = 0x00;
+    dwt_writetodevice(OTP_IF_ID,OTP_CTRL+1,1,wr_buf);
+    // Clear MAN_OVERRIDE
+    wr_buf[0] = 0x00;
+    dwt_writetodevice(OTP_IF_ID, OTP_CTRL,1,wr_buf);
+
+    deca_sleep(10);
+
+    if (((mode&0x0f) == 0x1)||((mode&0x0f) == 0x2))
+    {
+        // Read status register
+        dwt_readfromdevice(OTP_IF_ID, OTP_STAT,1,rd_buf);
+    }
+
+    return DWT_SUCCESS;
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn _dwt_otpprogword32()
+ *
+ * @brief function to program the OTP memory. Ensure that MR,MRa,MRb are reset to 0.
+ * VNM Charge pump needs to be enabled (see _dwt_otpsetmrregs)
+ * Note the address is only 11 bits long.
+ *
+ * input parameters
+ * @param address - address to read at
+ *
+ * output parameters
+ *
+ * returns DWT_SUCCESS for success, or DWT_ERROR for error
+ */
+uint32 _dwt_otpprogword32(uint32 data, uint16 address)
+{
+    uint8 rd_buf[1];
+    uint8 wr_buf[4];
+    uint8 otp_done;
+
+    // Read status register
+    dwt_readfromdevice(OTP_IF_ID, OTP_STAT, 1, rd_buf);
+
+    if((rd_buf[0] & 0x02) != 0x02)
+    {
+        return DWT_ERROR;
+    }
+
+    // Write the data
+    wr_buf[3] = (data>>24) & 0xff;
+    wr_buf[2] = (data>>16) & 0xff;
+    wr_buf[1] = (data>>8) & 0xff;
+    wr_buf[0] = data & 0xff;
+    dwt_writetodevice(OTP_IF_ID, OTP_WDAT, 4, wr_buf);
+
+    // Write the address [10:0]
+    wr_buf[1] = (address>>8) & 0x07;
+    wr_buf[0] = address & 0xff;
+    dwt_writetodevice(OTP_IF_ID, OTP_ADDR, 2, wr_buf);
+
+    // Enable Sequenced programming
+    wr_buf[0] = OTP_CTRL_OTPPROG;
+    dwt_writetodevice(OTP_IF_ID, OTP_CTRL, 1, wr_buf);
+    wr_buf[0] = 0x00; // And clear
+    dwt_writetodevice(OTP_IF_ID, OTP_CTRL, 1, wr_buf);
+
+    // WAIT for status to flag PRGM OK..
+    otp_done = 0;
+    while(otp_done == 0)
+    {
+        deca_sleep(1);
+        dwt_readfromdevice(OTP_IF_ID, OTP_STAT, 1, rd_buf);
+
+        if((rd_buf[0] & 0x01) == 0x01)
+        {
+            otp_done = 1;
+        }
+    }
+
+    return DWT_SUCCESS;
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_otpwriteandverify()
+ *
+ * @brief This is used to program 32-bit value into the DW1000 OTP memory.
+ *
+ * input parameters
+ * @param value - this is the 32-bit value to be programmed into OTP
+ * @param address - this is the 16-bit OTP address into which the 32-bit value is programmed
+ *
+ * output parameters
+ *
+ * returns DWT_SUCCESS for success, or DWT_ERROR for error
+ */
+uint32 dwt_otpwriteandverify(uint32 value, uint16 address)
+{
+    int prog_ok = DWT_SUCCESS;
+    int retry = 0;
+    // Firstly set the system clock to crystal
+    _dwt_enableclocks(FORCE_SYS_XTI); //set system clock to XTI
+
+    //
+    //!!!!!!!!!!!!!! NOTE !!!!!!!!!!!!!!!!!!!!!
+    //Set the supply to 3.7V
+    //
+
+    _dwt_otpsetmrregs(1); // Set mode for programming
+
+    // For each value to program - the readback/check is done couple of times to verify it has programmed successfully
+    while(1)
+    {
+        _dwt_otpprogword32(value, address);
+
+        if(_dwt_otpread(address) == value)
+        {
+            break;
+        }
+        retry++;
+        if(retry==5)
+        {
+            break;
+        }
+    }
+
+    // Even if the above does not exit before retry reaches 5, the programming has probably been successful
+
+    _dwt_otpsetmrregs(4); // Set mode for reading
+
+    if(_dwt_otpread(address) != value) // If this does not pass please check voltage supply on VDDIO
+    {
+        prog_ok = DWT_ERROR;
+    }
+
+    _dwt_otpsetmrregs(0); // Setting OTP mode register for low RM read - resetting the device would be alternative
+
+    return prog_ok;
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn _dwt_aonconfigupload()
+ *
+ * @brief This function uploads always on (AON) configuration, as set in the AON_CFG0_OFFSET register.
+ *
+ * input parameters
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void _dwt_aonconfigupload(void)
+{
+    dwt_write8bitoffsetreg(AON_ID, AON_CTRL_OFFSET, AON_CTRL_UPL_CFG);
+    dwt_write8bitoffsetreg(AON_ID, AON_CTRL_OFFSET, 0x00); // Clear the register
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn _dwt_aonarrayupload()
+ *
+ * @brief This function uploads always on (AON) data array and configuration. Thus if this function is used, then _dwt_aonconfigupload
+ * is not necessary. The DW1000 will go so SLEEP straight after this if the DWT_SLP_EN has been set.
+ *
+ * input parameters
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void _dwt_aonarrayupload(void)
+{
+    dwt_write8bitoffsetreg(AON_ID, AON_CTRL_OFFSET, 0x00); // Clear the register
+    dwt_write8bitoffsetreg(AON_ID, AON_CTRL_OFFSET, AON_CTRL_SAVE);
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_entersleep()
+ *
+ * @brief This function puts the device into deep sleep or sleep. dwt_configuresleep() should be called first
+ * to configure the sleep and on-wake/wake-up parameters
+ *
+ * input parameters
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_entersleep(void)
+{
+    // Copy config to AON - upload the new configuration
+    _dwt_aonarrayupload();
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_configuresleepcnt()
+ *
+ * @brief sets the sleep counter to new value, this function programs the high 16-bits of the 28-bit counter
+ *
+ * NOTE: this function needs to be run before dwt_configuresleep, also the SPI frequency has to be < 3MHz
+ *
+ * input parameters
+ * @param sleepcnt - this it value of the sleep counter to program
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_configuresleepcnt(uint16 sleepcnt)
+{
+    // Force system clock to crystal
+    _dwt_enableclocks(FORCE_SYS_XTI);
+
+    // Reset sleep configuration to make sure we don't accidentally go to sleep
+    dwt_write8bitoffsetreg(AON_ID, AON_CFG0_OFFSET, 0x00); // NB: this write change the default LPCLKDIVA value which is not used anyway.
+    dwt_write8bitoffsetreg(AON_ID, AON_CFG1_OFFSET, 0x00);
+
+    // Disable the sleep counter
+    _dwt_aonconfigupload();
+
+    // Set new value
+    dwt_write16bitoffsetreg(AON_ID, AON_CFG0_OFFSET + AON_CFG0_SLEEP_TIM_OFFSET, sleepcnt);
+    _dwt_aonconfigupload();
+
+    // Enable the sleep counter
+    dwt_write8bitoffsetreg(AON_ID, AON_CFG1_OFFSET, AON_CFG1_SLEEP_CEN);
+    _dwt_aonconfigupload();
+
+    // Put system PLL back on
+    _dwt_enableclocks(ENABLE_ALL_SEQ);
+}
+
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_calibratesleepcnt()
+ *
+ * @brief calibrates the local oscillator as its frequency can vary between 7 and 13kHz depending on temp and voltage
+ *
+ * NOTE: this function needs to be run before dwt_configuresleepcnt, so that we know what the counter units are
+ *
+ * input parameters
+ *
+ * output parameters
+ *
+ * returns the number of XTAL/2 cycles per low-power oscillator cycle. LP OSC frequency = 19.2 MHz/return value
+ */
+uint16 dwt_calibratesleepcnt(void)
+{
+    uint16 result;
+
+    // Enable calibration of the sleep counter
+    dwt_write8bitoffsetreg(AON_ID, AON_CFG1_OFFSET, AON_CFG1_LPOSC_CAL);
+    _dwt_aonconfigupload();
+
+    // Disable calibration of the sleep counter
+    dwt_write8bitoffsetreg(AON_ID, AON_CFG1_OFFSET, 0x00);
+    _dwt_aonconfigupload();
+
+    // Force system clock to crystal
+    _dwt_enableclocks(FORCE_SYS_XTI);
+
+    deca_sleep(1);
+
+    // Read the number of XTAL/2 cycles one LP oscillator cycle took.
+    // Set up address - Read upper byte first
+    dwt_write8bitoffsetreg(AON_ID, AON_ADDR_OFFSET, AON_ADDR_LPOSC_CAL_1);
+
+    // Enable manual override
+    dwt_write8bitoffsetreg(AON_ID, AON_CTRL_OFFSET, AON_CTRL_DCA_ENAB);
+
+    // Read confirm data that was written
+    dwt_write8bitoffsetreg(AON_ID, AON_CTRL_OFFSET, AON_CTRL_DCA_ENAB | AON_CTRL_DCA_READ);
+
+    // Read back byte from AON
+    result = dwt_read8bitoffsetreg(AON_ID, AON_RDAT_OFFSET);
+    result <<= 8;
+
+    // Set up address - Read lower byte
+    dwt_write8bitoffsetreg(AON_ID, AON_ADDR_OFFSET, AON_ADDR_LPOSC_CAL_0);
+
+    // Enable manual override
+    dwt_write8bitoffsetreg(AON_ID, AON_CTRL_OFFSET, AON_CTRL_DCA_ENAB);
+
+    // Read confirm data that was written
+    dwt_write8bitoffsetreg(AON_ID, AON_CTRL_OFFSET, AON_CTRL_DCA_ENAB | AON_CTRL_DCA_READ);
+
+    // Read back byte from AON
+    result |= dwt_read8bitoffsetreg(AON_ID, AON_RDAT_OFFSET);
+
+    // Disable manual override
+    dwt_write8bitoffsetreg(AON_ID, AON_CTRL_OFFSET, 0x00);
+
+    // Put system PLL back on
+    _dwt_enableclocks(ENABLE_ALL_SEQ);
+
+    // Returns the number of XTAL/2 cycles per one LP OSC cycle
+    // This can be converted into LP OSC frequency by 19.2 MHz/result
+    return result;
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_configuresleep()
+ *
+ * @brief configures the device for both DEEP_SLEEP and SLEEP modes, and on-wake mode
+ * i.e. before entering the sleep, the device should be programmed for TX or RX, then upon "waking up" the TX/RX settings
+ * will be preserved and the device can immediately perform the desired action TX/RX
+ *
+ * NOTE: e.g. Tag operation - after deep sleep, the device needs to just load the TX buffer and send the frame
+ *
+ *
+ *      mode: the array and LDE code (OTP/ROM) and LDO tune, and set sleep persist
+ *      DWT_PRESRV_SLEEP 0x0100 - preserve sleep
+ *      DWT_LOADOPSET    0x0080 - load operating parameter set on wakeup
+ *      DWT_CONFIG       0x0040 - download the AON array into the HIF (configuration download)
+ *      DWT_LOADEUI      0x0008
+ *      DWT_GOTORX       0x0002
+ *      DWT_TANDV        0x0001
+ *
+ *      wake: wake up parameters
+ *      DWT_XTAL_EN      0x10 - keep XTAL running during sleep
+ *      DWT_WAKE_SLPCNT  0x8 - wake up after sleep count
+ *      DWT_WAKE_CS      0x4 - wake up on chip select
+ *      DWT_WAKE_WK      0x2 - wake up on WAKEUP PIN
+ *      DWT_SLP_EN       0x1 - enable sleep/deep sleep functionality
+ *
+ * input parameters
+ * @param mode - config on-wake parameters
+ * @param wake - config wake up parameters
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_configuresleep(uint16 mode, uint8 wake)
+{
+    // Add predefined sleep settings before writing the mode
+    mode |= dw1000local.sleep_mode;
+    dwt_write16bitoffsetreg(AON_ID, AON_WCFG_OFFSET, mode);
+
+    dwt_write8bitoffsetreg(AON_ID, AON_CFG0_OFFSET, wake);
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_entersleepaftertx(int enable)
+ *
+ * @brief sets the auto TX to sleep bit. This means that after a frame
+ * transmission the device will enter deep sleep mode. The dwt_configuresleep() function
+ * needs to be called before this to configure the on-wake settings
+ *
+ * NOTE: the IRQ line has to be low/inactive (i.e. no pending events)
+ *
+ * input parameters
+ * @param enable - 1 to configure the device to enter deep sleep after TX, 0 - disables the configuration
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_entersleepaftertx(int enable)
+{
+    uint32 reg = dwt_read32bitoffsetreg(PMSC_ID, PMSC_CTRL1_OFFSET);
+    // Set the auto TX -> sleep bit
+    if(enable)
+    {
+        reg |= PMSC_CTRL1_ATXSLP;
+    }
+    else
+    {
+        reg &= ~(PMSC_CTRL1_ATXSLP);
+    }
+    dwt_write32bitoffsetreg(PMSC_ID, PMSC_CTRL1_OFFSET, reg);
+}
+
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_spicswakeup()
+ *
+ * @brief wake up the device from sleep mode using the SPI read,
+ * the device will wake up on chip select line going low if the line is held low for at least 500us.
+ * To define the length depending on the time one wants to hold
+ * the chip select line low, use the following formula:
+ *
+ *      length (bytes) = time (s) * byte_rate (Hz)
+ *
+ * where fastest byte_rate is spi_rate (Hz) / 8 if the SPI is sending the bytes back-to-back.
+ * To save time and power, a system designer could determine byte_rate value more precisely.
+ *
+ * NOTE: Alternatively the device can be waken up with WAKE_UP pin if configured for that operation
+ *
+ * input parameters
+ * @param buff   - this is a pointer to the dummy buffer which will be used in the SPI read transaction used for the WAKE UP of the device
+ * @param length - this is the length of the dummy buffer
+ *
+ * output parameters
+ *
+ * returns DWT_SUCCESS for success, or DWT_ERROR for error
+ */
+int dwt_spicswakeup(uint8 *buff, uint16 length)
+{
+    if(dwt_readdevid() != DWT_DEVICE_ID) // Device was in deep sleep (the first read fails)
+    {
+        // Need to keep chip select line low for at least 500us
+        dwt_readfromdevice(0x0, 0x0, length, buff); // Do a long read to wake up the chip (hold the chip select low)
+
+        // Need 5ms for XTAL to start and stabilise (could wait for PLL lock IRQ status bit !!!)
+        // NOTE: Polling of the STATUS register is not possible unless frequency is < 3MHz
+        deca_sleep(5);
+    }
+    else
+    {
+        return DWT_SUCCESS;
+    }
+    // DEBUG - check if still in sleep mode
+    if(dwt_readdevid() != DWT_DEVICE_ID)
+    {
+        return DWT_ERROR;
+    }
+
+    return DWT_SUCCESS;
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn _dwt_configlde()
+ *
+ * @brief configure LDE algorithm parameters
+ *
+ * input parameters
+ * @param prf   -   this is the PRF index (0 or 1) 0 corresponds to 16 and 1 to 64 PRF
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void _dwt_configlde(int prfIndex)
+{
+    dwt_write8bitoffsetreg(LDE_IF_ID, LDE_CFG1_OFFSET, LDE_PARAM1); // 8-bit configuration register
+
+    if(prfIndex)
+    {
+        dwt_write16bitoffsetreg( LDE_IF_ID, LDE_CFG2_OFFSET, (uint16) LDE_PARAM3_64); // 16-bit LDE configuration tuning register
+    }
+    else
+    {
+        dwt_write16bitoffsetreg( LDE_IF_ID, LDE_CFG2_OFFSET, (uint16) LDE_PARAM3_16);
+    }
+}
+
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn _dwt_loaducodefromrom()
+ *
+ * @brief  load ucode from OTP MEMORY or ROM
+ *
+ * input parameters
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void _dwt_loaducodefromrom(void)
+{
+    // Set up clocks
+    _dwt_enableclocks(FORCE_LDE);
+
+    // Kick off the LDE load
+    dwt_write16bitoffsetreg(OTP_IF_ID, OTP_CTRL, OTP_CTRL_LDELOAD); // Set load LDE kick bit
+
+    deca_sleep(1); // Allow time for code to upload (should take up to 120 us)
+
+    // Default clocks (ENABLE_ALL_SEQ)
+    _dwt_enableclocks(ENABLE_ALL_SEQ); // Enable clocks for sequencing
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_loadopsettabfromotp()
+ *
+ * @brief This is used to select which Operational Parameter Set table to load from OTP memory
+ *
+ * input parameters
+ * @param ops_sel - Operational Parameter Set table to load:
+ *                  DWT_OPSET_64LEN = 0x0 - load the operational parameter set table for 64 length preamble configuration
+ *                  DWT_OPSET_TIGHT = 0x1 - load the operational parameter set table for tight xtal offsets (<1ppm)
+ *                  DWT_OPSET_DEFLT = 0x2 - load the default operational parameter set table (this is loaded from reset)
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_loadopsettabfromotp(uint8 ops_sel)
+{
+    uint16 reg = ((ops_sel << OTP_SF_OPS_SEL_SHFT) & OTP_SF_OPS_SEL_MASK) | OTP_SF_OPS_KICK; // Select defined OPS table and trigger its loading
+
+    // Set up clocks
+    _dwt_enableclocks(FORCE_LDE);
+
+    dwt_write16bitoffsetreg(OTP_IF_ID, OTP_SF, reg);
+
+    // Default clocks (ENABLE_ALL_SEQ)
+    _dwt_enableclocks(ENABLE_ALL_SEQ); // Enable clocks for sequencing
+
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_setsmarttxpower()
+ *
+ * @brief This call enables or disables the smart TX power feature.
+ *
+ * input parameters
+ * @param enable - this enables or disables the TX smart power (1 = enable, 0 = disable)
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_setsmarttxpower(int enable)
+{
+    // Config system register
+    dw1000local.sysCFGreg = dwt_read32bitreg(SYS_CFG_ID) ; // Read sysconfig register
+
+    // Disable smart power configuration
+    if(enable)
+    {
+        dw1000local.sysCFGreg &= ~(SYS_CFG_DIS_STXP) ;
+    }
+    else
+    {
+        dw1000local.sysCFGreg |= SYS_CFG_DIS_STXP ;
+    }
+
+    dwt_write32bitreg(SYS_CFG_ID,dw1000local.sysCFGreg) ;
+}
+
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_enableautoack()
+ *
+ * @brief This call enables the auto-ACK feature. If the responseDelayTime (parameter) is 0, the ACK will be sent a.s.a.p.
+ * otherwise it will be sent with a programmed delay (in symbols), max is 255.
+ * NOTE: needs to have frame filtering enabled as well
+ *
+ * input parameters
+ * @param responseDelayTime - if non-zero the ACK is sent after this delay, max is 255.
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_enableautoack(uint8 responseDelayTime)
+{
+    // Set auto ACK reply delay
+    dwt_write8bitoffsetreg(ACK_RESP_T_ID, ACK_RESP_T_ACK_TIM_OFFSET, responseDelayTime); // In symbols
+    // Enable auto ACK
+    dw1000local.sysCFGreg |= SYS_CFG_AUTOACK;
+    dwt_write32bitreg(SYS_CFG_ID,dw1000local.sysCFGreg) ;
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_setdblrxbuffmode()
+ *
+ * @brief This call enables the double receive buffer mode
+ *
+ * input parameters
+ * @param enable - 1 to enable, 0 to disable the double buffer mode
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_setdblrxbuffmode(int enable)
+{
+    if(enable)
+    {
+        // Enable double RX buffer mode
+        dw1000local.sysCFGreg &= ~SYS_CFG_DIS_DRXB;
+        dw1000local.dblbuffon = 1;
+    }
+    else
+    {
+        // Disable double RX buffer mode
+        dw1000local.sysCFGreg |= SYS_CFG_DIS_DRXB;
+        dw1000local.dblbuffon = 0;
+    }
+
+    dwt_write32bitreg(SYS_CFG_ID,dw1000local.sysCFGreg) ;
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_setrxaftertxdelay()
+ *
+ * @brief This sets the receiver turn on delay time after a transmission of a frame
+ *
+ * input parameters
+ * @param rxDelayTime - (20 bits) - the delay is in UWB microseconds
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_setrxaftertxdelay(uint32 rxDelayTime)
+{
+    uint32 val = dwt_read32bitreg(ACK_RESP_T_ID) ; // Read ACK_RESP_T_ID register
+
+    val &= ~(ACK_RESP_T_W4R_TIM_MASK) ; // Clear the timer (19:0)
+
+    val |= (rxDelayTime & ACK_RESP_T_W4R_TIM_MASK) ; // In UWB microseconds (e.g. turn the receiver on 20uus after TX)
+
+    dwt_write32bitreg(ACK_RESP_T_ID, val) ;
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_setcallbacks()
+ *
+ * @brief This function is used to register the different callbacks called when one of the corresponding event occurs.
+ *
+ * NOTE: Callbacks can be undefined (set to NULL). In this case, dwt_isr() will process the event as usual but the 'null'
+ * callback will not be called.
+ *
+ * input parameters
+ * @param cbTxDone - the pointer to the TX confirmation event callback function
+ * @param cbRxOk - the pointer to the RX good frame event callback function
+ * @param cbRxTo - the pointer to the RX timeout events callback function
+ * @param cbRxErr - the pointer to the RX error events callback function
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_setcallbacks(dwt_cb_t cbTxDone, dwt_cb_t cbRxOk, dwt_cb_t cbRxTo, dwt_cb_t cbRxErr)
+{
+    dw1000local.cbTxDone = cbTxDone;
+    dw1000local.cbRxOk = cbRxOk;
+    dw1000local.cbRxTo = cbRxTo;
+    dw1000local.cbRxErr = cbRxErr;
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_checkirq()
+ *
+ * @brief This function checks if the IRQ line is active - this is used instead of interrupt handler
+ *
+ * input parameters
+ *
+ * output parameters
+ *
+ * return value is 1 if the IRQS bit is set and 0 otherwise
+ */
+uint8 dwt_checkirq(void)
+{
+    return (dwt_read8bitoffsetreg(SYS_STATUS_ID, SYS_STATUS_OFFSET) & SYS_STATUS_IRQS); // Reading the lower byte only is enough for this operation
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_isr()
+ *
+ * @brief This is the DW1000's general Interrupt Service Routine. It will process/report the following events:
+ *          - RXFCG (through cbRxOk callback)
+ *          - TXFRS (through cbTxDone callback)
+ *          - RXRFTO/RXPTO (through cbRxTo callback)
+ *          - RXPHE/RXFCE/RXRFSL/RXSFDTO/AFFREJ/LDEERR (through cbRxTo cbRxErr)
+ *        For all events, corresponding interrupts are cleared and necessary resets are performed. In addition, in the RXFCG case,
+ *        received frame information and frame control are read before calling the callback. If double buffering is activated, it
+ *        will also toggle between reception buffers once the reception callback processing has ended.
+ *
+ *        /!\ This version of the ISR supports double buffering but does not support automatic RX re-enabling!
+ *
+ * NOTE:  In PC based system using (Cheetah or ARM) USB to SPI converter there can be no interrupts, however we still need something
+ *        to take the place of it and operate in a polled way. In an embedded system this function should be configured to be triggered
+ *        on any of the interrupts described above.
+
+ * input parameters
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_isr(void)
+{
+    uint32 status = dw1000local.cbData.status = dwt_read32bitreg(SYS_STATUS_ID); // Read status register low 32bits
+
+    // Handle RX good frame event
+    if(status & SYS_STATUS_RXFCG)
+    {
+        uint16 finfo16;
+        uint16 len;
+
+        dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_GOOD); // Clear all receive status bits
+
+        dw1000local.cbData.rx_flags = 0;
+
+        // Read frame info - Only the first two bytes of the register are used here.
+        finfo16 = dwt_read16bitoffsetreg(RX_FINFO_ID, RX_FINFO_OFFSET);
+
+        // Report frame length - Standard frame length up to 127, extended frame length up to 1023 bytes
+        len = finfo16 & RX_FINFO_RXFL_MASK_1023;
+        if(dw1000local.longFrames == 0)
+        {
+            len &= RX_FINFO_RXFLEN_MASK;
+        }
+        dw1000local.cbData.datalength = len;
+
+        // Report ranging bit
+        if(finfo16 & RX_FINFO_RNG)
+        {
+            dw1000local.cbData.rx_flags |= DWT_CB_DATA_RX_FLAG_RNG;
+        }
+
+        // Report frame control - First bytes of the received frame.
+        dwt_readfromdevice(RX_BUFFER_ID, 0, FCTRL_LEN_MAX, dw1000local.cbData.fctrl);
+
+        // Because of a previous frame not being received properly, AAT bit can be set upon the proper reception of a frame not requesting for
+        // acknowledgement (ACK frame is not actually sent though). If the AAT bit is set, check ACK request bit in frame control to confirm (this
+        // implementation works only for IEEE802.15.4-2011 compliant frames).
+        // This issue is not documented at the time of writing this code. It should be in next release of DW1000 User Manual (v2.09, from July 2016).
+        if((status & SYS_STATUS_AAT) && ((dw1000local.cbData.fctrl[0] & FCTRL_ACK_REQ_MASK) == 0))
+        {
+            dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_AAT); // Clear AAT status bit in register
+            dw1000local.cbData.status &= ~SYS_STATUS_AAT; // Clear AAT status bit in callback data register copy
+            dw1000local.wait4resp = 0;
+        }
+
+        // Call the corresponding callback if present
+        if(dw1000local.cbRxOk != NULL)
+        {
+            dw1000local.cbRxOk(&dw1000local.cbData);
+        }
+
+        if (dw1000local.dblbuffon)
+        {
+            // Toggle the Host side Receive Buffer Pointer
+            dwt_write8bitoffsetreg(SYS_CTRL_ID, SYS_CTRL_HRBT_OFFSET, 1);
+        }
+    }
+
+    // Handle TX confirmation event
+    if(status & SYS_STATUS_TXFRS)
+    {
+        dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_TX); // Clear TX event bits
+
+        // In the case where this TXFRS interrupt is due to the automatic transmission of an ACK solicited by a response (with ACK request bit set)
+        // that we receive through using wait4resp to a previous TX (and assuming that the IRQ processing of that TX has already been handled), then
+        // we need to handle the IC issue which turns on the RX again in this situation (i.e. because it is wrongly applying the wait4resp after the
+        // ACK TX).
+        // See section "Transmit and automatically wait for response" in DW1000 User Manual
+        if((status & SYS_STATUS_AAT) && dw1000local.wait4resp)
+        {
+            dwt_forcetrxoff(); // Turn the RX off
+            dwt_rxreset(); // Reset in case we were late and a frame was already being received
+        }
+
+        // Call the corresponding callback if present
+        if(dw1000local.cbTxDone != NULL)
+        {
+            dw1000local.cbTxDone(&dw1000local.cbData);
+        }
+    }
+
+    // Handle frame reception/preamble detect timeout events
+    if(status & SYS_STATUS_ALL_RX_TO)
+    {
+        dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXRFTO); // Clear RX timeout event bits
+
+        dw1000local.wait4resp = 0;
+
+        // Because of an issue with receiver restart after error conditions, an RX reset must be applied after any error or timeout event to ensure
+        // the next good frame's timestamp is computed correctly.
+        // See section "RX Message timestamp" in DW1000 User Manual.
+        dwt_forcetrxoff();
+        dwt_rxreset();
+
+        // Call the corresponding callback if present
+        if(dw1000local.cbRxTo != NULL)
+        {
+            dw1000local.cbRxTo(&dw1000local.cbData);
+        }
+    }
+
+    // Handle RX errors events
+    if(status & SYS_STATUS_ALL_RX_ERR)
+    {
+        dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR); // Clear RX error event bits
+
+        dw1000local.wait4resp = 0;
+
+        // Because of an issue with receiver restart after error conditions, an RX reset must be applied after any error or timeout event to ensure
+        // the next good frame's timestamp is computed correctly.
+        // See section "RX Message timestamp" in DW1000 User Manual.
+        dwt_forcetrxoff();
+        dwt_rxreset();
+
+        // Call the corresponding callback if present
+        if(dw1000local.cbRxErr != NULL)
+        {
+            dw1000local.cbRxErr(&dw1000local.cbData);
+        }
+    }
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_isr_lplisten()
+ *
+ * @brief This is the DW1000's Interrupt Service Routine to use when low-power listening scheme is implemented. It will
+ *        only process/report the RXFCG event (through cbRxOk callback).
+ *        It clears RXFCG interrupt and reads received frame information and frame control before calling the callback.
+ *
+ *        /!\ This version of the ISR is designed for single buffering case only!
+ *
+ * input parameters
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_lowpowerlistenisr(void)
+{
+    uint32 status = dw1000local.cbData.status = dwt_read32bitreg(SYS_STATUS_ID); // Read status register low 32bits
+    uint16 finfo16;
+    uint16 len;
+
+    // The only interrupt handled when in low-power listening mode is RX good frame so proceed directly to the handling of the received frame.
+
+    // Deactivate low-power listening before clearing the interrupt. If not, the DW1000 will go back to sleep as soon as the interrupt is cleared.
+    dwt_setlowpowerlistening(0);
+
+    dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_GOOD); // Clear all receive status bits
+
+    dw1000local.cbData.rx_flags = 0;
+
+    // Read frame info - Only the first two bytes of the register are used here.
+    finfo16 = dwt_read16bitoffsetreg(RX_FINFO_ID, 0);
+
+    // Report frame length - Standard frame length up to 127, extended frame length up to 1023 bytes
+    len = finfo16 & RX_FINFO_RXFL_MASK_1023;
+    if(dw1000local.longFrames == 0)
+    {
+        len &= RX_FINFO_RXFLEN_MASK;
+    }
+    dw1000local.cbData.datalength = len;
+
+    // Report ranging bit
+    if(finfo16 & RX_FINFO_RNG)
+    {
+        dw1000local.cbData.rx_flags |= DWT_CB_DATA_RX_FLAG_RNG;
+    }
+
+    // Report frame control - First bytes of the received frame.
+    dwt_readfromdevice(RX_BUFFER_ID, 0, FCTRL_LEN_MAX, dw1000local.cbData.fctrl);
+
+    // Because of a previous frame not being received properly, AAT bit can be set upon the proper reception of a frame not requesting for
+    // acknowledgement (ACK frame is not actually sent though). If the AAT bit is set, check ACK request bit in frame control to confirm (this
+    // implementation works only for IEEE802.15.4-2011 compliant frames).
+    // This issue is not documented at the time of writing this code. It should be in next release of DW1000 User Manual (v2.09, from July 2016).
+    if((status & SYS_STATUS_AAT) && ((dw1000local.cbData.fctrl[0] & FCTRL_ACK_REQ_MASK) == 0))
+    {
+        dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_AAT); // Clear AAT status bit in register
+        dw1000local.cbData.status &= ~SYS_STATUS_AAT; // Clear AAT status bit in callback data register copy
+        dw1000local.wait4resp = 0;
+    }
+
+    // Call the corresponding callback if present
+    if(dw1000local.cbRxOk != NULL)
+    {
+        dw1000local.cbRxOk(&dw1000local.cbData);
+    }
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_setleds()
+ *
+ * @brief This is used to set up Tx/Rx GPIOs which could be used to control LEDs
+ * Note: not completely IC dependent, also needs board with LEDS fitted on right I/O lines
+ *       this function enables GPIOs 2 and 3 which are connected to LED3 and LED4 on EVB1000
+ *
+ * input parameters
+ * @param mode - this is a bit field interpreted as follows:
+ *          - bit 0: 1 to enable LEDs, 0 to disable them
+ *          - bit 1: 1 to make LEDs blink once on init. Only valid if bit 0 is set (enable LEDs)
+ *          - bit 2 to 7: reserved
+ *
+ * output parameters none
+ *
+ * no return value
+ */
+void dwt_setleds(uint8 mode)
+{
+    uint32 reg;
+
+    if (mode & DWT_LEDS_ENABLE)
+    {
+        // Set up MFIO for LED output.
+        reg = dwt_read32bitoffsetreg(GPIO_CTRL_ID, GPIO_MODE_OFFSET);
+        reg &= ~(GPIO_MSGP2_MASK | GPIO_MSGP3_MASK);
+        reg |= (GPIO_PIN2_RXLED | GPIO_PIN3_TXLED);
+        dwt_write32bitoffsetreg(GPIO_CTRL_ID, GPIO_MODE_OFFSET, reg);
+
+        // Enable LP Oscillator to run from counter and turn on de-bounce clock.
+        reg = dwt_read32bitoffsetreg(PMSC_ID, PMSC_CTRL0_OFFSET);
+        reg |= (PMSC_CTRL0_GPDCE | PMSC_CTRL0_KHZCLEN);
+        dwt_write32bitoffsetreg(PMSC_ID, PMSC_CTRL0_OFFSET, reg);
+
+        // Enable LEDs to blink and set default blink time.
+        reg = PMSC_LEDC_BLNKEN | PMSC_LEDC_BLINK_TIME_DEF;
+        // Make LEDs blink once if requested.
+        if (mode & DWT_LEDS_INIT_BLINK)
+        {
+            reg |= PMSC_LEDC_BLINK_NOW_ALL;
+        }
+        dwt_write32bitoffsetreg(PMSC_ID, PMSC_LEDC_OFFSET, reg);
+        // Clear force blink bits if needed.
+        if(mode & DWT_LEDS_INIT_BLINK)
+        {
+            reg &= ~PMSC_LEDC_BLINK_NOW_ALL;
+            dwt_write32bitoffsetreg(PMSC_ID, PMSC_LEDC_OFFSET, reg);
+        }
+    }
+    else
+    {
+        // Clear the GPIO bits that are used for LED control.
+        reg = dwt_read32bitoffsetreg(GPIO_CTRL_ID, GPIO_MODE_OFFSET);
+        reg &= ~(GPIO_MSGP2_MASK | GPIO_MSGP3_MASK);
+        dwt_write32bitoffsetreg(GPIO_CTRL_ID, GPIO_MODE_OFFSET, reg);
+    }
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn _dwt_enableclocks()
+ *
+ * @brief function to enable/disable clocks to particular digital blocks/system
+ *
+ * input parameters
+ * @param clocks - set of clocks to enable/disable
+ *
+ * output parameters none
+ *
+ * no return value
+ */
+void _dwt_enableclocks(int clocks)
+{
+    uint8 reg[2];
+
+    dwt_readfromdevice(PMSC_ID, PMSC_CTRL0_OFFSET, 2, reg);
+    switch(clocks)
+    {
+        case ENABLE_ALL_SEQ:
+        {
+            reg[0] = 0x00 ;
+            reg[1] = reg[1] & 0xfe;
+        }
+        break;
+        case FORCE_SYS_XTI:
+        {
+            // System and RX
+            reg[0] = 0x01 | (reg[0] & 0xfc);
+        }
+        break;
+        case FORCE_SYS_PLL:
+        {
+            // System
+            reg[0] = 0x02 | (reg[0] & 0xfc);
+        }
+        break;
+        case READ_ACC_ON:
+        {
+            reg[0] = 0x48 | (reg[0] & 0xb3);
+            reg[1] = 0x80 | reg[1];
+        }
+        break;
+        case READ_ACC_OFF:
+        {
+            reg[0] = reg[0] & 0xb3;
+            reg[1] = 0x7f & reg[1];
+        }
+        break;
+        case FORCE_OTP_ON:
+        {
+            reg[1] = 0x02 | reg[1];
+        }
+        break;
+        case FORCE_OTP_OFF:
+        {
+            reg[1] = reg[1] & 0xfd;
+        }
+        break;
+        case FORCE_TX_PLL:
+        {
+            reg[0] = 0x20 | (reg[0] & 0xcf);
+        }
+        break;
+        case FORCE_LDE:
+        {
+            reg[0] = 0x01;
+            reg[1] = 0x03;
+        }
+        break;
+        default:
+        break;
+    }
+
+
+    // Need to write lower byte separately before setting the higher byte(s)
+    dwt_writetodevice(PMSC_ID, PMSC_CTRL0_OFFSET, 1, &reg[0]);
+    dwt_writetodevice(PMSC_ID, 0x1, 1, &reg[1]);
+
+} // end _dwt_enableclocks()
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn _dwt_disablesequencing()
+ *
+ * @brief This function disables the TX blocks sequencing, it disables PMSC control of RF blocks, system clock is also set to XTAL
+ *
+ * input parameters none
+ *
+ * output parameters none
+ *
+ * no return value
+ */
+void _dwt_disablesequencing(void) // Disable sequencing and go to state "INIT"
+{
+    _dwt_enableclocks(FORCE_SYS_XTI); // Set system clock to XTI
+
+    dwt_write16bitoffsetreg(PMSC_ID, PMSC_CTRL1_OFFSET, PMSC_CTRL1_PKTSEQ_DISABLE); // Disable PMSC ctrl of RF and RX clk blocks
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_setdelayedtrxtime()
+ *
+ * @brief This API function configures the delayed transmit time or the delayed RX on time
+ *
+ * input parameters
+ * @param starttime - the TX/RX start time (the 32 bits should be the high 32 bits of the system time at which to send the message,
+ * or at which to turn on the receiver)
+ *
+ * output parameters none
+ *
+ * no return value
+ */
+void dwt_setdelayedtrxtime(uint32 starttime)
+{
+    dwt_write32bitoffsetreg(DX_TIME_ID, 1, starttime); // Write at offset 1 as the lower 9 bits of this register are ignored
+
+} // end dwt_setdelayedtrxtime()
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_starttx()
+ *
+ * @brief This call initiates the transmission, input parameter indicates which TX mode is used see below
+ *
+ * input parameters:
+ * @param mode - if 0 immediate TX (no response expected)
+ *               if 1 delayed TX (no response expected)
+ *               if 2 immediate TX (response expected - so the receiver will be automatically turned on after TX is done)
+ *               if 3 delayed TX (response expected - so the receiver will be automatically turned on after TX is done)
+ *
+ * output parameters
+ *
+ * returns DWT_SUCCESS for success, or DWT_ERROR for error (e.g. a delayed transmission will fail if the delayed time has passed)
+ */
+int dwt_starttx(uint8 mode)
+{
+    int retval = DWT_SUCCESS ;
+    uint8 temp  = 0x00;
+    uint16 checkTxOK = 0 ;
+
+    if(mode & DWT_RESPONSE_EXPECTED)
+    {
+        temp = (uint8)SYS_CTRL_WAIT4RESP ; // Set wait4response bit
+        dwt_write8bitoffsetreg(SYS_CTRL_ID, SYS_CTRL_OFFSET, temp);
+        dw1000local.wait4resp = 1;
+    }
+
+    if (mode & DWT_START_TX_DELAYED)
+    {
+        // Both SYS_CTRL_TXSTRT and SYS_CTRL_TXDLYS to correctly enable TX
+        temp |= (uint8)(SYS_CTRL_TXDLYS | SYS_CTRL_TXSTRT) ;
+        dwt_write8bitoffsetreg(SYS_CTRL_ID, SYS_CTRL_OFFSET, temp);
+        checkTxOK = dwt_read16bitoffsetreg(SYS_STATUS_ID, 3); // Read at offset 3 to get the upper 2 bytes out of 5
+        if ((checkTxOK & SYS_STATUS_TXERR) == 0) // Transmit Delayed Send set over Half a Period away or Power Up error (there is enough time to send but not to power up individual blocks).
+        {
+            retval = DWT_SUCCESS ; // All okay
+        }
+        else
+        {
+            // I am taking DSHP set to Indicate that the TXDLYS was set too late for the specified DX_TIME.
+            // Remedial Action - (a) cancel delayed send
+            temp = (uint8)SYS_CTRL_TRXOFF; // This assumes the bit is in the lowest byte
+            dwt_write8bitoffsetreg(SYS_CTRL_ID, SYS_CTRL_OFFSET, temp);
+            // Note event Delayed TX Time too Late
+            // Could fall through to start a normal send (below) just sending late.....
+            // ... instead return and assume return value of 1 will be used to detect and recover from the issue.
+            dw1000local.wait4resp = 0;
+            retval = DWT_ERROR ; // Failed !
+        }
+    }
+    else
+    {
+        temp |= (uint8)SYS_CTRL_TXSTRT ;
+        dwt_write8bitoffsetreg(SYS_CTRL_ID, SYS_CTRL_OFFSET, temp);
+    }
+
+    return retval;
+
+} // end dwt_starttx()
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_forcetrxoff()
+ *
+ * @brief This is used to turn off the transceiver
+ *
+ * input parameters
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_forcetrxoff(void)
+{
+    decaIrqStatus_t stat ;
+    uint32 mask;
+
+    mask = dwt_read32bitreg(SYS_MASK_ID) ; // Read set interrupt mask
+
+    // Need to beware of interrupts occurring in the middle of following read modify write cycle
+    // We can disable the radio, but before the status is cleared an interrupt can be set (e.g. the
+    // event has just happened before the radio was disabled)
+    // thus we need to disable interrupt during this operation
+    stat = decamutexon() ;
+
+    dwt_write32bitreg(SYS_MASK_ID, 0) ; // Clear interrupt mask - so we don't get any unwanted events
+
+    dwt_write8bitoffsetreg(SYS_CTRL_ID, SYS_CTRL_OFFSET, (uint8)SYS_CTRL_TRXOFF) ; // Disable the radio
+
+    // Forcing Transceiver off - so we do not want to see any new events that may have happened
+    dwt_write32bitreg(SYS_STATUS_ID, (SYS_STATUS_ALL_TX | SYS_STATUS_ALL_RX_ERR | SYS_STATUS_ALL_RX_TO | SYS_STATUS_ALL_RX_GOOD));
+
+    dwt_syncrxbufptrs();
+
+    dwt_write32bitreg(SYS_MASK_ID, mask) ; // Set interrupt mask to what it was
+
+    // Enable/restore interrupts again...
+    decamutexoff(stat) ;
+    dw1000local.wait4resp = 0;
+
+} // end deviceforcetrxoff()
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_syncrxbufptrs()
+ *
+ * @brief this function synchronizes rx buffer pointers
+ * need to make sure that the host/IC buffer pointers are aligned before starting RX
+ *
+ * input parameters:
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_syncrxbufptrs(void)
+{
+    uint8  buff ;
+    // Need to make sure that the host/IC buffer pointers are aligned before starting RX
+    buff = dwt_read8bitoffsetreg(SYS_STATUS_ID, 3); // Read 1 byte at offset 3 to get the 4th byte out of 5
+
+    if((buff & (SYS_STATUS_ICRBP >> 24)) !=     // IC side Receive Buffer Pointer
+       ((buff & (SYS_STATUS_HSRBP>>24)) << 1) ) // Host Side Receive Buffer Pointer
+    {
+        dwt_write8bitoffsetreg(SYS_CTRL_ID, SYS_CTRL_HRBT_OFFSET , 0x01) ; // We need to swap RX buffer status reg (write one to toggle internally)
+    }
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_setsniffmode()
+ *
+ * @brief enable/disable and configure SNIFF mode.
+ *
+ * SNIFF mode is a low-power reception mode where the receiver is sequenced on and off instead of being on all the time.
+ * The time spent in each state (on/off) is specified through the parameters below.
+ * See DW1000 User Manual section 4.5 "Low-Power SNIFF mode" for more details.
+ *
+ * input parameters:
+ * @param enable - 1 to enable SNIFF mode, 0 to disable. When 0, all other parameters are not taken into account.
+ * @param timeOn - duration of receiver ON phase, expressed in multiples of PAC size. The counter automatically adds 1 PAC
+ *                 size to the value set. Min value that can be set is 1 (i.e. an ON time of 2 PAC size), max value is 15.
+ * @param timeOff - duration of receiver OFF phase, expressed in multiples of 128/125 µs (~1 µs). Max value is 255.
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_setsniffmode(int enable, uint8 timeOn, uint8 timeOff)
+{
+    uint32 pmsc_reg;
+    if (enable)
+    {
+        /* Configure ON/OFF times and enable PLL2 on/off sequencing by SNIFF mode. */
+        uint16 sniff_reg = ((timeOff << 8) | timeOn) & RX_SNIFF_MASK;
+        dwt_write16bitoffsetreg(RX_SNIFF_ID, RX_SNIFF_OFFSET, sniff_reg);
+        pmsc_reg = dwt_read32bitoffsetreg(PMSC_ID, PMSC_CTRL0_OFFSET);
+        pmsc_reg |= PMSC_CTRL0_PLL2_SEQ_EN;
+        dwt_write32bitoffsetreg(PMSC_ID, PMSC_CTRL0_OFFSET, pmsc_reg);
+    }
+    else
+    {
+        /* Clear ON/OFF times and disable PLL2 on/off sequencing by SNIFF mode. */
+        dwt_write16bitoffsetreg(RX_SNIFF_ID, RX_SNIFF_OFFSET, 0x0000);
+        pmsc_reg = dwt_read32bitoffsetreg(PMSC_ID, PMSC_CTRL0_OFFSET);
+        pmsc_reg &= ~PMSC_CTRL0_PLL2_SEQ_EN;
+        dwt_write32bitoffsetreg(PMSC_ID, PMSC_CTRL0_OFFSET, pmsc_reg);
+    }
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_setlowpowerlistening()
+ *
+ * @brief enable/disable low-power listening mode.
+ *
+ * Low-power listening is a feature whereby the DW1000 is predominantly in the SLEEP state but wakes periodically, (after
+ * this "long sleep"), for a very short time to sample the air for a preamble sequence. This preamble sampling "listening"
+ * phase is actually two reception phases separated by a "short sleep" time. See DW1000 User Manual section "Low-Power
+ * Listening" for more details.
+ *
+ * NOTE: Before enabling low-power listening, the following functions have to be called to fully configure it:
+ *           - dwt_configuresleep() to configure long sleep phase. "mode" parameter should at least have DWT_PRESRV_SLEEP,
+ *             DWT_CONFIG and DWT_RX_EN set and "wake" parameter should at least have both DWT_WAKE_SLPCNT and DWT_SLP_EN set.
+ *           - dwt_calibratesleepcnt() and dwt_configuresleepcnt() to define the "long sleep" phase duration.
+ *           - dwt_setsnoozetime() to define the "short sleep" phase duration.
+ *           - dwt_setpreambledetecttimeout() to define the reception phases duration.
+ *           - dwt_setinterrupt() to activate RX good frame interrupt (DWT_INT_RFCG) only.
+ *       When configured, low-power listening mode can be triggered either by putting the DW1000 to sleep (using
+ *       dwt_entersleep()) or by activating reception (using dwt_rxenable()).
+ *
+ *       Please refer to the low-power listening examples (examples 8a/8b accompanying the API distribution on Decawave's
+ *       website). They form a working example code that shows how to use low-power listening correctly.
+ *
+ * input parameters:
+ * @param enable - 1 to enable low-power listening, 0 to disable.
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_setlowpowerlistening(int enable)
+{
+    uint32 pmsc_reg = dwt_read32bitoffsetreg(PMSC_ID, PMSC_CTRL1_OFFSET);
+    if (enable)
+    {
+        /* Configure RX to sleep and snooze features. */
+        pmsc_reg |= (PMSC_CTRL1_ARXSLP | PMSC_CTRL1_SNOZE);
+    }
+    else
+    {
+        /* Reset RX to sleep and snooze features. */
+        pmsc_reg &= ~(PMSC_CTRL1_ARXSLP | PMSC_CTRL1_SNOZE);
+    }
+    dwt_write32bitoffsetreg(PMSC_ID, PMSC_CTRL1_OFFSET, pmsc_reg);
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_setsnoozetime()
+ *
+ * @brief Set duration of "short sleep" phase when in low-power listening mode.
+ *
+ * input parameters:
+ * @param snooze_time - "short sleep" phase duration, expressed in multiples of 512/19.2 µs (~26.7 µs). The counter
+ *                      automatically adds 1 to the value set. The smallest working value that should be set is 1,
+ *                      i.e. giving a snooze time of 2 units (or ~53 µs).
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_setsnoozetime(uint8 snooze_time)
+{
+    dwt_write8bitoffsetreg(PMSC_ID, PMSC_SNOZT_OFFSET, snooze_time);
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_rxenable()
+ *
+ * @brief This call turns on the receiver, can be immediate or delayed (depending on the mode parameter). In the case of a
+ * "late" error the receiver will only be turned on if the DWT_IDLE_ON_DLY_ERR is not set.
+ * The receiver will stay turned on, listening to any messages until
+ * it either receives a good frame, an error (CRC, PHY header, Reed Solomon) or  it times out (SFD, Preamble or Frame).
+ *
+ * input parameters
+ * @param mode - this can be one of the following allowed values:
+ *
+ * DWT_START_RX_IMMEDIATE      0 used to enbale receiver immediately
+ * DWT_START_RX_DELAYED        1 used to set up delayed RX, if "late" error triggers, then the RX will be enabled immediately
+ * (DWT_START_RX_DELAYED | DWT_IDLE_ON_DLY_ERR) 3 used to disable re-enabling of receiver if delayed RX failed due to "late" error
+ * (DWT_START_RX_IMMEDIATE | DWT_NO_SYNC_PTRS) 4 used to re-enable RX without trying to sync IC and host side buffer pointers, typically when
+ *                                               performing manual RX re-enabling in double buffering mode
+ *
+ * returns DWT_SUCCESS for success, or DWT_ERROR for error (e.g. a delayed receive enable will be too far in the future if delayed time has passed)
+ */
+int dwt_rxenable(int mode)
+{
+    uint16 temp ;
+    uint8 temp1 ;
+
+    if ((mode & DWT_NO_SYNC_PTRS) == 0)
+    {
+        dwt_syncrxbufptrs();
+    }
+
+    temp = (uint16)SYS_CTRL_RXENAB ;
+
+    if (mode & DWT_START_RX_DELAYED)
+    {
+        temp |= (uint16)SYS_CTRL_RXDLYE ;
+    }
+
+    dwt_write16bitoffsetreg(SYS_CTRL_ID, SYS_CTRL_OFFSET, temp);
+
+    if (mode & DWT_START_RX_DELAYED) // check for errors
+    {
+        temp1 = dwt_read8bitoffsetreg(SYS_STATUS_ID, 3); // Read 1 byte at offset 3 to get the 4th byte out of 5
+        if ((temp1 & (SYS_STATUS_HPDWARN >> 24)) != 0) // if delay has passed do immediate RX on unless DWT_IDLE_ON_DLY_ERR is true
+        {
+            dwt_forcetrxoff(); // turn the delayed receive off
+
+            if((mode & DWT_IDLE_ON_DLY_ERR) == 0) // if DWT_IDLE_ON_DLY_ERR not set then re-enable receiver
+            {
+                dwt_write16bitoffsetreg(SYS_CTRL_ID, SYS_CTRL_OFFSET, SYS_CTRL_RXENAB);
+            }
+            return DWT_ERROR; // return warning indication
+        }
+    }
+
+    return DWT_SUCCESS;
+} // end dwt_rxenable()
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_setrxtimeout()
+ *
+ * @brief This call enables RX timeout (SY_STAT_RFTO event)
+ *
+ * input parameters
+ * @param time - how long the receiver remains on from the RX enable command
+ *               The time parameter used here is in 1.0256 us (512/499.2MHz) units
+ *               If set to 0 the timeout is disabled.
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_setrxtimeout(uint16 time)
+{
+    uint8 temp ;
+
+    temp = dwt_read8bitoffsetreg(SYS_CFG_ID, 3); // Read at offset 3 to get the upper byte only
+
+    if(time > 0)
+    {
+        dwt_write16bitoffsetreg(RX_FWTO_ID, RX_FWTO_OFFSET, time) ;
+
+        temp |= (uint8)(SYS_CFG_RXWTOE>>24); // Shift RXWTOE mask as we read the upper byte only
+        // OR in 32bit value (1 bit set), I know this is in high byte.
+        dw1000local.sysCFGreg |= SYS_CFG_RXWTOE;
+
+        dwt_write8bitoffsetreg(SYS_CFG_ID, 3, temp); // Write at offset 3 to write the upper byte only
+    }
+    else
+    {
+        temp &= ~((uint8)(SYS_CFG_RXWTOE>>24)); // Shift RXWTOE mask as we read the upper byte only
+        // AND in inverted 32bit value (1 bit clear), I know this is in high byte.
+        dw1000local.sysCFGreg &= ~(SYS_CFG_RXWTOE);
+
+        dwt_write8bitoffsetreg(SYS_CFG_ID, 3, temp); // Write at offset 3 to write the upper byte only
+    }
+
+} // end dwt_setrxtimeout()
+
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_setpreambledetecttimeout()
+ *
+ * @brief This call enables preamble timeout (SY_STAT_RXPTO event)
+ *
+ * input parameters
+ * @param  timeout - Preamble detection timeout, expressed in multiples of PAC size. The counter automatically adds 1 PAC
+ *                   size to the value set. Min value that can be set is 1 (i.e. a timeout of 2 PAC size).
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_setpreambledetecttimeout(uint16 timeout)
+{
+    dwt_write16bitoffsetreg(DRX_CONF_ID, DRX_PRETOC_OFFSET, timeout);
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn void dwt_setinterrupt()
+ *
+ * @brief This function enables the specified events to trigger an interrupt.
+ * The following events can be enabled:
+ * DWT_INT_TFRS         0x00000080          // frame sent
+ * DWT_INT_RFCG         0x00004000          // frame received with good CRC
+ * DWT_INT_RPHE         0x00001000          // receiver PHY header error
+ * DWT_INT_RFCE         0x00008000          // receiver CRC error
+ * DWT_INT_RFSL         0x00010000          // receiver sync loss error
+ * DWT_INT_RFTO         0x00020000          // frame wait timeout
+ * DWT_INT_RXPTO        0x00200000          // preamble detect timeout
+ * DWT_INT_SFDT         0x04000000          // SFD timeout
+ * DWT_INT_ARFE         0x20000000          // frame rejected (due to frame filtering configuration)
+ *
+ *
+ * input parameters:
+ * @param bitmask - sets the events which will generate interrupt
+ * @param enable - if set the interrupts are enabled else they are cleared
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_setinterrupt(uint32 bitmask, uint8 enable)
+{
+    decaIrqStatus_t stat ;
+    uint32 mask ;
+
+    // Need to beware of interrupts occurring in the middle of following read modify write cycle
+    stat = decamutexon() ;
+
+    mask = dwt_read32bitreg(SYS_MASK_ID) ; // Read register
+
+    if(enable)
+    {
+        mask |= bitmask ;
+    }
+    else
+    {
+        mask &= ~bitmask ; // Clear the bit
+    }
+    dwt_write32bitreg(SYS_MASK_ID,mask) ; // New value
+
+    decamutexoff(stat) ;
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_configeventcounters()
+ *
+ * @brief This is used to enable/disable the event counter in the IC
+ *
+ * input parameters
+ * @param - enable - 1 enables (and reset), 0 disables the event counters
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_configeventcounters(int enable)
+{
+    // Need to clear and disable, can't just clear
+    dwt_write8bitoffsetreg(DIG_DIAG_ID, EVC_CTRL_OFFSET, (uint8)(EVC_CLR));
+
+    if(enable)
+    {
+        dwt_write8bitoffsetreg(DIG_DIAG_ID, EVC_CTRL_OFFSET, (uint8)(EVC_EN)); // Enable
+    }
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_readeventcounters()
+ *
+ * @brief This is used to read the event counters in the IC
+ *
+ * input parameters
+ * @param counters - pointer to the dwt_deviceentcnts_t structure which will hold the read data
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_readeventcounters(dwt_deviceentcnts_t *counters)
+{
+    uint32 temp;
+
+    temp= dwt_read32bitoffsetreg(DIG_DIAG_ID, EVC_PHE_OFFSET); // Read sync loss (31-16), PHE (15-0)
+    counters->PHE = temp & 0xFFF;
+    counters->RSL = (temp >> 16) & 0xFFF;
+
+    temp = dwt_read32bitoffsetreg(DIG_DIAG_ID, EVC_FCG_OFFSET); // Read CRC bad (31-16), CRC good (15-0)
+    counters->CRCG = temp & 0xFFF;
+    counters->CRCB = (temp >> 16) & 0xFFF;
+
+    temp = dwt_read32bitoffsetreg(DIG_DIAG_ID, EVC_FFR_OFFSET); // Overruns (31-16), address errors (15-0)
+    counters->ARFE = temp & 0xFFF;
+    counters->OVER = (temp >> 16) & 0xFFF;
+
+    temp = dwt_read32bitoffsetreg(DIG_DIAG_ID, EVC_STO_OFFSET); // Read PTO (31-16), SFDTO (15-0)
+    counters->PTO = (temp >> 16) & 0xFFF;
+    counters->SFDTO = temp & 0xFFF;
+
+    temp = dwt_read32bitoffsetreg(DIG_DIAG_ID, EVC_FWTO_OFFSET); // Read RX TO (31-16), TXFRAME (15-0)
+    counters->TXF = (temp >> 16) & 0xFFF;
+    counters->RTO = temp & 0xFFF;
+
+    temp = dwt_read32bitoffsetreg(DIG_DIAG_ID, EVC_HPW_OFFSET); // Read half period warning events
+    counters->HPW = temp & 0xFFF;
+    counters->TXW = (temp >> 16) & 0xFFF;                       // Power-up warning events
+
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_rxreset()
+ *
+ * @brief this function resets the receiver of the DW1000
+ *
+ * input parameters:
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_rxreset(void)
+{
+    // Set RX reset
+    dwt_write8bitoffsetreg(PMSC_ID, PMSC_CTRL0_SOFTRESET_OFFSET, PMSC_CTRL0_RESET_RX);
+
+    // Clear RX reset
+    dwt_write8bitoffsetreg(PMSC_ID, PMSC_CTRL0_SOFTRESET_OFFSET, PMSC_CTRL0_RESET_CLEAR);
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_softreset()
+ *
+ * @brief this function resets the DW1000
+ *
+ * input parameters:
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_softreset(void)
+{
+    _dwt_disablesequencing();
+
+    // Clear any AON auto download bits (as reset will trigger AON download)
+    dwt_write16bitoffsetreg(AON_ID, AON_WCFG_OFFSET, 0x00);
+    // Clear the wake-up configuration
+    dwt_write8bitoffsetreg(AON_ID, AON_CFG0_OFFSET, 0x00);
+    // Upload the new configuration
+    _dwt_aonarrayupload();
+
+    // Reset HIF, TX, RX and PMSC
+    dwt_write8bitoffsetreg(PMSC_ID, PMSC_CTRL0_SOFTRESET_OFFSET, PMSC_CTRL0_RESET_ALL);
+
+    // DW1000 needs a 10us sleep to let clk PLL lock after reset - the PLL will automatically lock after the reset
+    // Could also have polled the PLL lock flag, but then the SPI needs to be < 3MHz !! So a simple delay is easier
+    deca_sleep(1);
+
+    // Clear reset
+    dwt_write8bitoffsetreg(PMSC_ID, PMSC_CTRL0_SOFTRESET_OFFSET, PMSC_CTRL0_RESET_CLEAR);
+
+    dw1000local.wait4resp = 0;
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_setxtaltrim()
+ *
+ * @brief This is used to adjust the crystal frequency
+ *
+ * input parameters:
+ * @param   value - crystal trim value (in range 0x0 to 0x1F) 31 steps (~1.5ppm per step)
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_setxtaltrim(uint8 value)
+{
+    // The 3 MSb in this 8-bit register must be kept to 0b011 to avoid any malfunction.
+    uint8 reg_val = (3 << 5) | (value & FS_XTALT_MASK);
+    dwt_write8bitoffsetreg(FS_CTRL_ID, FS_XTALT_OFFSET, reg_val);
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_getinitxtaltrim()
+ *
+ * @brief This function returns the value of XTAL trim that has been applied during initialisation (dwt_init). This can
+ *        be either the value read in OTP memory or a default value.
+ *
+ * NOTE: The value returned by this function is the initial value only! It is not updated on dwt_setxtaltrim calls.
+ *
+ * input parameters
+ *
+ * output parameters
+ *
+ * returns the XTAL trim value set upon initialisation
+ */
+uint8 dwt_getinitxtaltrim(void)
+{
+    return dw1000local.init_xtrim;
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_configcwmode()
+ *
+ * @brief this function sets the DW1000 to transmit cw signal at specific channel frequency
+ *
+ * input parameters:
+ * @param chan - specifies the operating channel (e.g. 1, 2, 3, 4, 5, 6 or 7)
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_configcwmode(uint8 chan)
+{
+#ifdef DWT_API_ERROR_CHECK
+    assert((chan >= 1) && (chan <= 7) && (chan != 6));
+#endif
+
+    //
+    // Disable TX/RX RF block sequencing (needed for cw frame mode)
+    //
+    _dwt_disablesequencing();
+
+    // Config RF pll (for a given channel)
+    // Configure PLL2/RF PLL block CFG/TUNE
+    dwt_write32bitoffsetreg(FS_CTRL_ID, FS_PLLCFG_OFFSET, fs_pll_cfg[chan_idx[chan]]);
+    dwt_write8bitoffsetreg(FS_CTRL_ID, FS_PLLTUNE_OFFSET, fs_pll_tune[chan_idx[chan]]);
+    // PLL wont be enabled until a TX/RX enable is issued later on
+    // Configure RF TX blocks (for specified channel and prf)
+    // Config RF TX control
+    dwt_write32bitoffsetreg(RF_CONF_ID, RF_TXCTRL_OFFSET, tx_config[chan_idx[chan]]);
+
+    //
+    // Enable RF PLL
+    //
+    dwt_write32bitreg(RF_CONF_ID, RF_CONF_TXPLLPOWEN_MASK); // Enable LDO and RF PLL blocks
+    dwt_write32bitreg(RF_CONF_ID, RF_CONF_TXALLEN_MASK); // Enable the rest of TX blocks
+
+    //
+    // Configure TX clocks
+    //
+    dwt_write8bitoffsetreg(PMSC_ID, PMSC_CTRL0_OFFSET, 0x22);
+    dwt_write8bitoffsetreg(PMSC_ID, 0x1, 0x07);
+
+    // Disable fine grain TX sequencing
+    dwt_setfinegraintxseq(0);
+
+    // Configure CW mode
+    dwt_write8bitoffsetreg(TX_CAL_ID, TC_PGTEST_OFFSET, TC_PGTEST_CW);
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_configcontinuousframemode()
+ *
+ * @brief this function sets the DW1000 to continuous tx frame mode for regulatory approvals testing.
+ *
+ * input parameters:
+ * @param framerepetitionrate - This is a 32-bit value that is used to set the interval between transmissions.
+*  The minimum value is 4. The units are approximately 8 ns. (or more precisely 512/(499.2e6*128) seconds)).
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_configcontinuousframemode(uint32 framerepetitionrate)
+{
+    //
+    // Disable TX/RX RF block sequencing (needed for continuous frame mode)
+    //
+    _dwt_disablesequencing();
+
+    //
+    // Enable RF PLL and TX blocks
+    //
+    dwt_write32bitreg(RF_CONF_ID, RF_CONF_TXPLLPOWEN_MASK); // Enable LDO and RF PLL blocks
+    dwt_write32bitreg(RF_CONF_ID, RF_CONF_TXALLEN_MASK); // Enable the rest of TX blocks
+
+    //
+    // Configure TX clocks
+    //
+    _dwt_enableclocks(FORCE_SYS_PLL);
+    _dwt_enableclocks(FORCE_TX_PLL);
+
+    // Set the frame repetition rate
+    if(framerepetitionrate < 4)
+    {
+        framerepetitionrate = 4;
+    }
+    dwt_write32bitreg(DX_TIME_ID, framerepetitionrate);
+
+    //
+    // Configure continuous frame TX
+    //
+    dwt_write8bitoffsetreg(DIG_DIAG_ID, DIAG_TMC_OFFSET, (uint8)(DIAG_TMC_TX_PSTM)); // Turn the tx power spectrum test mode - continuous sending of frames
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_readtempvbat()
+ *
+ * @brief this function reads the battery voltage and temperature of the MP
+ * The values read here will be the current values sampled by DW1000 AtoD converters.
+ * Note on Temperature: the temperature value needs to be converted to give the real temperature
+ * the formula is: 1.13 * reading - 113.0
+ * Note on Voltage: the voltage value needs to be converted to give the real voltage
+ * the formula is: 0.0057 * reading + 2.3
+ *
+ * NB: To correctly read the temperature this read should be done with xtal clock
+ * however that means that the receiver will be switched off, if receiver needs to be on then
+ * the timer is used to make sure the value is stable before reading
+ *
+ * input parameters:
+ * @param fastSPI - set to 1 if SPI rate > than 3MHz is used
+ *
+ * output parameters
+ *
+ * returns  (temp_raw<<8)|(vbat_raw)
+ */
+uint16 dwt_readtempvbat(uint8 fastSPI)
+{
+    uint8 wr_buf[2];
+    uint8 vbat_raw;
+    uint8 temp_raw;
+
+    // These writes should be single writes and in sequence
+    wr_buf[0] = 0x80; // Enable TLD Bias
+    dwt_writetodevice(RF_CONF_ID,0x11,1,wr_buf);
+
+    wr_buf[0] = 0x0A; // Enable TLD Bias and ADC Bias
+    dwt_writetodevice(RF_CONF_ID,0x12,1,wr_buf);
+
+    wr_buf[0] = 0x0f; // Enable Outputs (only after Biases are up and running)
+    dwt_writetodevice(RF_CONF_ID,0x12,1,wr_buf);    //
+
+    // Reading All SAR inputs
+    wr_buf[0] = 0x00;
+    dwt_writetodevice(TX_CAL_ID, TC_SARL_SAR_C,1,wr_buf);
+    wr_buf[0] = 0x01; // Set SAR enable
+    dwt_writetodevice(TX_CAL_ID, TC_SARL_SAR_C,1,wr_buf);
+
+    if(fastSPI == 1)
+    {
+        deca_sleep(1); // If using PLL clocks(and fast SPI rate) then this sleep is needed
+        // Read voltage and temperature.
+        dwt_readfromdevice(TX_CAL_ID, TC_SARL_SAR_LVBAT_OFFSET,2,wr_buf);
+    }
+    else //change to a slow clock
+    {
+        _dwt_enableclocks(FORCE_SYS_XTI); // NOTE: set system clock to XTI - this is necessary to make sure the values read are reliable
+        // Read voltage and temperature.
+        dwt_readfromdevice(TX_CAL_ID, TC_SARL_SAR_LVBAT_OFFSET,2,wr_buf);
+        // Default clocks (ENABLE_ALL_SEQ)
+        _dwt_enableclocks(ENABLE_ALL_SEQ); // Enable clocks for sequencing
+    }
+
+    vbat_raw = wr_buf[0];
+    temp_raw = wr_buf[1];
+
+    wr_buf[0] = 0x00; // Clear SAR enable
+    dwt_writetodevice(TX_CAL_ID, TC_SARL_SAR_C,1,wr_buf);
+
+    return ((temp_raw<<8)|(vbat_raw));
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_readwakeuptemp()
+ *
+ * @brief this function reads the temperature of the DW1000 that was sampled
+ * on waking from Sleep/Deepsleep. They are not current values, but read on last
+ * wakeup if DWT_TANDV bit is set in mode parameter of dwt_configuresleep
+ *
+ * input parameters:
+ *
+ * output parameters:
+ *
+ * returns: 8-bit raw temperature sensor value
+ */
+uint8 dwt_readwakeuptemp(void)
+{
+    return dwt_read8bitoffsetreg(TX_CAL_ID, TC_SARL_SAR_LTEMP_OFFSET);
+}
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_readwakeupvbat()
+ *
+ * @brief this function reads the battery voltage of the DW1000 that was sampled
+ * on waking from Sleep/Deepsleep. They are not current values, but read on last
+ * wakeup if DWT_TANDV bit is set in mode parameter of dwt_configuresleep
+ *
+ * input parameters:
+ *
+ * output parameters:
+ *
+ * returns: 8-bit raw battery voltage sensor value
+ */
+uint8 dwt_readwakeupvbat(void)
+{
+    return dwt_read8bitoffsetreg(TX_CAL_ID, TC_SARL_SAR_LVBAT_OFFSET);
+}
+
+
+/* ===============================================================================================
+   List of expected (known) device ID handled by this software
+   ===============================================================================================
+
+    0xDECA0130                               // DW1000 - MP
+
+   ===============================================================================================
+*/
+
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/decadriver/deca_device_api.h	Wed Dec 06 21:35:45 2017 +0000
@@ -0,0 +1,1745 @@
+/*! ----------------------------------------------------------------------------
+ * @file    deca_device_api.h
+ * @brief   DW1000 API Functions
+ *
+ * @attention
+ *
+ * Copyright 2013 (c) Decawave Ltd, Dublin, Ireland.
+ *
+ * All rights reserved.
+ *
+ */
+
+#ifndef _DECA_DEVICE_API_H_
+#define _DECA_DEVICE_API_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+#ifndef uint8
+#ifndef _DECA_UINT8_
+#define _DECA_UINT8_
+typedef unsigned char uint8;
+#endif
+#endif
+
+#ifndef uint16
+#ifndef _DECA_UINT16_
+#define _DECA_UINT16_
+typedef unsigned short uint16;
+#endif
+#endif
+
+#ifndef uint32
+#ifndef _DECA_UINT32_
+#define _DECA_UINT32_
+typedef unsigned long uint32;
+#endif
+#endif
+
+#ifndef int8
+#ifndef _DECA_INT8_
+#define _DECA_INT8_
+typedef signed char int8;
+#endif
+#endif
+
+#ifndef int16
+#ifndef _DECA_INT16_
+#define _DECA_INT16_
+typedef signed short int16;
+#endif
+#endif
+
+#ifndef int32
+#ifndef _DECA_INT32_
+#define _DECA_INT32_
+typedef signed long int32;
+#endif
+#endif
+
+#define DWT_SUCCESS (0)
+#define DWT_ERROR   (-1)
+
+#define DWT_TIME_UNITS          (1.0/499.2e6/128.0) //!< = 15.65e-12 s
+
+#define DWT_DEVICE_ID   (0xDECA0130)        //!< DW1000 MP device ID
+
+//! constants for selecting the bit rate for data TX (and RX)
+//! These are defined for write (with just a shift) the TX_FCTRL register
+#define DWT_BR_110K     0   //!< UWB bit rate 110 kbits/s
+#define DWT_BR_850K     1   //!< UWB bit rate 850 kbits/s
+#define DWT_BR_6M8      2   //!< UWB bit rate 6.8 Mbits/s
+
+//! constants for specifying the (Nominal) mean Pulse Repetition Frequency
+//! These are defined for direct write (with a shift if necessary) to CHAN_CTRL and TX_FCTRL regs
+#define DWT_PRF_16M     1   //!< UWB PRF 16 MHz
+#define DWT_PRF_64M     2   //!< UWB PRF 64 MHz
+
+//! constants for specifying Preamble Acquisition Chunk (PAC) Size in symbols
+#define DWT_PAC8        0   //!< PAC  8 (recommended for RX of preamble length  128 and below
+#define DWT_PAC16       1   //!< PAC 16 (recommended for RX of preamble length  256
+#define DWT_PAC32       2   //!< PAC 32 (recommended for RX of preamble length  512
+#define DWT_PAC64       3   //!< PAC 64 (recommended for RX of preamble length 1024 and up
+
+//! constants for specifying TX Preamble length in symbols
+//! These are defined to allow them be directly written into byte 2 of the TX_FCTRL register
+//! (i.e. a four bit value destined for bits 20..18 but shifted left by 2 for byte alignment)
+#define DWT_PLEN_4096   0x0C    //! Standard preamble length 4096 symbols
+#define DWT_PLEN_2048   0x28    //! Non-standard preamble length 2048 symbols
+#define DWT_PLEN_1536   0x18    //! Non-standard preamble length 1536 symbols
+#define DWT_PLEN_1024   0x08    //! Standard preamble length 1024 symbols
+#define DWT_PLEN_512    0x34    //! Non-standard preamble length 512 symbols
+#define DWT_PLEN_256    0x24    //! Non-standard preamble length 256 symbols
+#define DWT_PLEN_128    0x14    //! Non-standard preamble length 128 symbols
+#define DWT_PLEN_64     0x04    //! Standard preamble length 64 symbols
+
+#define DWT_SFDTOC_DEF              0x1041  // default SFD timeout value
+
+#define DWT_PHRMODE_STD             0x0     // standard PHR mode
+#define DWT_PHRMODE_EXT             0x3     // DW proprietary extended frames PHR mode
+
+// Defined constants for "mode" bitmask parameter passed into dwt_starttx() function.
+#define DWT_START_TX_IMMEDIATE      0
+#define DWT_START_TX_DELAYED        1
+#define DWT_RESPONSE_EXPECTED       2
+
+#define DWT_START_RX_IMMEDIATE  0
+#define DWT_START_RX_DELAYED    1    // Set up delayed RX, if "late" error triggers, then the RX will be enabled immediately
+#define DWT_IDLE_ON_DLY_ERR     2    // If delayed RX failed due to "late" error then if this
+                                     // flag is set the RX will not be re-enabled immediately, and device will be in IDLE when function exits
+#define DWT_NO_SYNC_PTRS        4    // Do not try to sync IC side and Host side buffer pointers when enabling RX. This is used to perform manual RX
+                                     // re-enabling when receiving a frame in double buffer mode.
+
+// Defined constants for "mode" bit field parameter passed to dwt_setleds() function.
+#define DWT_LEDS_DISABLE     0x00
+#define DWT_LEDS_ENABLE      0x01
+#define DWT_LEDS_INIT_BLINK  0x02
+
+//frame filtering configuration options
+#define DWT_FF_NOTYPE_EN            0x000           // no frame types allowed (FF disabled)
+#define DWT_FF_COORD_EN             0x002           // behave as coordinator (can receive frames with no dest address (PAN ID has to match))
+#define DWT_FF_BEACON_EN            0x004           // beacon frames allowed
+#define DWT_FF_DATA_EN              0x008           // data frames allowed
+#define DWT_FF_ACK_EN               0x010           // ack frames allowed
+#define DWT_FF_MAC_EN               0x020           // mac control frames allowed
+#define DWT_FF_RSVD_EN              0x040           // reserved frame types allowed
+
+//DW1000 interrupt events
+#define DWT_INT_TFRS            0x00000080          // frame sent
+#define DWT_INT_LDED            0x00000400          // micro-code has finished execution
+#define DWT_INT_RFCG            0x00004000          // frame received with good CRC
+#define DWT_INT_RPHE            0x00001000          // receiver PHY header error
+#define DWT_INT_RFCE            0x00008000          // receiver CRC error
+#define DWT_INT_RFSL            0x00010000          // receiver sync loss error
+#define DWT_INT_RFTO            0x00020000          // frame wait timeout
+#define DWT_INT_RXOVRR          0x00100000          // receiver overrun
+#define DWT_INT_RXPTO           0x00200000          // preamble detect timeout
+#define DWT_INT_SFDT            0x04000000          // SFD timeout
+#define DWT_INT_ARFE            0x20000000          // frame rejected (due to frame filtering configuration)
+
+
+//DW1000 SLEEP and WAKEUP configuration parameters
+#define DWT_PRESRV_SLEEP 0x0100                      // PRES_SLEEP - on wakeup preserve sleep bit
+#define DWT_LOADOPSET    0x0080                      // ONW_L64P - on wakeup load operating parameter set for 64 PSR
+#define DWT_CONFIG       0x0040                      // ONW_LDC - on wakeup restore (load) the saved configurations (from AON array into HIF)
+#define DWT_RX_EN        0x0002                      // ONW_RX - on wakeup activate reception
+#define DWT_TANDV        0x0001                      // ONW_RADC - on wakeup run ADC to sample temperature and voltage sensor values
+
+#define DWT_XTAL_EN      0x10                       // keep XTAL running during sleep
+#define DWT_WAKE_SLPCNT  0x8                        // wake up after sleep count
+#define DWT_WAKE_CS      0x4                        // wake up on chip select
+#define DWT_WAKE_WK      0x2                        // wake up on WAKEUP PIN
+#define DWT_SLP_EN       0x1                        // enable sleep/deep sleep functionality
+
+//DW1000 INIT configuration parameters
+#define DWT_LOADUCODE     0x1
+#define DWT_LOADNONE      0x0
+
+//DW1000 OTP operating parameter set selection
+#define DWT_OPSET_64LEN   0x0
+#define DWT_OPSET_TIGHT   0x1
+#define DWT_OPSET_DEFLT   0x2
+
+// Call-back data RX frames flags
+#define DWT_CB_DATA_RX_FLAG_RNG 0x1 // Ranging bit
+
+// TX/RX call-back data
+typedef struct
+{
+    uint32 status;      //initial value of register as ISR is entered
+    uint16 datalength;  //length of frame
+    uint8  fctrl[2];    //frame control bytes
+    uint8  rx_flags;    //RX frame flags, see above
+} dwt_cb_data_t;
+
+// Call-back type for all events
+typedef void (*dwt_cb_t)(const dwt_cb_data_t *);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * Structure typedef: dwt_config_t
+ *
+ * Structure for setting device configuration via dwt_configure() function
+ *
+ */
+typedef struct
+{
+    uint8 chan ;           //!< channel number {1, 2, 3, 4, 5, 7 }
+    uint8 prf ;            //!< Pulse Repetition Frequency {DWT_PRF_16M or DWT_PRF_64M}
+    uint8 txPreambLength ; //!< DWT_PLEN_64..DWT_PLEN_4096
+    uint8 rxPAC ;          //!< Acquisition Chunk Size (Relates to RX preamble length)
+    uint8 txCode ;         //!< TX preamble code
+    uint8 rxCode ;         //!< RX preamble code
+    uint8 nsSFD ;          //!< Boolean should we use non-standard SFD for better performance
+    uint8 dataRate ;       //!< Data Rate {DWT_BR_110K, DWT_BR_850K or DWT_BR_6M8}
+    uint8 phrMode ;        //!< PHR mode {0x0 - standard DWT_PHRMODE_STD, 0x3 - extended frames DWT_PHRMODE_EXT}
+    uint16 sfdTO ;         //!< SFD timeout value (in symbols)
+} dwt_config_t ;
+
+
+typedef struct
+{
+    uint8   PGdly;
+    //TX POWER
+    //31:24     BOOST_0.125ms_PWR
+    //23:16     BOOST_0.25ms_PWR-TX_SHR_PWR
+    //15:8      BOOST_0.5ms_PWR-TX_PHR_PWR
+    //7:0       DEFAULT_PWR-TX_DATA_PWR
+    uint32  power;
+}
+dwt_txconfig_t ;
+
+
+typedef struct
+{
+
+    uint16      maxNoise ;          // LDE max value of noise
+    uint16      firstPathAmp1 ;     // Amplitude at floor(index FP) + 1
+    uint16      stdNoise ;          // Standard deviation of noise
+    uint16      firstPathAmp2 ;     // Amplitude at floor(index FP) + 2
+    uint16      firstPathAmp3 ;     // Amplitude at floor(index FP) + 3
+    uint16      maxGrowthCIR ;      // Channel Impulse Response max growth CIR
+    uint16      rxPreamCount ;      // Count of preamble symbols accumulated
+    uint16      firstPath ;         // First p ath index (10.6 bits fixed point integer)
+}dwt_rxdiag_t ;
+
+
+typedef struct
+{
+    //all of the below are mapped to a 12-bit register in DW1000
+    uint16 PHE ;                    //number of received header errors
+    uint16 RSL ;                    //number of received frame sync loss events
+    uint16 CRCG ;                   //number of good CRC received frames
+    uint16 CRCB ;                   //number of bad CRC (CRC error) received frames
+    uint16 ARFE ;                   //number of address filter errors
+    uint16 OVER ;                   //number of receiver overflows (used in double buffer mode)
+    uint16 SFDTO ;                  //SFD timeouts
+    uint16 PTO ;                    //Preamble timeouts
+    uint16 RTO ;                    //RX frame wait timeouts
+    uint16 TXF ;                    //number of transmitted frames
+    uint16 HPW ;                    //half period warn
+    uint16 TXW ;                    //power up warn
+
+} dwt_deviceentcnts_t ;
+
+
+/********************************************************************************************************************/
+/*                                                 REMOVED API LIST                                                 */
+/********************************************************************************************************************/
+/*
+ * From version 4.0.0:
+ *  - dwt_setGPIOforEXTTRX: Replaced by dwt_setlnapamode to get equivalent functionality.
+ *  - dwt_setGPIOdirection: Renamed to dwt_setgpiodirection.
+ *  - dwt_setGPIOvalue: Renamed to dwt_setgpiovalue.
+ *  - dwt_setrxmode: Replaced by dwt_setsniffmode and dwt_setlowpowerlistening depending on the RX mode the user
+ *    wants to set up.
+ *  - dwt_checkoverrun: As automatic RX re-enabling is not supported anymore, this functions has become useless.
+ *  - dwt_setautorxreenable: As automatic RX re-enabling is not supported anymore, this functions has become
+ *    useless.
+ *  - dwt_getrangebias: Range bias correction values are platform dependent and should therefore be managed at user
+ *    application level.
+ *  - dwt_xtaltrim: Renamed to dwt_setxtaltrim.
+ *  - dwt_checkIRQ: Renamed to dwt_checkirq.
+ *
+ * From version 3.0.0:
+ *  - dwt_getldotune: As LDO loading is now automatically managed by the driver, this function has become useless.
+ *  - dwt_getotptxpower: TX power values and location in OTP memory are platform dependent and should therefore be
+ *    managed at user application level.
+ *  - dwt_readantennadelay: Antenna delay values and location in OTP memory are platform dependent and should
+ *    therefore be managed at user application level.
+ *  - dwt_readdignostics: Renamed to dwt_readdiagnostics.
+ */
+
+/********************************************************************************************************************/
+/*                                                     API LIST                                                     */
+/********************************************************************************************************************/
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_getpartid()
+ *
+ * @brief This is used to return the read part ID of the device
+ *
+ * NOTE: dwt_initialise() must be called prior to this function so that it can return a relevant value.
+ *
+ * input parameters
+ *
+ * output parameters
+ *
+ * returns the 32 bit part ID value as programmed in the factory
+ */
+uint32 dwt_getpartid(void);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_getlotid()
+ *
+ * @brief This is used to return the read lot ID of the device
+ *
+ * NOTE: dwt_initialise() must be called prior to this function so that it can return a relevant value.
+ *
+ * input parameters
+ *
+ * output parameters
+ *
+ * returns the 32 bit lot ID value as programmed in the factory
+ */
+uint32 dwt_getlotid(void);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_readdevid()
+ *
+ * @brief This is used to return the read device type and revision information of the DW1000 device (MP part is 0xDECA0130)
+ *
+ * input parameters
+ *
+ * output parameters
+ *
+ * returns the read value which for DW1000 is 0xDECA0130
+ */
+uint32 dwt_readdevid(void);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_otprevision()
+ *
+ * @brief This is used to return the read OTP revision
+ *
+ * NOTE: dwt_initialise() must be called prior to this function so that it can return a relevant value.
+ *
+ * input parameters
+ *
+ * output parameters
+ *
+ * returns the read OTP revision value
+ */
+uint8 dwt_otprevision(void);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_setfinegraintxseq()
+ *
+ * @brief This function enables/disables the fine grain TX sequencing (enabled by default).
+ *
+ * input parameters
+ * @param enable - 1 to enable fine grain TX sequencing, 0 to disable it.
+ *
+ * output parameters none
+ *
+ * no return value
+ */
+void dwt_setfinegraintxseq(int enable);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_setlnapamode()
+ *
+ * @brief This is used to enable GPIO for external LNA or PA functionality - HW dependent, consult the DW1000 User Manual.
+ *        This can also be used for debug as enabling TX and RX GPIOs is quite handy to monitor DW1000's activity.
+ *
+ * NOTE: Enabling PA functionality requires that fine grain TX sequencing is deactivated. This can be done using
+ *       dwt_setfinegraintxseq().
+ *
+ * input parameters
+ * @param lna - 1 to enable LNA functionality, 0 to disable it
+ * @param pa - 1 to enable PA functionality, 0 to disable it
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_setlnapamode(int lna, int pa);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_setgpiodirection()
+ *
+ * @brief This is used to set GPIO direction as an input (1) or output (0)
+ *
+ * input parameters
+ * @param gpioNum    -   this is the GPIO to configure - see GxM0... GxM8 in the deca_regs.h file
+ * @param direction  -   this sets the GPIO direction - see GxP0... GxP8 in the deca_regs.h file
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_setgpiodirection(uint32 gpioNum, uint32 direction);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_setgpiovalue()
+ *
+ * @brief This is used to set GPIO value as (1) or (0) only applies if the GPIO is configured as output
+ *
+ * input parameters
+ * @param gpioNum    -   this is the GPIO to configure - see GxM0... GxM8 in the deca_regs.h file
+ * @param value  -   this sets the GPIO value - see GDP0... GDP8 in the deca_regs.h file
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_setgpiovalue(uint32 gpioNum, uint32 value);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_initialise()
+ *
+ * @brief This function initiates communications with the DW1000 transceiver
+ * and reads its DEV_ID register (address 0x00) to verify the IC is one supported
+ * by this software (e.g. DW1000 32-bit device ID value is 0xDECA0130).  Then it
+ * does any initial once only device configurations needed for use and initialises
+ * as necessary any static data items belonging to this low-level driver.
+ *
+ * NOTES:
+ * 1.this function needs to be run before dwt_configuresleep, also the SPI frequency has to be < 3MHz
+ * 2.it also reads and applies LDO tune and crystal trim values from OTP memory
+ *
+ * input parameters
+ * @param config    -   specifies what configuration to load
+ *                  DWT_LOADUCODE     0x1 - load the LDE microcode from ROM - enabled accurate RX timestamp
+ *                  DWT_LOADNONE      0x0 - do not load any values from OTP memory
+ *
+ * output parameters
+ *
+ * returns DWT_SUCCESS for success, or DWT_ERROR for error
+ */
+int dwt_initialise(uint16 config) ;
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_configure()
+ *
+ * @brief This function provides the main API for the configuration of the
+ * DW1000 and this low-level driver.  The input is a pointer to the data structure
+ * of type dwt_config_t that holds all the configurable items.
+ * The dwt_config_t structure shows which ones are supported
+ *
+ * input parameters
+ * @param config    -   pointer to the configuration structure, which contains the device configuration data.
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_configure(dwt_config_t *config) ;
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_configuretxrf()
+ *
+ * @brief This function provides the API for the configuration of the TX spectrum
+ * including the power and pulse generator delay. The input is a pointer to the data structure
+ * of type dwt_txconfig_t that holds all the configurable items.
+ *
+ * input parameters
+ * @param config    -   pointer to the txrf configuration structure, which contains the tx rf config data
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_configuretxrf(dwt_txconfig_t *config) ;
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_setrxantennadelay()
+ *
+ * @brief This API function writes the antenna delay (in time units) to RX registers
+ *
+ * input parameters:
+ * @param rxDelay - this is the total (RX) antenna delay value, which
+ *                          will be programmed into the RX register
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_setrxantennadelay(uint16 antennaDly);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_settxantennadelay()
+ *
+ * @brief This API function writes the antenna delay (in time units) to TX registers
+ *
+ * input parameters:
+ * @param txDelay - this is the total (TX) antenna delay value, which
+ *                          will be programmed into the TX delay register
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_settxantennadelay(uint16 antennaDly);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_setsmarttxpower()
+ *
+ * @brief This call enables or disables the smart TX power feature.
+ *
+ * input parameters
+ * @param enable - this enables or disables the TX smart power (1 = enable, 0 = disable)
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_setsmarttxpower(int enable);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_writetxdata()
+ *
+ * @brief This API function writes the supplied TX data into the DW1000's
+ * TX buffer.  The input parameters are the data length in bytes and a pointer
+ * to those data bytes.
+ *
+ * input parameters
+ * @param txFrameLength  - This is the total frame length, including the two byte CRC.
+ *                         Note: this is the length of TX message (including the 2 byte CRC) - max is 1023
+ *                         standard PHR mode allows up to 127 bytes
+ *                         if > 127 is programmed, DWT_PHRMODE_EXT needs to be set in the phrMode configuration
+ *                         see dwt_configure function
+ * @param txFrameBytes   - Pointer to the user’s buffer containing the data to send.
+ * @param txBufferOffset - This specifies an offset in the DW1000’s TX Buffer at which to start writing data.
+ *
+ * output parameters
+ *
+ * returns DWT_SUCCESS for success, or DWT_ERROR for error
+ */
+int dwt_writetxdata(uint16 txFrameLength, uint8 *txFrameBytes, uint16 txBufferOffset) ;
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_writetxfctrl()
+ *
+ * @brief This API function configures the TX frame control register before the transmission of a frame
+ *
+ * input parameters:
+ * @param txFrameLength - this is the length of TX message (including the 2 byte CRC) - max is 1023
+ *                              NOTE: standard PHR mode allows up to 127 bytes
+ *                              if > 127 is programmed, DWT_PHRMODE_EXT needs to be set in the phrMode configuration
+ *                              see dwt_configure function
+ * @param txBufferOffset - the offset in the tx buffer to start writing the data
+ * @param ranging - 1 if this is a ranging frame, else 0
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_writetxfctrl(uint16 txFrameLength, uint16 txBufferOffset, int ranging);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_starttx()
+ *
+ * @brief This call initiates the transmission, input parameter indicates which TX mode is used see below
+ *
+ * input parameters:
+ * @param mode - if 0 immediate TX (no response expected)
+ *               if 1 delayed TX (no response expected)
+ *               if 2 immediate TX (response expected - so the receiver will be automatically turned on after TX is done)
+ *               if 3 delayed TX (response expected - so the receiver will be automatically turned on after TX is done)
+ *
+ * output parameters
+ *
+ * returns DWT_SUCCESS for success, or DWT_ERROR for error (e.g. a delayed transmission will fail if the delayed time has passed)
+ */
+int dwt_starttx(uint8 mode) ;
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_setdelayedtrxtime()
+ *
+ * @brief This API function configures the delayed transmit time or the delayed RX on time
+ *
+ * input parameters
+ * @param starttime - the TX/RX start time (the 32 bits should be the high 32 bits of the system time at which to send the message,
+ * or at which to turn on the receiver)
+ *
+ * output parameters none
+ *
+ * no return value
+ */
+void dwt_setdelayedtrxtime(uint32 starttime) ;
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_readtxtimestamp()
+ *
+ * @brief This is used to read the TX timestamp (adjusted with the programmed antenna delay)
+ *
+ * input parameters
+ * @param timestamp - a pointer to a 5-byte buffer which will store the read TX timestamp time
+ *
+ * output parameters - the timestamp buffer will contain the value after the function call
+ *
+ * no return value
+ */
+void dwt_readtxtimestamp(uint8 * timestamp);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_readtxtimestamphi32()
+ *
+ * @brief This is used to read the high 32-bits of the TX timestamp (adjusted with the programmed antenna delay)
+ *
+ * input parameters
+ *
+ * output parameters
+ *
+ * returns high 32-bits of TX timestamp
+ */
+uint32 dwt_readtxtimestamphi32(void);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_readtxtimestamplo32()
+ *
+ * @brief This is used to read the low 32-bits of the TX timestamp (adjusted with the programmed antenna delay)
+ *
+ * input parameters
+ *
+ * output parameters
+ *
+ * returns low 32-bits of TX timestamp
+ */
+uint32 dwt_readtxtimestamplo32(void);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_readrxtimestamp()
+ *
+ * @brief This is used to read the RX timestamp (adjusted time of arrival)
+ *
+ * input parameters
+ * @param timestamp - a pointer to a 5-byte buffer which will store the read RX timestamp time
+ *
+ * output parameters - the timestamp buffer will contain the value after the function call
+ *
+ * no return value
+ */
+void dwt_readrxtimestamp(uint8 * timestamp);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_readrxtimestamphi32()
+ *
+ * @brief This is used to read the high 32-bits of the RX timestamp (adjusted with the programmed antenna delay)
+ *
+ * input parameters
+ *
+ * output parameters
+ *
+ * returns high 32-bits of RX timestamp
+ */
+uint32 dwt_readrxtimestamphi32(void);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_readrxtimestamplo32()
+ *
+ * @brief This is used to read the low 32-bits of the RX timestamp (adjusted with the programmed antenna delay)
+ *
+ * input parameters
+ *
+ * output parameters
+ *
+ * returns low 32-bits of RX timestamp
+ */
+uint32 dwt_readrxtimestamplo32(void);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_readsystimestamphi32()
+ *
+ * @brief This is used to read the high 32-bits of the system time
+ *
+ * input parameters
+ *
+ * output parameters
+ *
+ * returns high 32-bits of system time timestamp
+ */
+uint32 dwt_readsystimestamphi32(void);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_readsystime()
+ *
+ * @brief This is used to read the system time
+ *
+ * input parameters
+ * @param timestamp - a pointer to a 5-byte buffer which will store the read system time
+ *
+ * output parameters
+ * @param timestamp - the timestamp buffer will contain the value after the function call
+ *
+ * no return value
+ */
+void dwt_readsystime(uint8 * timestamp);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_forcetrxoff()
+ *
+ * @brief This is used to turn off the transceiver
+ *
+ * input parameters
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_forcetrxoff(void);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_syncrxbufptrs()
+ *
+ * @brief this function synchronizes rx buffer pointers
+ * need to make sure that the host/IC buffer pointers are aligned before starting RX
+ *
+ * input parameters:
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_syncrxbufptrs(void);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_rxenable()
+ *
+ * @brief This call turns on the receiver, can be immediate or delayed (depending on the mode parameter). In the case of a
+ * "late" error the receiver will only be turned on if the DWT_IDLE_ON_DLY_ERR is not set.
+ * The receiver will stay turned on, listening to any messages until
+ * it either receives a good frame, an error (CRC, PHY header, Reed Solomon) or  it times out (SFD, Preamble or Frame).
+ *
+ * input parameters
+ * @param mode - this can be one of the following allowed values:
+ *
+ * DWT_START_RX_IMMEDIATE      0 used to enbale receiver immediately
+ * DWT_START_RX_DELAYED        1 used to set up delayed RX, if "late" error triggers, then the RX will be enabled immediately
+ * (DWT_START_RX_DELAYED | DWT_IDLE_ON_DLY_ERR) 3 used to disable re-enabling of receiver if delayed RX failed due to "late" error
+ * (DWT_START_RX_IMMEDIATE | DWT_NO_SYNC_PTRS) 4 used to re-enable RX without trying to sync IC and host side buffer pointers, typically when
+ *                                               performing manual RX re-enabling in double buffering mode
+ *
+ * returns DWT_SUCCESS for success, or DWT_ERROR for error (e.g. a delayed receive enable will be too far in the future if delayed time has passed)
+ */
+int dwt_rxenable(int mode);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_setsniffmode()
+ *
+ * @brief enable/disable and configure SNIFF mode.
+ *
+ * SNIFF mode is a low-power reception mode where the receiver is sequenced on and off instead of being on all the time.
+ * The time spent in each state (on/off) is specified through the parameters below.
+ * See DW1000 User Manual section 4.5 "Low-Power SNIFF mode" for more details.
+ *
+ * input parameters:
+ * @param enable - 1 to enable SNIFF mode, 0 to disable. When 0, all other parameters are not taken into account.
+ * @param timeOn - duration of receiver ON phase, expressed in multiples of PAC size. The counter automatically adds 1 PAC
+ *                 size to the value set. Min value that can be set is 1 (i.e. an ON time of 2 PAC size), max value is 15.
+ * @param timeOff - duration of receiver OFF phase, expressed in multiples of 128/125 µs (~1 µs). Max value is 255.
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_setsniffmode(int enable, uint8 timeOn, uint8 timeOff);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_setlowpowerlistening()
+ *
+ * @brief enable/disable low-power listening mode.
+ *
+ * Low-power listening is a feature whereby the DW1000 is predominantly in the SLEEP state but wakes periodically, (after
+ * this "long sleep"), for a very short time to sample the air for a preamble sequence. This preamble sampling "listening"
+ * phase is actually two reception phases separated by a "short sleep" time. See DW1000 User Manual section "Low-Power
+ * Listening" for more details.
+ *
+ * NOTE: Before enabling low-power listening, the following functions have to be called to fully configure it:
+ *           - dwt_configuresleep() to configure long sleep phase. "mode" parameter should at least have DWT_PRESRV_SLEEP,
+ *             DWT_CONFIG and DWT_RX_EN set and "wake" parameter should at least have both DWT_WAKE_SLPCNT and DWT_SLP_EN set.
+ *           - dwt_calibratesleepcnt() and dwt_configuresleepcnt() to define the "long sleep" phase duration.
+ *           - dwt_setsnoozetime() to define the "short sleep" phase duration.
+ *           - dwt_setpreambledetecttimeout() to define the reception phases duration.
+ *           - dwt_setinterrupt() to activate RX good frame interrupt (DWT_INT_RFCG) only.
+ *       When configured, low-power listening mode can be triggered either by putting the DW1000 to sleep (using
+ *       dwt_entersleep()) or by activating reception (using dwt_rxenable()).
+ *
+ *       Please refer to the low-power listening examples (examples 8a/8b accompanying the API distribution on Decawave's
+ *       website). They form a working example code that shows how to use low-power listening correctly.
+ *
+ * input parameters:
+ * @param enable - 1 to enable low-power listening, 0 to disable.
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_setlowpowerlistening(int enable);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_setsnoozetime()
+ *
+ * @brief Set duration of "short sleep" phase when in low-power listening mode.
+ *
+ * input parameters:
+ * @param snooze_time - "short sleep" phase duration, expressed in multiples of 512/19.2 µs (~26.7 µs). The counter
+ *                      automatically adds 1 to the value set. The smallest working value that should be set is 1,
+ *                      i.e. giving a snooze time of 2 units (or ~53 µs).
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_setsnoozetime(uint8 snooze_time);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_setdblrxbuffmode()
+ *
+ * @brief This call enables the double receive buffer mode
+ *
+ * input parameters
+ * @param enable - 1 to enable, 0 to disable the double buffer mode
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_setdblrxbuffmode(int enable);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_setrxtimeout()
+ *
+ * @brief This call enables RX timeout (SY_STAT_RFTO event)
+ *
+ * input parameters
+ * @param time - how long the receiver remains on from the RX enable command
+ *               The time parameter used here is in 1.0256 us (512/499.2MHz) units
+ *               If set to 0 the timeout is disabled.
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_setrxtimeout(uint16 time);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_setpreambledetecttimeout()
+ *
+ * @brief This call enables preamble timeout (SY_STAT_RXPTO event)
+ *
+ * input parameters
+ * @param  timeout - Preamble detection timeout, expressed in multiples of PAC size. The counter automatically adds 1 PAC
+ *                   size to the value set. Min value that can be set is 1 (i.e. a timeout of 2 PAC size).
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_setpreambledetecttimeout(uint16 timeout);
+
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_calibratesleepcnt()
+ *
+ * @brief calibrates the local oscillator as its frequency can vary between 7 and 13kHz depending on temp and voltage
+ *
+ * NOTE: this function needs to be run before dwt_configuresleepcnt, so that we know what the counter units are
+ *
+ * input parameters
+ *
+ * output parameters
+ *
+ * returns the number of XTAL/2 cycles per low-power oscillator cycle. LP OSC frequency = 19.2 MHz/return value
+ */
+uint16 dwt_calibratesleepcnt(void);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_configuresleepcnt()
+ *
+ * @brief sets the sleep counter to new value, this function programs the high 16-bits of the 28-bit counter
+ *
+ * NOTE: this function needs to be run before dwt_configuresleep, also the SPI frequency has to be < 3MHz
+ *
+ * input parameters
+ * @param sleepcnt - this it value of the sleep counter to program
+ *
+ * output parameters
+ *
+ * no return value
+ */
+ void dwt_configuresleepcnt(uint16 sleepcnt);
+
+ /*! ------------------------------------------------------------------------------------------------------------------
+  * @fn dwt_configuresleep()
+  *
+  * @brief configures the device for both DEEP_SLEEP and SLEEP modes, and on-wake mode
+  * i.e. before entering the sleep, the device should be programmed for TX or RX, then upon "waking up" the TX/RX settings
+  * will be preserved and the device can immediately perform the desired action TX/RX
+  *
+  * NOTE: e.g. Tag operation - after deep sleep, the device needs to just load the TX buffer and send the frame
+  *
+  *
+  *      mode: the array and LDE code (OTP/ROM) and LDO tune, and set sleep persist
+  *      DWT_PRESRV_SLEEP 0x0100 - preserve sleep
+  *      DWT_LOADOPSET    0x0080 - load operating parameter set on wakeup
+  *      DWT_CONFIG       0x0040 - download the AON array into the HIF (configuration download)
+  *      DWT_LOADEUI      0x0008
+  *      DWT_GOTORX       0x0002
+  *      DWT_TANDV        0x0001
+  *
+  *      wake: wake up parameters
+  *      DWT_XTAL_EN      0x10 - keep XTAL running during sleep
+  *      DWT_WAKE_SLPCNT  0x8 - wake up after sleep count
+  *      DWT_WAKE_CS      0x4 - wake up on chip select
+  *      DWT_WAKE_WK      0x2 - wake up on WAKEUP PIN
+  *      DWT_SLP_EN       0x1 - enable sleep/deep sleep functionality
+  *
+  * input parameters
+  * @param mode - config on-wake parameters
+  * @param wake - config wake up parameters
+  *
+  * output parameters
+  *
+  * no return value
+  */
+void dwt_configuresleep(uint16 mode, uint8 wake);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_entersleep()
+ *
+ * @brief This function puts the device into deep sleep or sleep. dwt_configuresleep() should be called first
+ * to configure the sleep and on-wake/wake-up parameters
+ *
+ * input parameters
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_entersleep(void);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_entersleepaftertx(int enable)
+ *
+ * @brief sets the auto TX to sleep bit. This means that after a frame
+ * transmission the device will enter deep sleep mode. The dwt_configuresleep() function
+ * needs to be called before this to configure the on-wake settings
+ *
+ * NOTE: the IRQ line has to be low/inactive (i.e. no pending events)
+ *
+ * input parameters
+ * @param enable - 1 to configure the device to enter deep sleep after TX, 0 - disables the configuration
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_entersleepaftertx(int enable);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_spicswakeup()
+ *
+ * @brief wake up the device from sleep mode using the SPI read,
+ * the device will wake up on chip select line going low if the line is held low for at least 500us.
+ * To define the length depending on the time one wants to hold
+ * the chip select line low, use the following formula:
+ *
+ *      length (bytes) = time (s) * byte_rate (Hz)
+ *
+ * where fastest byte_rate is spi_rate (Hz) / 8 if the SPI is sending the bytes back-to-back.
+ * To save time and power, a system designer could determine byte_rate value more precisely.
+ *
+ * NOTE: Alternatively the device can be waken up with WAKE_UP pin if configured for that operation
+ *
+ * input parameters
+ * @param buff   - this is a pointer to the dummy buffer which will be used in the SPI read transaction used for the WAKE UP of the device
+ * @param length - this is the length of the dummy buffer
+ *
+ * output parameters
+ *
+ * returns DWT_SUCCESS for success, or DWT_ERROR for error
+ */
+int dwt_spicswakeup(uint8 *buff, uint16 length);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_setcallbacks()
+ *
+ * @brief This function is used to register the different callbacks called when one of the corresponding event occurs.
+ *
+ * NOTE: Callbacks can be undefined (set to NULL). In this case, dwt_isr() will process the event as usual but the 'null'
+ * callback will not be called.
+ *
+ * input parameters
+ * @param cbTxDone - the pointer to the TX confirmation event callback function
+ * @param cbRxOk - the pointer to the RX good frame event callback function
+ * @param cbRxTo - the pointer to the RX timeout events callback function
+ * @param cbRxErr - the pointer to the RX error events callback function
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_setcallbacks(dwt_cb_t cbTxDone, dwt_cb_t cbRxOk, dwt_cb_t cbRxTo, dwt_cb_t cbRxErr);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_checkirq()
+ *
+ * @brief This function checks if the IRQ line is active - this is used instead of interrupt handler
+ *
+ * input parameters
+ *
+ * output parameters
+ *
+ * return value is 1 if the IRQS bit is set and 0 otherwise
+ */
+uint8 dwt_checkirq(void);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_isr()
+ *
+ * @brief This is the DW1000's general Interrupt Service Routine. It will process/report the following events:
+ *          - RXFCG (through cbRxOk callback)
+ *          - TXFRS (through cbTxDone callback)
+ *          - RXRFTO/RXPTO (through cbRxTo callback)
+ *          - RXPHE/RXFCE/RXRFSL/RXSFDTO/AFFREJ/LDEERR (through cbRxTo cbRxErr)
+ *        For all events, corresponding interrupts are cleared and necessary resets are performed. In addition, in the RXFCG case,
+ *        received frame information and frame control are read before calling the callback. If double buffering is activated, it
+ *        will also toggle between reception buffers once the reception callback processing has ended.
+ *
+ *        /!\ This version of the ISR supports double buffering but does not support automatic RX re-enabling!
+ *
+ * NOTE:  In PC based system using (Cheetah or ARM) USB to SPI converter there can be no interrupts, however we still need something
+ *        to take the place of it and operate in a polled way. In an embedded system this function should be configured to be triggered
+ *        on any of the interrupts described above.
+
+ * input parameters
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_isr(void);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_isr_lplisten()
+ *
+ * @brief This is the DW1000's Interrupt Service Routine to use when low-power listening scheme is implemented. It will
+ *        only process/report the RXFCG event (through cbRxOk callback).
+ *        It clears RXFCG interrupt and reads received frame information and frame control before calling the callback.
+ *
+ *        /!\ This version of the ISR is designed for single buffering case only!
+ *
+ * input parameters
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_lowpowerlistenisr(void);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn void dwt_setinterrupt()
+ *
+ * @brief This function enables the specified events to trigger an interrupt.
+ * The following events can be enabled:
+ * DWT_INT_TFRS         0x00000080          // frame sent
+ * DWT_INT_RFCG         0x00004000          // frame received with good CRC
+ * DWT_INT_RPHE         0x00001000          // receiver PHY header error
+ * DWT_INT_RFCE         0x00008000          // receiver CRC error
+ * DWT_INT_RFSL         0x00010000          // receiver sync loss error
+ * DWT_INT_RFTO         0x00020000          // frame wait timeout
+ * DWT_INT_RXPTO        0x00200000          // preamble detect timeout
+ * DWT_INT_SFDT         0x04000000          // SFD timeout
+ * DWT_INT_ARFE         0x20000000          // frame rejected (due to frame filtering configuration)
+ *
+ *
+ * input parameters:
+ * @param bitmask - sets the events which will generate interrupt
+ * @param enable - if set the interrupts are enabled else they are cleared
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_setinterrupt( uint32 bitmask, uint8 enable);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_setpanid()
+ *
+ * @brief This is used to set the PAN ID
+ *
+ * input parameters
+ * @param panID - this is the PAN ID
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_setpanid(uint16 panID);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_setaddress16()
+ *
+ * @brief This is used to set 16-bit (short) address
+ *
+ * input parameters
+ * @param shortAddress - this sets the 16 bit short address
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_setaddress16(uint16 shortAddress);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_seteui()
+ *
+ * @brief This is used to set the EUI 64-bit (long) address
+ *
+ * input parameters
+ * @param eui64 - this is the pointer to a buffer that contains the 64bit address
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_seteui(uint8 *eui64);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_geteui()
+ *
+ * @brief This is used to get the EUI 64-bit from the DW1000
+ *
+ * input parameters
+ * @param eui64 - this is the pointer to a buffer that will contain the read 64-bit EUI value
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_geteui(uint8 *eui64);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_otpread()
+ *
+ * @brief This is used to read the OTP data from given address into provided array
+ *
+ * input parameters
+ * @param address - this is the OTP address to read from
+ * @param array - this is the pointer to the array into which to read the data
+ * @param length - this is the number of 32 bit words to read (array needs to be at least this length)
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_otpread(uint32 address, uint32 *array, uint8 length);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_enableframefilter()
+ *
+ * @brief This is used to enable the frame filtering - (the default option is to
+ * accept any data and ACK frames with correct destination address
+ *
+ * input parameters
+ * @param - bitmask - enables/disables the frame filtering options according to
+ *      DWT_FF_NOTYPE_EN        0x000   no frame types allowed
+ *      DWT_FF_COORD_EN         0x002   behave as coordinator (can receive frames with no destination address (PAN ID has to match))
+ *      DWT_FF_BEACON_EN        0x004   beacon frames allowed
+ *      DWT_FF_DATA_EN          0x008   data frames allowed
+ *      DWT_FF_ACK_EN           0x010   ack frames allowed
+ *      DWT_FF_MAC_EN           0x020   mac control frames allowed
+ *      DWT_FF_RSVD_EN          0x040   reserved frame types allowed
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_enableframefilter(uint16 bitmask);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_enableautoack()
+ *
+ * @brief This call enables the auto-ACK feature. If the responseDelayTime (parameter) is 0, the ACK will be sent a.s.a.p.
+ * otherwise it will be sent with a programmed delay (in symbols), max is 255.
+ * NOTE: needs to have frame filtering enabled as well
+ *
+ * input parameters
+ * @param responseDelayTime - if non-zero the ACK is sent after this delay, max is 255.
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_enableautoack(uint8 responseDelayTime);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_setrxaftertxdelay()
+ *
+ * @brief This sets the receiver turn on delay time after a transmission of a frame
+ *
+ * input parameters
+ * @param rxDelayTime - (20 bits) - the delay is in UWB microseconds
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_setrxaftertxdelay(uint32 rxDelayTime);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_rxreset()
+ *
+ * @brief this function resets the receiver of the DW1000
+ *
+ * input parameters:
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_rxreset(void);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_softreset()
+ *
+ * @brief this function resets the DW1000
+ *
+ * input parameters:
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_softreset(void) ;
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_readrxdata()
+ *
+ * @brief This is used to read the data from the RX buffer, from an offset location give by offset parameter
+ *
+ * input parameters
+ * @param buffer - the buffer into which the data will be read
+ * @param length - the length of data to read (in bytes)
+ * @param rxBufferOffset - the offset in the rx buffer from which to read the data
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_readrxdata(uint8 *buffer, uint16 length, uint16 rxBufferOffset);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_readaccdata()
+ *
+ * @brief This is used to read the data from the Accumulator buffer, from an offset location give by offset parameter
+ *
+ * NOTE: Because of an internal memory access delay when reading the accumulator the first octet output is a dummy octet
+ *       that should be discarded. This is true no matter what sub-index the read begins at.
+ *
+ * input parameters
+ * @param buffer - the buffer into which the data will be read
+ * @param length - the length of data to read (in bytes)
+ * @param accOffset - the offset in the acc buffer from which to read the data
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_readaccdata(uint8 *buffer, uint16 length, uint16 rxBufferOffset);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_readdiagnostics()
+ *
+ * @brief this function reads the RX signal quality diagnostic data
+ *
+ * input parameters
+ * @param diagnostics - diagnostic structure pointer, this will contain the diagnostic data read from the DW1000
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_readdiagnostics(dwt_rxdiag_t * diagnostics);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_loadopsettabfromotp()
+ *
+ * @brief This is used to select which Operational Parameter Set table to load from OTP memory
+ *
+ * input parameters
+ * @param ops_sel - Operational Parameter Set table to load:
+ *                  DWT_OPSET_64LEN = 0x0 - load the operational parameter set table for 64 length preamble configuration
+ *                  DWT_OPSET_TIGHT = 0x1 - load the operational parameter set table for tight xtal offsets (<1ppm)
+ *                  DWT_OPSET_DEFLT = 0x2 - load the default operational parameter set table (this is loaded from reset)
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_loadopsettabfromotp(uint8 ops_sel);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_configeventcounters()
+ *
+ * @brief This is used to enable/disable the event counter in the IC
+ *
+ * input parameters
+ * @param - enable - 1 enables (and reset), 0 disables the event counters
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_configeventcounters(int enable);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_readeventcounters()
+ *
+ * @brief This is used to read the event counters in the IC
+ *
+ * input parameters
+ * @param counters - pointer to the dwt_deviceentcnts_t structure which will hold the read data
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_readeventcounters(dwt_deviceentcnts_t *counters);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_otpwriteandverify()
+ *
+ * @brief This is used to program 32-bit value into the DW1000 OTP memory.
+ *
+ * input parameters
+ * @param value - this is the 32-bit value to be programmed into OTP
+ * @param address - this is the 16-bit OTP address into which the 32-bit value is programmed
+ *
+ * output parameters
+ *
+ * returns DWT_SUCCESS for success, or DWT_ERROR for error
+ */
+uint32 dwt_otpwriteandverify(uint32 value, uint16 address);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_setleds()
+ *
+ * @brief This is used to set up Tx/Rx GPIOs which could be used to control LEDs
+ * Note: not completely IC dependent, also needs board with LEDS fitted on right I/O lines
+ *       this function enables GPIOs 2 and 3 which are connected to LED3 and LED4 on EVB1000
+ *
+ * input parameters
+ * @param mode - this is a bit field interpreted as follows:
+ *          - bit 0: 1 to enable LEDs, 0 to disable them
+ *          - bit 1: 1 to make LEDs blink once on init. Only valid if bit 0 is set (enable LEDs)
+ *          - bit 2 to 7: reserved
+ *
+ * output parameters none
+ *
+ * no return value
+ */
+void dwt_setleds(uint8 mode);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_setxtaltrim()
+ *
+ * @brief This is used to adjust the crystal frequency
+ *
+ * input parameters:
+ * @param   value - crystal trim value (in range 0x0 to 0x1F) 31 steps (~1.5ppm per step)
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_setxtaltrim(uint8 value);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_getinitxtaltrim()
+ *
+ * @brief This function returns the value of XTAL trim that has been applied during initialisation (dwt_init). This can
+ *        be either the value read in OTP memory or a default value.
+ *
+ * NOTE: The value returned by this function is the initial value only! It is not updated on dwt_setxtaltrim calls.
+ *
+ * input parameters
+ *
+ * output parameters
+ *
+ * returns the XTAL trim value set upon initialisation
+ */
+uint8 dwt_getinitxtaltrim(void);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_configcwmode()
+ *
+ * @brief this function sets the DW1000 to transmit cw signal at specific channel frequency
+ *
+ * input parameters:
+ * @param chan - specifies the operating channel (e.g. 1, 2, 3, 4, 5, 6 or 7)
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_configcwmode(uint8 chan);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_configcontinuousframemode()
+ *
+ * @brief this function sets the DW1000 to continuous tx frame mode for regulatory approvals testing.
+ *
+ * input parameters:
+ * @param framerepetitionrate - This is a 32-bit value that is used to set the interval between transmissions.
+*  The minimum value is 4. The units are approximately 8 ns. (or more precisely 512/(499.2e6*128) seconds)).
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_configcontinuousframemode(uint32 framerepetitionrate);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_readtempvbat()
+ *
+ * @brief this function reads the battery voltage and temperature of the MP
+ * The values read here will be the current values sampled by DW1000 AtoD converters.
+ * Note on Temperature: the temperature value needs to be converted to give the real temperature
+ * the formula is: 1.13 * reading - 113.0
+ * Note on Voltage: the voltage value needs to be converted to give the real voltage
+ * the formula is: 0.0057 * reading + 2.3
+ *
+ * NB: To correctly read the temperature this read should be done with xtal clock
+ * however that means that the receiver will be switched off, if receiver needs to be on then
+ * the timer is used to make sure the value is stable before reading
+ *
+ * input parameters:
+ * @param fastSPI - set to 1 if SPI rate > than 3MHz is used
+ *
+ * output parameters
+ *
+ * returns  (temp_raw<<8)|(vbat_raw)
+ */
+uint16 dwt_readtempvbat(uint8 fastSPI);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_readwakeuptemp()
+ *
+ * @brief this function reads the temperature of the DW1000 that was sampled
+ * on waking from Sleep/Deepsleep. They are not current values, but read on last
+ * wakeup if DWT_TANDV bit is set in mode parameter of dwt_configuresleep
+ *
+ * input parameters:
+ *
+ * output parameters:
+ *
+ * returns: 8-bit raw temperature sensor value
+ */
+uint8 dwt_readwakeuptemp(void) ;
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_readwakeupvbat()
+ *
+ * @brief this function reads the battery voltage of the DW1000 that was sampled
+ * on waking from Sleep/Deepsleep. They are not current values, but read on last
+ * wakeup if DWT_TANDV bit is set in mode parameter of dwt_configuresleep
+ *
+ * input parameters:
+ *
+ * output parameters:
+ *
+ * returns: 8-bit raw battery voltage sensor value
+ */
+uint8 dwt_readwakeupvbat(void) ;
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_writetodevice()
+ *
+ * @brief  this function is used to write to the DW1000 device registers
+ * Notes:
+ *        1. Firstly we create a header (the first byte is a header byte)
+ *        a. check if sub index is used, if subindexing is used - set bit-6 to 1 to signify that the sub-index address follows the register index byte
+ *        b. set bit-7 (or with 0x80) for write operation
+ *        c. if extended sub address index is used (i.e. if index > 127) set bit-7 of the first sub-index byte following the first header byte
+ *
+ *        2. Write the header followed by the data bytes to the DW1000 device
+ *
+ *
+ * input parameters:
+ * @param recordNumber  - ID of register file or buffer being accessed
+ * @param index         - byte index into register file or buffer being accessed
+ * @param length        - number of bytes being written
+ * @param buffer        - pointer to buffer containing the 'length' bytes to be written
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_writetodevice
+(
+    uint16      recordNumber,   // input parameter - ID of register file or buffer being accessed
+    uint16      index,          // input parameter - byte index into register file or buffer being accessed
+    uint32      length,         // input parameter - number of bytes being written
+    const uint8 *buffer         // input parameter - pointer to buffer containing the 'length' bytes to be written
+) ;
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_readfromdevice()
+ *
+ * @brief  this function is used to read from the DW1000 device registers
+ * Notes:
+ *        1. Firstly we create a header (the first byte is a header byte)
+ *        a. check if sub index is used, if subindexing is used - set bit-6 to 1 to signify that the sub-index address follows the register index byte
+ *        b. set bit-7 (or with 0x80) for write operation
+ *        c. if extended sub address index is used (i.e. if index > 127) set bit-7 of the first sub-index byte following the first header byte
+ *
+ *        2. Write the header followed by the data bytes to the DW1000 device
+ *        3. Store the read data in the input buffer
+ *
+ * input parameters:
+ * @param recordNumber  - ID of register file or buffer being accessed
+ * @param index         - byte index into register file or buffer being accessed
+ * @param length        - number of bytes being read
+ * @param buffer        - pointer to buffer in which to return the read data.
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_readfromdevice
+(
+    uint16  recordNumber,       // input parameter - ID of register file or buffer being accessed
+    uint16  index,              // input parameter - byte index into register file or buffer being accessed
+    uint32  length,             // input parameter - number of bytes being read
+    uint8   *buffer             // input parameter - pointer to buffer in which to return the read data.
+) ;
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_read32bitoffsetreg()
+ *
+ * @brief  this function is used to read 32-bit value from the DW1000 device registers
+ *
+ * input parameters:
+ * @param regFileID - ID of register file or buffer being accessed
+ * @param regOffset - the index into register file or buffer being accessed
+ *
+ * output parameters
+ *
+ * returns 32 bit register value
+ */
+uint32 dwt_read32bitoffsetreg(int regFileID, int regOffset) ;
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_write32bitoffsetreg()
+ *
+ * @brief  this function is used to write 32-bit value to the DW1000 device registers
+ *
+ * input parameters:
+ * @param regFileID - ID of register file or buffer being accessed
+ * @param regOffset - the index into register file or buffer being accessed
+ * @param regval    - the value to write
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_write32bitoffsetreg(int regFileID, int regOffset, uint32 regval);
+
+#define dwt_write32bitreg(x,y)  dwt_write32bitoffsetreg(x,0,y)
+#define dwt_read32bitreg(x)     dwt_read32bitoffsetreg(x,0)
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_read16bitoffsetreg()
+ *
+ * @brief  this function is used to read 16-bit value from the DW1000 device registers
+ *
+ * input parameters:
+ * @param regFileID - ID of register file or buffer being accessed
+ * @param regOffset - the index into register file or buffer being accessed
+ *
+ * output parameters
+ *
+ * returns 16 bit register value
+ */
+uint16 dwt_read16bitoffsetreg(int regFileID, int regOffset);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_write16bitoffsetreg()
+ *
+ * @brief  this function is used to write 16-bit value to the DW1000 device registers
+ *
+ * input parameters:
+ * @param regFileID - ID of register file or buffer being accessed
+ * @param regOffset - the index into register file or buffer being accessed
+ * @param regval    - the value to write
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_write16bitoffsetreg(int regFileID, int regOffset, uint16 regval) ;
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_read8bitoffsetreg()
+ *
+ * @brief  this function is used to read an 8-bit value from the DW1000 device registers
+ *
+ * input parameters:
+ * @param regFileID - ID of register file or buffer being accessed
+ * @param regOffset - the index into register file or buffer being accessed
+ *
+ * output parameters
+ *
+ * returns 8-bit register value
+ */
+uint8 dwt_read8bitoffsetreg(int regFileID, int regOffset);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn dwt_write8bitoffsetreg()
+ *
+ * @brief  this function is used to write an 8-bit value to the DW1000 device registers
+ *
+ * input parameters:
+ * @param regFileID - ID of register file or buffer being accessed
+ * @param regOffset - the index into register file or buffer being accessed
+ * @param regval    - the value to write
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void dwt_write8bitoffsetreg(int regFileID, int regOffset, uint8 regval);
+
+
+/****************************************************************************************************************************************************
+ *
+ * Declaration of platform-dependent lower level functions.
+ *
+ ****************************************************************************************************************************************************/
+
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn writetospi()
+ *
+ * @brief
+ * NB: In porting this to a particular microprocessor, the implementer needs to define the two low
+ * level abstract functions to write to and read from the SPI the definitions should be in deca_spi.c file.
+ * Low level abstract function to write to the SPI
+ * Takes two separate byte buffers for write header and write data
+ * returns 0 for success, or -1 for error
+ *
+ * Note: The body of this function is defined in deca_spi.c and is platform specific
+ *
+ * input parameters:
+ * @param headerLength  - number of bytes header being written
+ * @param headerBuffer  - pointer to buffer containing the 'headerLength' bytes of header to be written
+ * @param bodylength    - number of bytes data being written
+ * @param bodyBuffer    - pointer to buffer containing the 'bodylength' bytes od data to be written
+ *
+ * output parameters
+ *
+ * returns DWT_SUCCESS for success, or DWT_ERROR for error
+ */
+int writetospi(uint16 headerLength, const uint8 *headerBuffer, uint32 bodylength, const uint8 *bodyBuffer);
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn readfromspi()
+ *
+ * @brief
+ * NB: In porting this to a particular microprocessor, the implementer needs to define the two low
+ * level abstract functions to write to and read from the SPI the definitions should be in deca_spi.c file.
+ * Low level abstract function to write to the SPI
+ * Takes two separate byte buffers for write header and write data
+ * returns 0 for success, or -1 for error
+ *
+ * Note: The body of this function is defined in deca_spi.c and is platform specific
+ *
+ * input parameters:
+ * @param headerLength  - number of bytes header to write
+ * @param headerBuffer  - pointer to buffer containing the 'headerLength' bytes of header to write
+ * @param readlength    - number of bytes data being read
+ * @param readBuffer    - pointer to buffer containing to return the data (NB: size required = headerLength + readlength)
+ *
+ * output parameters
+ *
+ * returns DWT_SUCCESS for success (and the position in the buffer at which data begins), or DWT_ERROR for error
+ */
+int readfromspi(uint16 headerLength, const uint8 *headerBuffer, uint32 readlength, uint8 *readBuffer);
+
+// ---------------------------------------------------------------------------
+//
+// NB: The purpose of the deca_mutex.c file is to provide for microprocessor interrupt enable/disable, this is used for
+//     controlling mutual exclusion from critical sections in the code where interrupts and background
+//     processing may interact.  The code using this is kept to a minimum and the disabling time is also
+//     kept to a minimum, so blanket interrupt disable may be the easiest way to provide this.  But at a
+//     minimum those interrupts coming from the decawave device should be disabled/re-enabled by this activity.
+//
+//     In porting this to a particular microprocessor, the implementer may choose to use #defines here
+//     to map these calls transparently to the target system.  Alternatively the appropriate code may
+//     be embedded in the functions provided in the deca_irq.c file.
+//
+// ---------------------------------------------------------------------------
+
+typedef int decaIrqStatus_t ; // Type for remembering IRQ status
+
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn decamutexon()
+ *
+ * @brief This function should disable interrupts. This is called at the start of a critical section
+ * It returns the IRQ state before disable, this value is used to re-enable in decamutexoff call
+ *
+ * Note: The body of this function is defined in deca_mutex.c and is platform specific
+ *
+ * input parameters:
+ *
+ * output parameters
+ *
+ * returns the state of the DW1000 interrupt
+ */
+decaIrqStatus_t decamutexon(void) ;
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn decamutexoff()
+ *
+ * @brief This function should re-enable interrupts, or at least restore their state as returned(&saved) by decamutexon
+ * This is called at the end of a critical section
+ *
+ * Note: The body of this function is defined in deca_mutex.c and is platform specific
+ *
+ * input parameters:
+ * @param s - the state of the DW1000 interrupt as returned by decamutexon
+ *
+ * output parameters
+ *
+ * returns the state of the DW1000 interrupt
+ */
+void decamutexoff(decaIrqStatus_t s) ;
+
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @fn deca_sleep()
+ *
+ * @brief Wait for a given amount of time.
+ * NB: The body of this function is defined in deca_sleep.c and is platform specific
+ *
+ * input parameters:
+ * @param time_ms - time to wait in milliseconds
+ *
+ * output parameters
+ *
+ * no return value
+ */
+void deca_sleep(unsigned int time_ms);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* _DECA_DEVICE_API_H_ */
+
+
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/decadriver/deca_param_types.h	Wed Dec 06 21:35:45 2017 +0000
@@ -0,0 +1,68 @@
+/*! ----------------------------------------------------------------------------
+ *  @file    deca_param_types.h
+ *  @brief   Decawave general type definitions for configuration structures
+ *
+ * @attention
+ *
+ * Copyright 2013 (c) Decawave Ltd, Dublin, Ireland.
+ *
+ * All rights reserved.
+ *
+ */
+#ifndef _DECA_PARAM_TYPES_H_
+#define _DECA_PARAM_TYPES_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+#include "deca_types.h"
+
+#define NUM_BR 3
+#define NUM_PRF 2
+#define NUM_PACS 4
+#define NUM_BW 2            //2 bandwidths are supported
+#define NUM_SFD 2           //supported number of SFDs - standard = 0, non-standard = 1
+#define NUM_CH 6            //supported channels are 1, 2, 3, 4, 5, 7
+#define NUM_CH_SUPPORTED 8  //supported channels are '0', 1, 2, 3, 4, 5, '6', 7
+#define PCODES 25           //supported preamble codes
+
+
+typedef struct {
+    uint32 lo32;
+    uint16 target[NUM_PRF];
+} agc_cfg_struct ;
+
+extern const agc_cfg_struct agc_config ;
+
+//SFD threshold settings for 110k, 850k, 6.8Mb standard and non-standard
+extern const uint16 sftsh[NUM_BR][NUM_SFD];
+
+extern const uint16 dtune1[NUM_PRF];
+
+#define XMLPARAMS_VERSION   (1.17f)
+
+extern const uint32 fs_pll_cfg[NUM_CH];
+extern const uint8 fs_pll_tune[NUM_CH];
+extern const uint8 rx_config[NUM_BW];
+extern const uint32 tx_config[NUM_CH];
+extern const uint8 dwnsSFDlen[NUM_BR]; //length of SFD for each of the bitrates
+extern const uint32 digital_bb_config[NUM_PRF][NUM_PACS];
+extern const uint8 chan_idx[NUM_CH_SUPPORTED];
+
+#define PEAK_MULTPLIER  (0x60) //3 -> (0x3 * 32) & 0x00E0
+#define N_STD_FACTOR    (13)
+#define LDE_PARAM1      (PEAK_MULTPLIER | N_STD_FACTOR)
+
+#define LDE_PARAM3_16 (0x1607)
+#define LDE_PARAM3_64 (0x0607)
+
+extern const uint16 lde_replicaCoeff[PCODES];
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
+
+
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/decadriver/deca_params_init.c	Wed Dec 06 21:35:45 2017 +0000
@@ -0,0 +1,149 @@
+/*! ----------------------------------------------------------------------------
+ *  @file    deca_params_init.c
+ *  @brief   DW1000 configuration parameters
+ *
+ * @attention
+ *
+ * Copyright 2013 (c) Decawave Ltd, Dublin, Ireland.
+ *
+ * All rights reserved.
+ *
+ *
+ * -------------------------------------------------------------------------------------------------------------------
+**/
+#include <stdio.h>
+#include <stdlib.h>
+
+#include "deca_regs.h"
+#include "deca_device_api.h"
+#include "deca_param_types.h"
+
+
+//-----------------------------------------
+// map the channel number to the index in the configuration arrays below
+// 0th element is chan 1, 1st is chan 2, 2nd is chan 3, 3rd is chan 4, 4th is chan 5, 5th is chan 7
+const uint8 chan_idx[NUM_CH_SUPPORTED] = {0, 0, 1, 2, 3, 4, 0, 5};
+
+//-----------------------------------------
+const uint32 tx_config[NUM_CH] =
+{
+    RF_TXCTRL_CH1,
+    RF_TXCTRL_CH2,
+    RF_TXCTRL_CH3,
+    RF_TXCTRL_CH4,
+    RF_TXCTRL_CH5,
+    RF_TXCTRL_CH7,
+};
+
+//Frequency Synthesiser - PLL configuration
+const uint32 fs_pll_cfg[NUM_CH] =
+{
+    FS_PLLCFG_CH1,
+    FS_PLLCFG_CH2,
+    FS_PLLCFG_CH3,
+    FS_PLLCFG_CH4,
+    FS_PLLCFG_CH5,
+    FS_PLLCFG_CH7
+};
+
+//Frequency Synthesiser - PLL tuning
+const uint8 fs_pll_tune[NUM_CH] =
+{
+    FS_PLLTUNE_CH1,
+    FS_PLLTUNE_CH2,
+    FS_PLLTUNE_CH3,
+    FS_PLLTUNE_CH4,
+    FS_PLLTUNE_CH5,
+    FS_PLLTUNE_CH7
+};
+
+//bandwidth configuration
+const uint8 rx_config[NUM_BW] =
+{
+    RF_RXCTRLH_NBW,
+    RF_RXCTRLH_WBW
+};
+
+
+const agc_cfg_struct agc_config =
+{
+    AGC_TUNE2_VAL,
+    { AGC_TUNE1_16M , AGC_TUNE1_64M }  //adc target
+};
+
+//DW non-standard SFD length for 110k, 850k and 6.81M
+const uint8 dwnsSFDlen[NUM_BR] =
+{
+    DW_NS_SFD_LEN_110K,
+    DW_NS_SFD_LEN_850K,
+    DW_NS_SFD_LEN_6M8
+};
+
+// SFD Threshold
+const uint16 sftsh[NUM_BR][NUM_SFD] =
+{
+    {
+        DRX_TUNE0b_110K_STD,
+        DRX_TUNE0b_110K_NSTD
+    },
+    {
+        DRX_TUNE0b_850K_STD,
+        DRX_TUNE0b_850K_NSTD
+    },
+    {
+        DRX_TUNE0b_6M8_STD,
+        DRX_TUNE0b_6M8_NSTD
+    }
+};
+
+const uint16 dtune1[NUM_PRF] =
+{
+    DRX_TUNE1a_PRF16,
+    DRX_TUNE1a_PRF64
+};
+
+const uint32 digital_bb_config[NUM_PRF][NUM_PACS] =
+{
+    {
+        DRX_TUNE2_PRF16_PAC8,
+        DRX_TUNE2_PRF16_PAC16,
+        DRX_TUNE2_PRF16_PAC32,
+        DRX_TUNE2_PRF16_PAC64
+    },
+    {
+        DRX_TUNE2_PRF64_PAC8,
+        DRX_TUNE2_PRF64_PAC16,
+        DRX_TUNE2_PRF64_PAC32,
+        DRX_TUNE2_PRF64_PAC64
+    }
+};
+
+const uint16 lde_replicaCoeff[PCODES] =
+{
+    0, // No preamble code 0
+    LDE_REPC_PCODE_1,
+    LDE_REPC_PCODE_2,
+    LDE_REPC_PCODE_3,
+    LDE_REPC_PCODE_4,
+    LDE_REPC_PCODE_5,
+    LDE_REPC_PCODE_6,
+    LDE_REPC_PCODE_7,
+    LDE_REPC_PCODE_8,
+    LDE_REPC_PCODE_9,
+    LDE_REPC_PCODE_10,
+    LDE_REPC_PCODE_11,
+    LDE_REPC_PCODE_12,
+    LDE_REPC_PCODE_13,
+    LDE_REPC_PCODE_14,
+    LDE_REPC_PCODE_15,
+    LDE_REPC_PCODE_16,
+    LDE_REPC_PCODE_17,
+    LDE_REPC_PCODE_18,
+    LDE_REPC_PCODE_19,
+    LDE_REPC_PCODE_20,
+    LDE_REPC_PCODE_21,
+    LDE_REPC_PCODE_22,
+    LDE_REPC_PCODE_23,
+    LDE_REPC_PCODE_24
+};
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/decadriver/deca_regs.h	Wed Dec 06 21:35:45 2017 +0000
@@ -0,0 +1,1350 @@
+/*! ------------------------------------------------------------------------------------------------------------------
+ * @file    deca_regs.h
+ * @brief   DW1000 Register Definitions
+ *          This file supports assembler and C development for DW1000 enabled devices
+ *
+ * @attention
+ *
+ * Copyright 2013 (c) Decawave Ltd, Dublin, Ireland.
+ *
+ * All rights reserved.
+ *
+ */
+
+#ifndef _DECA_REGS_H_
+#define _DECA_REGS_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "deca_version.h"
+
+
+/****************************************************************************//**
+ * @brief Bit definitions for register DEV_ID
+**/
+#define DEV_ID_ID               0x00            /* Device ID register, includes revision info (0xDECA0130) */
+#define DEV_ID_LEN              (4)
+/* mask and shift */
+#define DEV_ID_REV_MASK         0x0000000FUL    /* Revision */
+#define DEV_ID_VER_MASK         0x000000F0UL    /* Version */
+#define DEV_ID_MODEL_MASK       0x0000FF00UL    /* The MODEL identifies the device. The DW1000 is device type 0x01 */
+#define DEV_ID_RIDTAG_MASK      0xFFFF0000UL    /* Register Identification Tag 0XDECA */
+
+/****************************************************************************//**
+ * @brief Bit definitions for register EUI_64
+**/
+#define EUI_64_ID               0x01            /* IEEE Extended Unique Identifier (63:0) */
+#define EUI_64_OFFSET           0x00
+#define EUI_64_LEN              (8)
+
+/****************************************************************************//**
+ * @brief Bit definitions for register PANADR
+**/
+#define PANADR_ID               0x03            /* PAN ID (31:16) and Short Address (15:0) */
+#define PANADR_LEN              (4)
+/*mask and shift */
+#define PANADR_SHORT_ADDR_OFFSET 0              /* In bytes */
+#define PANADR_SHORT_ADDR_MASK  0x0000FFFFUL    /* Short Address */
+#define PANADR_PAN_ID_OFFSET     2              /* In bytes */
+#define PANADR_PAN_ID_MASK      0xFFFF00F0UL    /* PAN Identifier */
+
+/****************************************************************************//**
+ * @brief Bit definitions for register 0x05
+**/
+#define REG_05_ID_RESERVED      0x05
+
+/****************************************************************************//**
+ * @brief Bit definitions for register SYS_CFG
+**/
+#define SYS_CFG_ID              0x04            /* System Configuration (31:0) */
+#define SYS_CFG_LEN             (4)
+/*mask and shift */
+#define SYS_CFG_MASK            0xF047FFFFUL    /* access mask to SYS_CFG_ID */
+#define SYS_CFG_FF_ALL_EN       0x000001FEUL    /* Frame filtering options all frames allowed */
+/*offset 0 */
+#define SYS_CFG_FFE             0x00000001UL    /* Frame Filtering Enable. This bit enables the frame filtering functionality */
+#define SYS_CFG_FFBC            0x00000002UL    /* Frame Filtering Behave as a Co-ordinator */
+#define SYS_CFG_FFAB            0x00000004UL    /* Frame Filtering Allow Beacon frame reception */
+#define SYS_CFG_FFAD            0x00000008UL    /* Frame Filtering Allow Data frame reception */
+#define SYS_CFG_FFAA            0x00000010UL    /* Frame Filtering Allow Acknowledgment frame reception */
+#define SYS_CFG_FFAM            0x00000020UL    /* Frame Filtering Allow MAC command frame reception */
+#define SYS_CFG_FFAR            0x00000040UL    /* Frame Filtering Allow Reserved frame types */
+#define SYS_CFG_FFA4            0x00000080UL    /* Frame Filtering Allow frames with frame type field of 4, (binary 100) */
+/*offset 8 */
+#define SYS_CFG_FFA5            0x00000100UL    /* Frame Filtering Allow frames with frame type field of 5, (binary 101) */
+#define SYS_CFG_HIRQ_POL        0x00000200UL    /* Host interrupt polarity */
+#define SYS_CFG_SPI_EDGE        0x00000400UL    /* SPI data launch edge */
+#define SYS_CFG_DIS_FCE         0x00000800UL    /* Disable frame check error handling */
+#define SYS_CFG_DIS_DRXB        0x00001000UL    /* Disable Double RX Buffer */
+#define SYS_CFG_DIS_PHE         0x00002000UL    /* Disable receiver abort on PHR error */
+#define SYS_CFG_DIS_RSDE        0x00004000UL    /* Disable Receiver Abort on RSD error */
+#define SYS_CFG_FCS_INIT2F      0x00008000UL    /* initial seed value for the FCS generation and checking function */
+/*offset 16 */
+#define SYS_CFG_PHR_MODE_SHFT   16
+#define SYS_CFG_PHR_MODE_00     0x00000000UL    /* Standard Frame mode */
+#define SYS_CFG_PHR_MODE_11     0x00030000UL    /* Long Frames mode */
+#define SYS_CFG_DIS_STXP        0x00040000UL    /* Disable Smart TX Power control */
+#define SYS_CFG_RXM110K         0x00400000UL    /* Receiver Mode 110 kbps data rate */
+/*offset 24 */
+#define SYS_CFG_RXWTOE          0x10000000UL    /* Receive Wait Timeout Enable. */
+#define SYS_CFG_RXAUTR          0x20000000UL    /* Receiver Auto-Re-enable. This bit is used to cause the receiver to re-enable automatically */
+#define SYS_CFG_AUTOACK         0x40000000UL    /* Automatic Acknowledgement Enable */
+#define SYS_CFG_AACKPEND        0x80000000UL    /* Automatic Acknowledgement Pending bit control */
+
+
+/****************************************************************************//**
+ * @brief Bit definitions for register SYS_TIME
+**/
+#define SYS_TIME_ID             0x06            /* System Time Counter (40-bit) */
+#define SYS_TIME_OFFSET         0x00
+#define SYS_TIME_LEN            (5)             /* Note 40 bit register */
+
+
+/****************************************************************************//**
+ * @brief Bit definitions for register  0x07
+**/
+#define REG_07_ID_RESERVED      0x07
+
+/****************************************************************************//**
+ * @brief Bit definitions for register TX_FCTRL
+**/
+#define TX_FCTRL_ID             0x08            /* Transmit Frame Control */
+#define TX_FCTRL_LEN            (5)             /* Note 40 bit register */
+/*masks (low 32 bit) */
+#define TX_FCTRL_TFLEN_MASK     0x0000007FUL    /* bit mask to access Transmit Frame Length */
+#define TX_FCTRL_TFLE_MASK      0x00000380UL    /* bit mask to access Transmit Frame Length Extension */
+#define TX_FCTRL_FLE_MASK       0x000003FFUL    /* bit mask to access Frame Length field */
+#define TX_FCTRL_TXBR_MASK      0x00006000UL    /* bit mask to access Transmit Bit Rate */
+#define TX_FCTRL_TXPRF_MASK     0x00030000UL    /* bit mask to access Transmit Pulse Repetition Frequency */
+#define TX_FCTRL_TXPSR_MASK     0x000C0000UL    /* bit mask to access Transmit Preamble Symbol Repetitions (PSR). */
+#define TX_FCTRL_PE_MASK        0x00300000UL    /* bit mask to access Preamble Extension */
+#define TX_FCTRL_TXPSR_PE_MASK  0x003C0000UL    /* bit mask to access Transmit Preamble Symbol Repetitions (PSR). */
+#define TX_FCTRL_SAFE_MASK_32   0xFFFFE3FFUL    /* FSCTRL has fields which should always be writen zero */
+/*offset 0 */
+/*offset 8 */
+#define TX_FCTRL_TXBR_110k      0x00000000UL    /* Transmit Bit Rate = 110k */
+#define TX_FCTRL_TXBR_850k      0x00002000UL    /* Transmit Bit Rate = 850k */
+#define TX_FCTRL_TXBR_6M        0x00004000UL    /* Transmit Bit Rate = 6.8M */
+#define TX_FCTRL_TXBR_SHFT      (13)            /* shift to access Data Rate field */
+#define TX_FCTRL_TR             0x00008000UL    /* Transmit Ranging enable */
+#define TX_FCTRL_TR_SHFT        (15)            /* shift to access Ranging bit */
+/*offset 16 */
+#define TX_FCTRL_TXPRF_SHFT     (16)            /* shift to access Pulse Repetition Frequency field */
+#define TX_FCTRL_TXPRF_4M       0x00000000UL    /* Transmit Pulse Repetition Frequency = 4 Mhz */
+#define TX_FCTRL_TXPRF_16M      0x00010000UL    /* Transmit Pulse Repetition Frequency = 16 Mhz */
+#define TX_FCTRL_TXPRF_64M      0x00020000UL    /* Transmit Pulse Repetition Frequency = 64 Mhz */
+#define TX_FCTRL_TXPSR_SHFT     (18)            /* shift to access Preamble Symbol Repetitions field */
+#define TX_FCTRL_PE_SHFT        (20)            /* shift to access Preamble length Extension to allow specification of non-standard values */
+#define TX_FCTRL_TXPSR_PE_16    0x00000000UL    /* bit mask to access Preamble Extension = 16 */
+#define TX_FCTRL_TXPSR_PE_64    0x00040000UL    /* bit mask to access Preamble Extension = 64 */
+#define TX_FCTRL_TXPSR_PE_128   0x00140000UL    /* bit mask to access Preamble Extension = 128 */
+#define TX_FCTRL_TXPSR_PE_256   0x00240000UL    /* bit mask to access Preamble Extension = 256 */
+#define TX_FCTRL_TXPSR_PE_512   0x00340000UL    /* bit mask to access Preamble Extension = 512 */
+#define TX_FCTRL_TXPSR_PE_1024  0x00080000UL    /* bit mask to access Preamble Extension = 1024 */
+#define TX_FCTRL_TXPSR_PE_1536  0x00180000UL    /* bit mask to access Preamble Extension = 1536 */
+#define TX_FCTRL_TXPSR_PE_2048  0x00280000UL    /* bit mask to access Preamble Extension = 2048 */
+#define TX_FCTRL_TXPSR_PE_4096  0x000C0000UL    /* bit mask to access Preamble Extension = 4096 */
+/*offset 22 */
+#define TX_FCTRL_TXBOFFS_SHFT   (22)            /* Shift to access transmit buffer index offset */
+#define TX_FCTRL_TXBOFFS_MASK   0xFFC00000UL    /* bit mask to access Transmit buffer index offset 10-bit field */
+/*offset 32 */
+#define TX_FCTRL_IFSDELAY_MASK  0xFF00000000ULL /* bit mask to access Inter-Frame Spacing field */
+
+/****************************************************************************//**
+ * @brief Bit definitions for register TX_BUFFER
+**/
+#define TX_BUFFER_ID            0x09            /* Transmit Data Buffer */
+#define TX_BUFFER_LEN           (1024)
+
+/****************************************************************************//**
+ * @brief Bit definitions for register  DX_TIME
+**/
+#define DX_TIME_ID              0x0A            /* Delayed Send or Receive Time (40-bit) */
+#define DX_TIME_LEN             (5)
+
+/****************************************************************************//**
+ * @brief Bit definitions for register 0x08
+**/
+#define REG_0B_ID_RESERVED      0x0B
+
+/****************************************************************************//**
+ * @brief Bit definitions for register RX_FWTO
+**/
+#define RX_FWTO_ID              0x0C            /* Receive Frame Wait Timeout Period */
+#define RX_FWTO_OFFSET          0x00
+#define RX_FWTO_LEN             (2)             /* doc bug*/
+/*mask and shift */
+#define RX_FWTO_MASK            0xFFFF
+
+/****************************************************************************//**
+ * @brief Bit definitions for register SYS_CTRL
+**/
+#define SYS_CTRL_ID             0x0D            /* System Control Register */
+#define SYS_CTRL_OFFSET         0x00
+#define SYS_CTRL_LEN            (4)
+/*masks */
+#define SYS_CTRL_MASK_32        0x010003CFUL    /* System Control Register access mask (all unused fields should always be writen as zero) */
+/*offset 0 */
+#define SYS_CTRL_SFCST          0x00000001UL    /* Suppress Auto-FCS Transmission (on this frame) */
+#define SYS_CTRL_TXSTRT         0x00000002UL    /* Start Transmitting Now */
+#define SYS_CTRL_TXDLYS         0x00000004UL    /* Transmitter Delayed Sending (initiates sending when SYS_TIME == TXD_TIME */
+#define SYS_CTRL_CANSFCS        0x00000008UL    /* Cancel Suppression of auto-FCS transmission (on the current frame) */
+#define SYS_CTRL_TRXOFF         0x00000040UL    /* Transceiver Off. Force Transciever OFF abort TX or RX immediately */
+#define SYS_CTRL_WAIT4RESP      0x00000080UL    /* Wait for Response */
+/*offset 8 */
+#define SYS_CTRL_RXENAB         0x00000100UL    /* Enable Receiver Now */
+#define SYS_CTRL_RXDLYE         0x00000200UL    /* Receiver Delayed Enable (Enables Receiver when SY_TIME[0x??] == RXD_TIME[0x??] CHECK comment*/
+/*offset 16 */
+/*offset 24 */
+#define SYS_CTRL_HSRBTOGGLE     0x01000000UL    /* Host side receiver buffer pointer toggle - toggles 0/1 host side data set pointer */
+#define SYS_CTRL_HRBT           (SYS_CTRL_HSRBTOGGLE)
+#define SYS_CTRL_HRBT_OFFSET    (3)
+
+/****************************************************************************//**
+ * @brief Bit definitions for register  SYS_MASK
+**/
+#define SYS_MASK_ID             0x0E            /* System Event Mask Register */
+#define SYS_MASK_LEN            (4)
+/*masks */
+#define SYS_MASK_MASK_32        0x3FF7FFFEUL    /* System Event Mask Register access mask (all unused fields should always be writen as zero) */
+/*offset 0 */
+#define SYS_MASK_MCPLOCK        0x00000002UL    /* Mask clock PLL lock event    */
+#define SYS_MASK_MESYNCR        0x00000004UL    /* Mask clock PLL lock event    */
+#define SYS_MASK_MAAT           0x00000008UL    /* Mask automatic acknowledge trigger event */
+#define SYS_MASK_MTXFRB         0x00000010UL    /* Mask transmit frame begins event */
+#define SYS_MASK_MTXPRS         0x00000020UL    /* Mask transmit preamble sent event    */
+#define SYS_MASK_MTXPHS         0x00000040UL    /* Mask transmit PHY Header Sent event  */
+#define SYS_MASK_MTXFRS         0x00000080UL    /* Mask transmit frame sent event   */
+/*offset 8 */
+#define SYS_MASK_MRXPRD         0x00000100UL    /* Mask receiver preamble detected event    */
+#define SYS_MASK_MRXSFDD        0x00000200UL    /* Mask receiver SFD detected event */
+#define SYS_MASK_MLDEDONE       0x00000400UL    /* Mask LDE processing done event   */
+#define SYS_MASK_MRXPHD         0x00000800UL    /* Mask receiver PHY header detect event    */
+#define SYS_MASK_MRXPHE         0x00001000UL    /* Mask receiver PHY header error event */
+#define SYS_MASK_MRXDFR         0x00002000UL    /* Mask receiver data frame ready event */
+#define SYS_MASK_MRXFCG         0x00004000UL    /* Mask receiver FCS good event */
+#define SYS_MASK_MRXFCE         0x00008000UL    /* Mask receiver FCS error event    */
+/*offset 16 */
+#define SYS_MASK_MRXRFSL        0x00010000UL    /* Mask receiver Reed Solomon Frame Sync Loss event */
+#define SYS_MASK_MRXRFTO        0x00020000UL    /* Mask Receive Frame Wait Timeout event    */
+#define SYS_MASK_MLDEERR        0x00040000UL    /* Mask leading edge detection processing error event   */
+#define SYS_MASK_MRXOVRR        0x00100000UL    /* Mask Receiver Overrun event  */
+#define SYS_MASK_MRXPTO         0x00200000UL    /* Mask Preamble detection timeout event    */
+#define SYS_MASK_MGPIOIRQ       0x00400000UL    /* Mask GPIO interrupt event    */
+#define SYS_MASK_MSLP2INIT      0x00800000UL    /* Mask SLEEP to INIT event */
+/*offset 24*/
+#define SYS_MASK_MRFPLLLL       0x01000000UL    /* Mask RF PLL Loosing Lock warning event   */
+#define SYS_MASK_MCPLLLL        0x02000000UL    /* Mask Clock PLL Loosing Lock warning event    */
+#define SYS_MASK_MRXSFDTO       0x04000000UL    /* Mask Receive SFD timeout event   */
+#define SYS_MASK_MHPDWARN       0x08000000UL    /* Mask Half Period Delay Warning event */
+#define SYS_MASK_MTXBERR        0x10000000UL    /* Mask Transmit Buffer Error event */
+#define SYS_MASK_MAFFREJ        0x20000000UL    /* Mask Automatic Frame Filtering rejection event   */
+
+/****************************************************************************//**
+ * @brief Bit definitions for register SYS_STATUS
+**/
+#define SYS_STATUS_ID           0x0F            /* System event Status Register */
+#define SYS_STATUS_OFFSET       0x00
+#define SYS_STATUS_LEN          (5)             /* Note 40 bit register */
+/*masks */
+#define SYS_STATUS_MASK_32      0xFFF7FFFFUL    /* System event Status Register access mask (all unused fields should always be writen as zero) */
+/*offset 0 */
+#define SYS_STATUS_IRQS         0x00000001UL    /* Interrupt Request Status READ ONLY */
+#define SYS_STATUS_CPLOCK       0x00000002UL    /* Clock PLL Lock */
+#define SYS_STATUS_ESYNCR       0x00000004UL    /* External Sync Clock Reset */
+#define SYS_STATUS_AAT          0x00000008UL    /* Automatic Acknowledge Trigger */
+#define SYS_STATUS_TXFRB        0x00000010UL    /* Transmit Frame Begins */
+#define SYS_STATUS_TXPRS        0x00000020UL    /* Transmit Preamble Sent */
+#define SYS_STATUS_TXPHS        0x00000040UL    /* Transmit PHY Header Sent */
+#define SYS_STATUS_TXFRS        0x00000080UL    /* Transmit Frame Sent: This is set when the transmitter has completed the sending of a frame */
+/*offset 8 */
+#define SYS_STATUS_RXPRD        0x00000100UL    /* Receiver Preamble Detected status */
+#define SYS_STATUS_RXSFDD       0x00000200UL    /* Receiver Start Frame Delimiter Detected. */
+#define SYS_STATUS_LDEDONE      0x00000400UL    /* LDE processing done */
+#define SYS_STATUS_RXPHD        0x00000800UL    /* Receiver PHY Header Detect */
+#define SYS_STATUS_RXPHE        0x00001000UL    /* Receiver PHY Header Error */
+#define SYS_STATUS_RXDFR        0x00002000UL    /* Receiver Data Frame Ready */
+#define SYS_STATUS_RXFCG        0x00004000UL    /* Receiver FCS Good */
+#define SYS_STATUS_RXFCE        0x00008000UL    /* Receiver FCS Error */
+/*offset 16 */
+#define SYS_STATUS_RXRFSL       0x00010000UL    /* Receiver Reed Solomon Frame Sync Loss */
+#define SYS_STATUS_RXRFTO       0x00020000UL    /* Receive Frame Wait Timeout */
+#define SYS_STATUS_LDEERR       0x00040000UL    /* Leading edge detection processing error */
+#define SYS_STATUS_reserved     0x00080000UL    /* bit19 reserved */
+#define SYS_STATUS_RXOVRR       0x00100000UL    /* Receiver Overrun */
+#define SYS_STATUS_RXPTO        0x00200000UL    /* Preamble detection timeout */
+#define SYS_STATUS_GPIOIRQ      0x00400000UL    /* GPIO interrupt */
+#define SYS_STATUS_SLP2INIT     0x00800000UL    /* SLEEP to INIT */
+/*offset 24 */
+#define SYS_STATUS_RFPLL_LL     0x01000000UL    /* RF PLL Losing Lock */
+#define SYS_STATUS_CLKPLL_LL    0x02000000UL    /* Clock PLL Losing Lock */
+#define SYS_STATUS_RXSFDTO      0x04000000UL    /* Receive SFD timeout */
+#define SYS_STATUS_HPDWARN      0x08000000UL    /* Half Period Delay Warning */
+#define SYS_STATUS_TXBERR       0x10000000UL    /* Transmit Buffer Error */
+#define SYS_STATUS_AFFREJ       0x20000000UL    /* Automatic Frame Filtering rejection */
+#define SYS_STATUS_HSRBP        0x40000000UL    /* Host Side Receive Buffer Pointer */
+#define SYS_STATUS_ICRBP        0x80000000UL    /* IC side Receive Buffer Pointer READ ONLY */
+/*offset 32 */
+#define SYS_STATUS_RXRSCS       0x0100000000ULL /* Receiver Reed-Solomon Correction Status */
+#define SYS_STATUS_RXPREJ       0x0200000000ULL /* Receiver Preamble Rejection */
+#define SYS_STATUS_TXPUTE       0x0400000000ULL /* Transmit power up time error */
+
+#define SYS_STATUS_TXERR        (0x0408)        /* These bits are the 16 high bits of status register TXPUTE and HPDWARN flags */
+
+/* All RX events after a correct packet reception mask. */
+#define SYS_STATUS_ALL_RX_GOOD (SYS_STATUS_RXDFR | SYS_STATUS_RXFCG | SYS_STATUS_RXPRD | \
+                                SYS_STATUS_RXSFDD | SYS_STATUS_RXPHD | SYS_STATUS_LDEDONE)
+
+/* All double buffer events mask. */
+#define SYS_STATUS_ALL_DBLBUFF (SYS_STATUS_RXDFR | SYS_STATUS_RXFCG)
+
+/* All RX errors mask. */
+#define SYS_STATUS_ALL_RX_ERR  (SYS_STATUS_RXPHE | SYS_STATUS_RXFCE | SYS_STATUS_RXRFSL | SYS_STATUS_RXSFDTO \
+                                | SYS_STATUS_AFFREJ | SYS_STATUS_LDEERR)
+
+/* User defined RX timeouts (frame wait timeout and preamble detect timeout) mask. */
+#define SYS_STATUS_ALL_RX_TO   (SYS_STATUS_RXRFTO | SYS_STATUS_RXPTO)
+
+/* All TX events mask. */
+#define SYS_STATUS_ALL_TX      (SYS_STATUS_AAT | SYS_STATUS_TXFRB | SYS_STATUS_TXPRS | \
+                                SYS_STATUS_TXPHS | SYS_STATUS_TXFRS )
+
+
+/****************************************************************************//**
+ * @brief Bit definitions for register RX_FINFO
+**/
+#define RX_FINFO_ID             0x10            /* RX Frame Information (in double buffer set) */
+#define RX_FINFO_OFFSET         0x00
+#define RX_FINFO_LEN            (4)
+/*mask and shift */
+#define RX_FINFO_MASK_32        0xFFFFFBFFUL    /* System event Status Register access mask (all unused fields should always be writen as zero) */
+#define RX_FINFO_RXFLEN_MASK    0x0000007FUL    /* Receive Frame Length (0 to 127) */
+#define RX_FINFO_RXFLE_MASK     0x00000380UL    /* Receive Frame Length Extension (0 to 7)<<7 */
+#define RX_FINFO_RXFL_MASK_1023 0x000003FFUL    /* Receive Frame Length Extension (0 to 1023) */
+
+#define RX_FINFO_RXNSPL_MASK    0x00001800UL    /* Receive Non-Standard Preamble Length */
+#define RX_FINFO_RXPSR_MASK     0x000C0000UL    /* RX Preamble Repetition. 00 = 16 symbols, 01 = 64 symbols, 10 = 1024 symbols, 11 = 4096 symbols */
+
+#define RX_FINFO_RXPEL_MASK     0x000C1800UL    /* Receive Preamble Length = RXPSR+RXNSPL */
+#define RX_FINFO_RXPEL_64       0x00040000UL    /* Receive Preamble length = 64 */
+#define RX_FINFO_RXPEL_128      0x00040800UL    /* Receive Preamble length = 128 */
+#define RX_FINFO_RXPEL_256      0x00041000UL    /* Receive Preamble length = 256 */
+#define RX_FINFO_RXPEL_512      0x00041800UL    /* Receive Preamble length = 512 */
+#define RX_FINFO_RXPEL_1024     0x00080000UL    /* Receive Preamble length = 1024 */
+#define RX_FINFO_RXPEL_1536     0x00080800UL    /* Receive Preamble length = 1536 */
+#define RX_FINFO_RXPEL_2048     0x00081000UL    /* Receive Preamble length = 2048 */
+#define RX_FINFO_RXPEL_4096     0x000C0000UL    /* Receive Preamble length = 4096 */
+
+#define RX_FINFO_RXBR_MASK      0x00006000UL    /* Receive Bit Rate report. This field reports the received bit rate */
+#define RX_FINFO_RXBR_110k      0x00000000UL    /* Received bit rate = 110 kbps */
+#define RX_FINFO_RXBR_850k      0x00002000UL    /* Received bit rate = 850 kbps */
+#define RX_FINFO_RXBR_6M        0x00004000UL    /* Received bit rate = 6.8 Mbps */
+#define RX_FINFO_RXBR_SHIFT     (13)
+
+#define RX_FINFO_RNG            0x00008000UL    /* Receiver Ranging. Ranging bit in the received PHY header identifying the frame as a ranging packet. */
+#define RX_FINFO_RNG_SHIFT      (15)
+
+#define RX_FINFO_RXPRF_MASK     0x00030000UL    /* RX Pulse Repetition Rate report */
+#define RX_FINFO_RXPRF_16M      0x00010000UL    /* PRF being employed in the receiver = 16M */
+#define RX_FINFO_RXPRF_64M      0x00020000UL    /* PRF being employed in the receiver = 64M */
+#define RX_FINFO_RXPRF_SHIFT    (16)
+
+#define RX_FINFO_RXPACC_MASK    0xFFF00000UL    /* Preamble Accumulation Count */
+#define RX_FINFO_RXPACC_SHIFT   (20)
+
+
+/****************************************************************************//**
+ * @brief Bit definitions for register RX_BUFFER
+**/
+#define RX_BUFFER_ID            0x11            /* Receive Data Buffer (in double buffer set) */
+#define RX_BUFFER_LEN           (1024)
+
+
+/****************************************************************************//**
+ * @brief Bit definitions for register RX_FQUAL
+**/
+#define RX_FQUAL_ID             0x12            /* Rx Frame Quality information (in double buffer set) */
+#define RX_FQUAL_LEN            (8)             /* note 64 bit register*/
+/*mask and shift */
+/*offset 0 */
+#define RX_EQUAL_STD_NOISE_MASK 0x0000FFFFULL   /* Standard Deviation of Noise */
+#define RX_EQUAL_STD_NOISE_SHIFT (0)
+#define STD_NOISE_MASK          RX_EQUAL_STD_NOISE_MASK
+#define STD_NOISE_SHIFT         RX_EQUAL_STD_NOISE_SHIFT
+/*offset 16 */
+#define RX_EQUAL_FP_AMPL2_MASK  0xFFFF0000ULL   /* First Path Amplitude point 2 */
+#define RX_EQUAL_FP_AMPL2_SHIFT (16)
+#define FP_AMPL2_MASK           RX_EQUAL_FP_AMPL2_MASK
+#define FP_AMPL2_SHIFT          RX_EQUAL_FP_AMPL2_SHIFT
+/*offset 32*/
+#define RX_EQUAL_PP_AMPL3_MASK  0x0000FFFF00000000ULL   /* First Path Amplitude point 3 */
+#define RX_EQUAL_PP_AMPL3_SHIFT (32)
+#define PP_AMPL3_MASK           RX_EQUAL_PP_AMPL3_MASK
+#define PP_AMPL3_SHIFT          RX_EQUAL_PP_AMPL3_SHIFT
+/*offset 48*/
+#define RX_EQUAL_CIR_MXG_MASK   0xFFFF000000000000ULL   /* Channel Impulse Response Max Growth */
+#define RX_EQUAL_CIR_MXG_SHIFT  (48)
+#define CIR_MXG_MASK            RX_EQUAL_CIR_MXG_MASK
+#define CIR_MXG_SHIFT           RX_EQUAL_CIR_MXG_SHIFT
+
+
+
+/****************************************************************************//**
+ * @brief Bit definitions for register RX_TTCKI
+ *      The value here is the interval over which the timing offset reported
+ *      in the RXTOFS field of Register file: 0x14 – RX_TTCKO is measured.
+ *      The clock offset is calculated by dividing RXTTCKI by RXTOFS.
+ *      The value in RXTTCKI will take just one of two values depending on the PRF: 0x01F00000 @ 16 MHz PRF,
+ *      and 0x01FC0000 @ 64 MHz PRF.
+**/
+#define RX_TTCKI_ID             0x13            /* Receiver Time Tracking Interval (in double buffer set) */
+#define RX_TTCKI_LEN            (4)
+
+/****************************************************************************//**
+ * @brief Bit definitions for register RX_TTCKO
+**/
+#define RX_TTCKO_ID             0x14            /* Receiver Time Tracking Offset (in double buffer set) */
+#define RX_TTCKO_LEN            (5)             /* Note 40 bit register */
+/*mask and shift */
+#define RX_TTCKO_MASK_32        0xFF07FFFFUL    /* Receiver Time Tracking Offset access mask (all unused fields should always be writen as zero) */
+/*offset 0 */
+#define RX_TTCKO_RXTOFS_MASK    0x0007FFFFUL    /* RX time tracking offset. This RXTOFS value is a 19-bit signed quantity*/
+/*offset 24 */
+#define RX_TTCKO_RSMPDEL_MASK   0xFF000000UL    /* This 8-bit field reports an internal re-sampler delay value */
+/*offset 32 */
+#define RX_TTCKO_RCPHASE_MASK   0x7F0000000000ULL   /* This 7-bit field reports the receive carrier phase adjustment at time the ranging timestamp is made. */
+
+
+/****************************************************************************//**
+ * @brief Bit definitions for register RX_TIME
+**/
+#define RX_TIME_ID              0x15            /* Receive Message Time of Arrival (in double buffer set) */
+#define RX_TIME_LLEN            (14)
+#define RX_TIME_RX_STAMP_LEN    (5)             /* read only 5 bytes (the adjusted timestamp (40:0)) */
+#define RX_STAMP_LEN            RX_TIME_RX_STAMP_LEN
+/*mask and shift */
+#define RX_TIME_RX_STAMP_OFFSET  (0) /* byte 0..4 40 bit Reports the fully adjusted time of reception. */
+#define RX_TIME_FP_INDEX_OFFSET  (5)    /* byte 5..6 16 bit First path index. */
+#define RX_TIME_FP_AMPL1_OFFSET  (7)    /* byte 7..8 16 bit First Path Amplitude point 1 */   /* doc bug */
+#define RX_TIME_FP_RAWST_OFFSET  (9)    /* byte 9..13 40 bit Raw Timestamp for the frame */
+
+
+/****************************************************************************//**
+ * @brief Bit definitions for register
+**/
+#define REG_16_ID_RESERVED      0x16
+
+
+/****************************************************************************//**
+ * @brief Bit definitions for register
+**/
+#define TX_TIME_ID              0x17            /* Transmit Message Time of Sending */
+#define TX_TIME_LLEN            (10)
+#define TX_TIME_TX_STAMP_LEN    (5)             /* 40-bits = 5 bytes */
+#define TX_STAMP_LEN            TX_TIME_TX_STAMP_LEN
+/*mask and shift */
+#define TX_TIME_TX_STAMP_OFFSET  (0) /* byte 0..4 40 bit Reports the fully adjusted time of transmission */
+#define TX_TIME_TX_RAWST_OFFSET  (5) /* byte 5..9 40 bit Raw Timestamp for the frame */
+
+
+
+
+/****************************************************************************//**
+ * @brief Bit definitions for register TX_ANTD
+**/
+#define TX_ANTD_ID              0x18            /* 16-bit Delay from Transmit to Antenna */
+#define TX_ANTD_OFFSET          0x00
+#define TX_ANTD_LEN             (2)
+
+
+
+
+/****************************************************************************//**
+ * @brief Bit definitions for register SYS_STATES
+ *        Register map register file 0x19 is reserved
+ *
+**/
+#define SYS_STATE_ID            0x19            /* System State information READ ONLY */
+#define SYS_STATE_LEN           (5)
+
+/****************************************************************************//**
+ * @brief Bit definitions for register ACK_RESP_T
+**/
+/* Acknowledge (31:24 preamble symbol delay before auto ACK is sent) and respose (19:0 - unit 1us) timer */
+#define ACK_RESP_T_ID           0x1A            /* Acknowledgement Time and Response Time */
+#define ACK_RESP_T_LEN          (4)
+/*mask and shift */
+#define ACK_RESP_T_MASK         0xFF0FFFFFUL    /* Acknowledgement Time and Response access mask */
+#define ACK_RESP_T_W4R_TIM_OFFSET 0             /* In bytes */
+#define ACK_RESP_T_W4R_TIM_MASK 0x000FFFFFUL    /* Wait-for-Response turn-around Time 20 bit field */
+#define W4R_TIM_MASK            ACK_RESP_T_W4R_TIM_MASK
+#define ACK_RESP_T_ACK_TIM_OFFSET 3             /* In bytes */
+#define ACK_RESP_T_ACK_TIM_MASK 0xFF000000UL    /* Auto-Acknowledgement turn-around Time */
+#define ACK_TIM_MASK            ACK_RESP_T_ACK_TIM_MASK
+
+
+
+/****************************************************************************//**
+ * @brief Bit definitions for register 0x1B 0x1C
+**/
+#define REG_1B_ID_RESERVED      0x1B
+#define REG_1C_ID_RESERVED      0x1C
+
+/****************************************************************************//**
+ * @brief Bit definitions for register RX_SNIFF
+ *        Sniff Mode Configuration or Pulsed Preamble Reception Configuration
+**/
+#define RX_SNIFF_ID             0x1D            /* Sniff Mode Configuration */
+#define RX_SNIFF_OFFSET         0x00
+#define RX_SNIFF_LEN            (4)
+/*mask and shift */
+#define RX_SNIFF_MASK           0x0000FF0FUL    /*  */
+#define RX_SNIFF_SNIFF_ONT_MASK 0x0000000FUL    /* SNIFF Mode ON time. Specified in units of PAC */
+#define SNIFF_ONT_MASK          RX_SNIFF_SNIFF_ONT_MASK
+#define RX_SNIFF_SNIFF_OFFT_MASK 0x0000FF00UL   /* SNIFF Mode OFF time specified in units of approximately 1mkS, or 128 system clock cycles.*/
+#define SNIFF_OFFT_MASK         RX_SNIFF_SNIFF_OFFT_MASK
+
+
+
+/****************************************************************************//**
+ * @brief Bit definitions for register TX_POWER
+**/
+#define TX_POWER_ID             0x1E            /* TX Power Control */
+#define TX_POWER_LEN            (4)
+/*mask and shift definition for Smart Transmit Power Control*/
+#define TX_POWER_BOOSTNORM_MASK 0x00000000UL    /* This is the normal power setting used for frames that do not fall */
+#define BOOSTNORM_MASK          TX_POWER_BOOSTNORM_MASK
+#define TX_POWER_BOOSTNORM_SHIFT (0)
+#define TX_POWER_BOOSTP500_MASK 0x00000000UL    /* This value sets the power applied during transmission at the 6.8 Mbps data rate frames that are less than 0.5 ms duration */
+#define BOOSTP500_MASK          TX_POWER_BOOSTP500_MASK
+#define TX_POWER_BOOSTP500_SHIFT (8)
+#define TX_POWER_BOOSTP250_MASK 0x00000000UL    /* This value sets the power applied during transmission at the 6.8 Mbps data rate frames that are less than 0.25 ms duration */
+#define BOOSTP250_MASK          TX_POWER_BOOSTP250_MASK
+#define TX_POWER_BOOSTP250_SHIFT (16)
+#define TX_POWER_BOOSTP125_MASK 0x00000000UL    /* This value sets the power applied during transmission at the 6.8 Mbps data rate frames that are less than 0.125 ms */
+#define BOOSTP125_MASK          TX_POWER_BOOSTP125_MASK
+#define TX_POWER_BOOSTP125_SHIFT (24)
+/*mask and shift definition for Manual Transmit Power Control (DIS_STXP=1 in SYS_CFG)*/
+#define TX_POWER_MAN_DEFAULT    0x0E080222UL
+#define TX_POWER_TXPOWPHR_MASK  0x0000FF00UL    /* This power setting is applied during the transmission of the PHY header (PHR) portion of the frame. */
+#define TX_POWER_TXPOWSD_MASK   0x00FF0000UL    /* This power setting is applied during the transmission of the synchronisation header (SHR) and data portions of the frame. */
+
+
+/****************************************************************************//**
+ * @brief Bit definitions for register CHAN_CTRL
+**/
+#define CHAN_CTRL_ID            0x1F            /* Channel Control */
+#define CHAN_CTRL_LEN           (4)
+/*mask and shift */
+#define CHAN_CTRL_MASK          0xFFFF00FFUL    /* Channel Control Register access mask */
+#define CHAN_CTRL_TX_CHAN_MASK  0x0000000FUL    /* Supported channels are 1, 2, 3, 4, 5, and 7.*/
+#define CHAN_CTRL_TX_CHAN_SHIFT (0)             /* Bits 0..3        TX channel number 0-15 selection */
+
+#define CHAN_CTRL_RX_CHAN_MASK  0x000000F0UL
+#define CHAN_CTRL_RX_CHAN_SHIFT (4)             /* Bits 4..7        RX channel number 0-15 selection */
+
+#define CHAN_CTRL_RXFPRF_MASK   0x000C0000UL    /* Bits 18..19      Specify (Force) RX Pulse Repetition Rate: 00 = 4 MHz, 01 = 16 MHz, 10 = 64MHz. */
+#define CHAN_CTRL_RXFPRF_SHIFT  (18)
+/* Specific RXFPRF configuration */
+#define CHAN_CTRL_RXFPRF_4      0x00000000UL    /* Specify (Force) RX Pulse Repetition Rate: 00 = 4 MHz, 01 = 16 MHz, 10 = 64MHz. */
+#define CHAN_CTRL_RXFPRF_16     0x00040000UL    /* Specify (Force) RX Pulse Repetition Rate: 00 = 4 MHz, 01 = 16 MHz, 10 = 64MHz. */
+#define CHAN_CTRL_RXFPRF_64     0x00080000UL    /* Specify (Force) RX Pulse Repetition Rate: 00 = 4 MHz, 01 = 16 MHz, 10 = 64MHz. */
+#define CHAN_CTRL_TX_PCOD_MASK  0x07C00000UL    /* Bits 22..26      TX Preamble Code selection, 1 to 24. */
+#define CHAN_CTRL_TX_PCOD_SHIFT (22)
+#define CHAN_CTRL_RX_PCOD_MASK  0xF8000000UL    /* Bits 27..31      RX Preamble Code selection, 1 to 24. */
+#define CHAN_CTRL_RX_PCOD_SHIFT (27)
+/*offset 16 */
+#define CHAN_CTRL_DWSFD         0x00020000UL    /* Bit 17 This bit enables a non-standard DecaWave proprietary SFD sequence. */
+#define CHAN_CTRL_DWSFD_SHIFT   (17)
+#define CHAN_CTRL_TNSSFD        0x00100000UL    /* Bit 20 This bit enables the use of user-defined SFD when transmitting */
+#define CHAN_CTRL_TNSSFD_SHIFT  (20)
+#define CHAN_CTRL_RNSSFD        0x00200000UL    /* Bit 21 This bit enables the use of user-defined SFD when receiving */
+#define CHAN_CTRL_RNSSFD_SHIFT  (21)
+
+
+
+
+/****************************************************************************//**
+ * @brief Bit definitions for register 0x20
+**/
+#define REG_20_ID_RESERVED      0x20
+
+/****************************************************************************//**
+ * @brief Bit definitions for register USR_SFD
+ *        Please read User Manual : User defined SFD sequence
+**/
+#define USR_SFD_ID              0x21            /* User-specified short/long TX/RX SFD sequences */
+#define USR_SFD_LEN             (41)
+#define DW_NS_SFD_LEN_110K      64              /* Decawave non-standard SFD length for 110 kbps */
+#define DW_NS_SFD_LEN_850K      16              /* Decawave non-standard SFD length for 850 kbps */
+#define DW_NS_SFD_LEN_6M8       8               /* Decawave non-standard SFD length for 6.8 Mbps */
+
+
+/****************************************************************************//**
+ * @brief Bit definitions for register
+**/
+#define REG_22_ID_RESERVED      0x22
+
+/****************************************************************************//**
+ * @brief Bit definitions for register AGC_CTRL
+ * Please take care to write to this register as doing so may cause the DW1000 to malfunction
+**/
+#define AGC_CTRL_ID             0x23            /* Automatic Gain Control configuration */
+#define AGC_CTRL_LEN            (32)
+#define AGC_CFG_STS_ID          AGC_CTRL_ID
+/* offset from AGC_CTRL_ID in bytes */
+#define AGC_CTRL1_OFFSET        (0x02)
+#define AGC_CTRL1_LEN           (2)
+#define AGC_CTRL1_MASK          0x0001          /* access mask to AGC configuration and control register */
+#define AGC_CTRL1_DIS_AM        0x0001          /* Disable AGC Measurement. The DIS_AM bit is set by default. */
+/* offset from AGC_CTRL_ID in bytes */
+/* Please take care not to write other values to this register as doing so may cause the DW1000 to malfunction */
+#define AGC_TUNE1_OFFSET        (0x04)
+#define AGC_TUNE1_LEN           (2)
+#define AGC_TUNE1_MASK          0xFFFF          /* It is a 16-bit tuning register for the AGC. */
+#define AGC_TUNE1_16M           0x8870
+#define AGC_TUNE1_64M           0x889B
+/* offset from AGC_CTRL_ID in bytes */
+/* Please take care not to write other values to this register as doing so may cause the DW1000 to malfunction */
+#define AGC_TUNE2_OFFSET        (0x0C)
+#define AGC_TUNE2_LEN           (4)
+#define AGC_TUNE2_MASK          0xFFFFFFFFUL
+#define AGC_TUNE2_VAL           0X2502A907UL
+/* offset from AGC_CTRL_ID in bytes */
+/* Please take care not to write other values to this register as doing so may cause the DW1000 to malfunction */
+#define AGC_TUNE3_OFFSET        (0x12)
+#define AGC_TUNE3_LEN           (2)
+#define AGC_TUNE3_MASK          0xFFFF
+#define AGC_TUNE3_VAL           0X0055
+/* offset from AGC_CTRL_ID in bytes */
+#define AGC_STAT1_OFFSET        (0x1E)
+#define AGC_STAT1_LEN           (3)
+#define AGC_STAT1_MASK          0x0FFFFF
+#define AGC_STAT1_EDG1_MASK     0x0007C0        /* This 5-bit gain value relates to input noise power measurement. */
+#define AGC_STAT1_EDG2_MASK     0x0FF800        /* This 9-bit value relates to the input noise power measurement. */
+
+/****************************************************************************//**
+ * @brief Bit definitions for register EXT_SYNC
+**/
+#define EXT_SYNC_ID             0x24            /* External synchronisation control */
+#define EXT_SYNC_LEN            (12)
+/* offset from EXT_SYNC_ID in bytes */
+#define EC_CTRL_OFFSET          (0x00)
+#define EC_CTRL_LEN             (4)
+#define EC_CTRL_MASK            0x00000FFBUL    /* sub-register 0x00 is the External clock synchronisation counter configuration register */
+#define EC_CTRL_OSTSM           0x00000001UL    /* External transmit synchronisation mode enable */
+#define EC_CTRL_OSRSM           0x00000002UL    /* External receive synchronisation mode enable */
+#define EC_CTRL_PLLLCK          0x04            /* PLL lock detect enable */
+#define EC_CTRL_OSTRM           0x00000800UL    /* External timebase reset mode enable */
+#define EC_CTRL_WAIT_MASK       0x000007F8UL    /* Wait counter used for external transmit synchronisation and external timebase reset */
+/* offset from EXT_SYNC_ID in bytes */
+#define EC_RXTC_OFFSET          (0x04)
+#define EC_RXTC_LEN             (4)
+#define EC_RXTC_MASK            0xFFFFFFFFUL    /* External clock synchronisation counter captured on RMARKER */
+/* offset from EXT_SYNC_ID in bytes */
+#define EC_GOLP                 (0x08)
+#define EC_GOLP_LEN             (4)
+#define EC_GOLP_MASK            0x0000003FUL    /* sub-register 0x08 is the External clock offset to first path 1 GHz counter, EC_GOLP */
+#define EC_GOLP_OFFSET_EXT_MASK 0x0000003FUL    /* This register contains the 1 GHz count from the arrival of the RMARKER and the next edge of the external clock. */
+
+
+/****************************************************************************//**
+ * @brief Bit definitions for register ACC_MEM
+**/
+#define ACC_MEM_ID              0x25            /* Read access to accumulator data */
+#define ACC_MEM_LEN             (4064)
+
+
+/****************************************************************************//**
+ * @brief Bit definitions for register GPIO_CTRL
+**/
+#define GPIO_CTRL_ID            0x26            /* Peripheral register bus 1 access - GPIO control */
+#define GPIO_CTRL_LEN           (44)
+
+/* offset from GPIO_CTRL in bytes */
+#define GPIO_MODE_OFFSET        0x00            /* sub-register 0x00 is the GPIO Mode Control Register */
+#define GPIO_MODE_LEN           (4)
+#define GPIO_MODE_MASK          0x00FFFFC0UL
+
+#define GPIO_MSGP0_MASK         0x000000C0UL    /* Mode Selection for GPIO0/RXOKLED */
+#define GPIO_MSGP1_MASK         0x00000300UL    /* Mode Selection for GPIO1/SFDLED */
+#define GPIO_MSGP2_MASK         0x00000C00UL    /* Mode Selection for GPIO2/RXLED */
+#define GPIO_MSGP3_MASK         0x00003000UL    /* Mode Selection for GPIO3/TXLED */
+#define GPIO_MSGP4_MASK         0x0000C000UL    /* Mode Selection for GPIO4/EXTPA */
+#define GPIO_MSGP5_MASK         0x00030000UL    /* Mode Selection for GPIO5/EXTTXE */
+#define GPIO_MSGP6_MASK         0x000C0000UL    /* Mode Selection for GPIO6/EXTRXE */
+#define GPIO_MSGP7_MASK         0x00300000UL    /* Mode Selection for SYNC/GPIO7 */
+#define GPIO_MSGP8_MASK         0x00C00000UL    /* Mode Selection for IRQ/GPIO8 */
+
+#define GPIO_PIN2_RXLED         0x00000400UL    /* The pin operates as the RXLED output */
+#define GPIO_PIN3_TXLED         0x00001000UL    /* The pin operates as the TXLED output */
+#define GPIO_PIN4_EXTPA         0x00004000UL    /* The pin operates as the EXTPA output */
+#define GPIO_PIN5_EXTTXE        0x00010000UL    /* The pin operates as the EXTTXE output */
+#define GPIO_PIN6_EXTRXE        0x00040000UL    /* The pin operates as the EXTRXE output */
+
+/* offset from GPIO_CTRL in bytes */
+#define GPIO_DIR_OFFSET         0x08            /* sub-register 0x08 is the GPIO Direction Control Register */
+#define GPIO_DIR_LEN            (3)
+#define GPIO_DIR_MASK           0x0011FFFFUL
+
+#define GxP0                    0x00000001UL    /* GPIO0 Only changed if the GxM0 mask bit has a value of 1 for the write operation*/
+#define GxP1                    0x00000002UL    /* GPIO1. (See GDP0). */
+#define GxP2                    0x00000004UL    /* GPIO2. (See GDP0). */
+#define GxP3                    0x00000008UL    /* GPIO3. (See GDP0). */
+#define GxP4                    0x00000100UL    /* GPIO4. (See GDP0). */
+#define GxP5                    0x00000200UL    /* GPIO5. (See GDP0). */
+#define GxP6                    0x00000400UL    /* GPIO6. (See GDP0). */
+#define GxP7                    0x00000800UL    /* GPIO7. (See GDP0). */
+#define GxP8                    0x00010000UL    /* GPIO8 */
+
+#define GxM0                    0x00000010UL    /* Mask for GPIO0 */
+#define GxM1                    0x00000020UL    /* Mask for GPIO1. (See GDM0). */
+#define GxM2                    0x00000040UL    /* Mask for GPIO2. (See GDM0). */
+#define GxM3                    0x00000080UL    /* Mask for GPIO3. (See GDM0). */
+#define GxM4                    0x00001000UL    /* Mask for GPIO4. (See GDM0). */
+#define GxM5                    0x00002000UL    /* Mask for GPIO5. (See GDM0). */
+#define GxM6                    0x00004000UL    /* Mask for GPIO6. (See GDM0). */
+#define GxM7                    0x00008000UL    /* Mask for GPIO7. (See GDM0). */
+#define GxM8                    0x00100000UL    /* Mask for GPIO8. (See GDM0). */
+
+#define GDP0    GxP0    /* Direction Selection for GPIO0. 1 = input, 0 = output. Only changed if the GDM0 mask bit has a value of 1 for the write operation*/
+#define GDP1    GxP1    /* Direction Selection for GPIO1. (See GDP0). */
+#define GDP2    GxP2    /* Direction Selection for GPIO2. (See GDP0). */
+#define GDP3    GxP3    /* Direction Selection for GPIO3. (See GDP0). */
+#define GDP4    GxP4    /* Direction Selection for GPIO4. (See GDP0). */
+#define GDP5    GxP5    /* Direction Selection for GPIO5. (See GDP0). */
+#define GDP6    GxP6    /* Direction Selection for GPIO6. (See GDP0). */
+#define GDP7    GxP7    /* Direction Selection for GPIO7. (See GDP0). */
+#define GDP8    GxP8    /* Direction Selection for GPIO8 */
+
+#define GDM0    GxM0    /* Mask for setting the direction of GPIO0 */
+#define GDM1    GxM1    /* Mask for setting the direction of GPIO1. (See GDM0). */
+#define GDM2    GxM2    /* Mask for setting the direction of GPIO2. (See GDM0). */
+#define GDM3    GxM3    /* Mask for setting the direction of GPIO3. (See GDM0). */
+#define GDM4    GxM4    /* Mask for setting the direction of GPIO4. (See GDM0). */
+#define GDM5    GxM5    /* Mask for setting the direction of GPIO5. (See GDM0). */
+#define GDM6    GxM6    /* Mask for setting the direction of GPIO6. (See GDM0). */
+#define GDM7    GxM7    /* Mask for setting the direction of GPIO7. (See GDM0). */
+#define GDM8    GxM8    /* Mask for setting the direction of GPIO8. (See GDM0). */
+
+/* offset from GPIO_CTRL in bytes */
+#define GPIO_DOUT_OFFSET        0x0C            /* sub-register 0x0C is the GPIO data output register. */
+#define GPIO_DOUT_LEN           (3)
+#define GPIO_DOUT_MASK          GPIO_DIR_MASK
+
+/* offset from GPIO_CTRL in bytes */
+#define GPIO_IRQE_OFFSET        0x10            /* sub-register 0x10 is the GPIO interrupt enable register */
+#define GPIO_IRQE_LEN           (4)
+#define GPIO_IRQE_MASK          0x000001FFUL
+#define GIRQx0                  0x00000001UL    /* IRQ bit0 */
+#define GIRQx1                  0x00000002UL    /* IRQ bit1 */
+#define GIRQx2                  0x00000004UL    /* IRQ bit2 */
+#define GIRQx3                  0x00000008UL    /* IRQ bit3 */
+#define GIRQx4                  0x00000010UL    /* IRQ bit4 */
+#define GIRQx5                  0x00000020UL    /* IRQ bit5 */
+#define GIRQx6                  0x00000040UL    /* IRQ bit6 */
+#define GIRQx7                  0x00000080UL    /* IRQ bit7 */
+#define GIRQx8                  0x00000100UL    /* IRQ bit8 */
+#define GIRQE0  GIRQx0  /* GPIO IRQ Enable for GPIO0 input. Value 1 = enable, 0 = disable*/
+#define GIRQE1  GIRQx1  /*  */
+#define GIRQE2  GIRQx2  /*  */
+#define GIRQE3  GIRQx3  /*  */
+#define GIRQE4  GIRQx4  /*  */
+#define GIRQE5  GIRQx5  /*  */
+#define GIRQE6  GIRQx6  /*  */
+#define GIRQE7  GIRQx7  /*  */
+#define GIRQE8  GIRQx8  /* Value 1 = enable, 0 = disable */
+
+/* offset from GPIO_CTRL in bytes */
+#define GPIO_ISEN_OFFSET        0x14    /* sub-register 0x14 is the GPIO interrupt sense selection register */
+#define GPIO_ISEN_LEN           (4)
+#define GPIO_ISEN_MASK          GPIO_IRQE_MASK
+#define GISEN0  GIRQx0  /* GPIO IRQ Sense selection GPIO0 input. Value 0 = High or Rising-Edge, 1 = Low or falling-edge.*/
+#define GISEN1  GIRQx1  /*  */
+#define GISEN2  GIRQx2  /*  */
+#define GISEN3  GIRQx3  /*  */
+#define GISEN4  GIRQx4  /*  */
+#define GISEN5  GIRQx5  /*  */
+#define GISEN6  GIRQx6  /*  */
+#define GISEN7  GIRQx7  /*  */
+#define GISEN8  GIRQx8  /* Value 0 = High or Rising-Edge, 1 = Low or falling-edge */
+
+/* offset from GPIO_CTRL in bytes */
+#define GPIO_IMODE_OFFSET       0x18    /* sub-register 0x18 is the GPIO interrupt mode selection register */
+#define GPIO_IMODE_LEN          (4)
+#define GPIO_IMODE_MASK         GPIO_IRQE_MASK
+#define GIMOD0  GIRQx0  /* GPIO IRQ Mode selection for GPIO0 input. Value 0 = Level sensitive interrupt. Value 1 = Edge triggered interrupt */
+#define GIMOD1  GIRQx1  /*  */
+#define GIMOD2  GIRQx2  /*  */
+#define GIMOD3  GIRQx3  /*  */
+#define GIMOD4  GIRQx4  /*  */
+#define GIMOD5  GIRQx5  /*  */
+#define GIMOD6  GIRQx6  /*  */
+#define GIMOD7  GIRQx7  /*  */
+#define GIMOD8  GIRQx8  /* Value 0 = Level, 1 = Edge. */
+
+/* offset from EXT_SYNC_ID in bytes */
+#define GPIO_IBES_OFFSET        0x1C    /* sub-register 0x1C is the GPIO interrupt “Both Edge” selection register */
+#define GPIO_IBES_LEN           (4)
+#define GPIO_IBES_MASK          GPIO_IRQE_MASK  /*  */
+#define GIBES0  GIRQx0  /* GPIO IRQ “Both Edge” selection for GPIO0 input. Value 0 = GPIO_IMODE register selects the edge. Value 1 = Both edges trigger the interrupt. */
+#define GIBES1  GIRQx1  /*  */
+#define GIBES2  GIRQx2  /*  */
+#define GIBES3  GIRQx3  /*  */
+#define GIBES4  GIRQx4  /*  */
+#define GIBES5  GIRQx5  /*  */
+#define GIBES6  GIRQx6  /*  */
+#define GIBES7  GIRQx7  /*  */
+#define GIBES8  GIRQx8  /* Value 0 = use GPIO_IMODE, 1 = Both Edges */
+
+/* offset from GPIO_CTRL in bytes */
+#define GPIO_ICLR_OFFSET        0x20    /* sub-register 0x20 is the GPIO interrupt clear register */
+#define GPIO_ICLR_LEN           (4)
+#define GPIO_ICLR_MASK          GPIO_IRQE_MASK  /*  */
+#define GICLR0  GIRQx0  /* GPIO IRQ latch clear for GPIO0 input. Write 1 to clear the GPIO0 interrupt latch. Writing 0 has no effect. Reading returns zero */
+#define GICLR1  GIRQx1  /*  */
+#define GICLR2  GIRQx2  /*  */
+#define GICLR3  GIRQx3  /*  */
+#define GICLR4  GIRQx4  /*  */
+#define GICLR5  GIRQx5  /*  */
+#define GICLR6  GIRQx6  /*  */
+#define GICLR7  GIRQx7  /*  */
+#define GICLR8  GIRQx8  /* Write 1 to clear the interrupt latch */
+
+/* offset from GPIO_CTRL in bytes */
+#define GPIO_IDBE_OFFSET        0x24    /* sub-register 0x24 is the GPIO interrupt de-bounce enable register */
+#define GPIO_IDBE_LEN           (4)
+#define GPIO_IDBE_MASK          GPIO_IRQE_MASK
+#define GIDBE0  GIRQx0  /* GPIO IRQ de-bounce enable for GPIO0. Value 1 = de-bounce enabled. Value 0 = de-bounce disabled */
+#define GIDBE1  GIRQx1  /*  */
+#define GIDBE2  GIRQx2  /*  */
+#define GIDBE3  GIRQx3  /*  */
+#define GIDBE4  GIRQx4  /*  */
+#define GIDBE5  GIRQx5  /*  */
+#define GIDBE6  GIRQx6  /*  */
+#define GIDBE7  GIRQx7  /*  */
+#define GIDBE8  GIRQx8  /* Value 1 = de-bounce enabled, 0 = de-bounce disabled */
+
+/* offset from GPIO_CTRL in bytes */
+#define GPIO_RAW_OFFSET         0x28    /* sub-register 0x28 allows the raw state of the GPIO pin to be read. */
+#define GPIO_RAW_LEN            (4)
+#define GPIO_RAW_MASK           GPIO_IRQE_MASK
+#define GRAWP0  GIRQx0  /* This bit reflects the raw state of GPIO0 */
+#define GRAWP1  GIRQx1  /*  */
+#define GRAWP2  GIRQx2  /*  */
+#define GRAWP3  GIRQx3  /*  */
+#define GRAWP4  GIRQx4  /*  */
+#define GRAWP5  GIRQx5  /*  */
+#define GRAWP6  GIRQx6  /*  */
+#define GRAWP7  GIRQx7  /*  */
+#define GRAWP8  GIRQx8  /* This bit reflects the raw state of GPIO8 */
+
+/****************************************************************************//**
+ * @brief Bit definitions for register  DRX_CONF
+ * Digital Receiver configuration block
+**/
+#define DRX_CONF_ID             0x27            /* Digital Receiver configuration */
+#define DRX_CONF_LEN            (44)
+/* offset from DRX_CONF_ID in bytes */
+#define DRX_TUNE0b_OFFSET       (0x02)  /* sub-register 0x02 is a 16-bit tuning register. */
+#define DRX_TUNE0b_LEN          (2)
+#define DRX_TUNE0b_MASK         0xFFFF  /* 7.2.40.2 Sub-Register 0x27:02 – DRX_TUNE0b */
+#define DRX_TUNE0b_110K_STD     0x000A
+#define DRX_TUNE0b_110K_NSTD    0x0016
+#define DRX_TUNE0b_850K_STD     0x0001
+#define DRX_TUNE0b_850K_NSTD    0x0006
+#define DRX_TUNE0b_6M8_STD      0x0001
+#define DRX_TUNE0b_6M8_NSTD     0x0002
+
+/* offset from DRX_CONF_ID in bytes */
+#define DRX_TUNE1a_OFFSET       0x04    /* 7.2.40.3 Sub-Register 0x27:04 – DRX_TUNE1a */
+#define DRX_TUNE1a_LEN          (2)
+#define DRX_TUNE1a_MASK         0xFFFF
+#define DRX_TUNE1a_PRF16        0x0087
+#define DRX_TUNE1a_PRF64        0x008D
+
+/* offset from DRX_CONF_ID in bytes */
+#define DRX_TUNE1b_OFFSET       0x06    /* 7.2.40.4 Sub-Register 0x27:06 – DRX_TUNE1b */
+#define DRX_TUNE1b_LEN          (2)
+#define DRX_TUNE1b_MASK         0xFFFF
+#define DRX_TUNE1b_110K         0x0064
+#define DRX_TUNE1b_850K_6M8     0x0020
+#define DRX_TUNE1b_6M8_PRE64    0x0010
+
+/* offset from DRX_CONF_ID in bytes */
+#define DRX_TUNE2_OFFSET        0x08    /* 7.2.40.5 Sub-Register 0x27:08 – DRX_TUNE2 */
+#define DRX_TUNE2_LEN           (4)
+#define DRX_TUNE2_MASK          0xFFFFFFFFUL
+#define DRX_TUNE2_PRF16_PAC8    0x311A002DUL
+#define DRX_TUNE2_PRF16_PAC16   0x331A0052UL
+#define DRX_TUNE2_PRF16_PAC32   0x351A009AUL
+#define DRX_TUNE2_PRF16_PAC64   0x371A011DUL
+#define DRX_TUNE2_PRF64_PAC8    0x313B006BUL
+#define DRX_TUNE2_PRF64_PAC16   0x333B00BEUL
+#define DRX_TUNE2_PRF64_PAC32   0x353B015EUL
+#define DRX_TUNE2_PRF64_PAC64   0x373B0296UL
+
+/* offset from DRX_CONF_ID in bytes */
+/* WARNING: Please do NOT set DRX_SFDTOC to zero (disabling SFD detection timeout)
+ * since this risks IC malfunction due to prolonged receiver activity in the event of false preamble detection.
+ */
+#define DRX_SFDTOC_OFFSET       0x20    /* 7.2.40.7 Sub-Register 0x27:20 – DRX_SFDTOC */
+#define DRX_SFDTOC_LEN          (2)
+#define DRX_SFDTOC_MASK         0xFFFF
+
+/* offset from DRX_CONF_ID in bytes */
+#define DRX_PRETOC_OFFSET       0x24    /* 7.2.40.9 Sub-Register 0x27:24 – DRX_PRETOC */
+#define DRX_PRETOC_LEN          (2)
+#define DRX_PRETOC_MASK         0xFFFF
+
+/* offset from DRX_CONF_ID in bytes */
+#define DRX_TUNE4H_OFFSET       0x26    /* 7.2.40.10 Sub-Register 0x27:26 – DRX_TUNE4H */
+#define DRX_TUNE4H_LEN          (2)
+#define DRX_TUNE4H_MASK         0xFFFF
+#define DRX_TUNE4H_PRE64        0x0010
+#define DRX_TUNE4H_PRE128PLUS   0x0028
+
+
+/****************************************************************************//**
+ * @brief Bit definitions for register  RF_CONF
+ * Analog RF Configuration block
+ * Refer to section 7.2.41 Register file: 0x28 – Analog RF configuration block
+**/
+#define RF_CONF_ID              0x28            /* Analog RF Configuration */
+#define RF_CONF_LEN             (58)
+#define RF_CONF_TXEN_MASK       0x00400000UL   /* TX enable */
+#define RF_CONF_RXEN_MASK       0x00200000UL   /* RX enable */
+#define RF_CONF_TXPOW_MASK      0x001F0000UL   /* turn on power all LDOs */
+#define RF_CONF_PLLEN_MASK      0x0000E000UL   /* enable PLLs */
+#define RF_CONF_TXBLOCKSEN_MASK 0x00001F00UL   /* enable TX blocks */
+#define RF_CONF_TXPLLPOWEN_MASK (RF_CONF_PLLEN_MASK | RF_CONF_TXPOW_MASK)
+#define RF_CONF_TXALLEN_MASK    (RF_CONF_TXEN_MASK | RF_CONF_TXPOW_MASK | RF_CONF_PLLEN_MASK | RF_CONF_TXBLOCKSEN_MASK)
+/* offset from TX_CAL_ID in bytes */
+#define RF_RXCTRLH_OFFSET       0x0B            /* Analog RX Control Register */
+#define RF_RXCTRLH_LEN          (1)
+#define RF_RXCTRLH_NBW          0xD8            /* RXCTRLH value for narrow bandwidth channels */
+#define RF_RXCTRLH_WBW          0xBC            /* RXCTRLH value for wide bandwidth channels */
+/* offset from TX_CAL_ID in bytes */
+#define RF_TXCTRL_OFFSET        0x0C            /* Analog TX Control Register */
+#define RF_TXCTRL_LEN           (4)
+#define RF_TXCTRL_TXMTUNE_MASK  0x000001E0UL    /* Transmit mixer tuning register */
+#define RF_TXCTRL_TXTXMQ_MASK   0x00000E00UL    /* Transmit mixer Q-factor tuning register */
+#define RF_TXCTRL_CH1           0x00005C40UL    /* 32-bit value to program to Sub-Register 0x28:0C – RF_TXCTRL */
+#define RF_TXCTRL_CH2           0x00045CA0UL    /* 32-bit value to program to Sub-Register 0x28:0C – RF_TXCTRL */
+#define RF_TXCTRL_CH3           0x00086CC0UL    /* 32-bit value to program to Sub-Register 0x28:0C – RF_TXCTRL */
+#define RF_TXCTRL_CH4           0x00045C80UL    /* 32-bit value to program to Sub-Register 0x28:0C – RF_TXCTRL */
+#define RF_TXCTRL_CH5           0x001E3FE0UL    /* 32-bit value to program to Sub-Register 0x28:0C – RF_TXCTRL */
+#define RF_TXCTRL_CH7           0x001E7DE0UL    /* 32-bit value to program to Sub-Register 0x28:0C – RF_TXCTRL */
+
+/* offset from TX_CAL_ID in bytes */
+#define RF_STATUS_OFFSET        0x2C
+
+/****************************************************************************//**
+ * @brief Bit definitions for register
+**/
+#define REG_29_ID_RESERVED      0x29
+
+/****************************************************************************//**
+ * @brief Bit definitions for register TX_CAL
+ * Refer to section 7.2.43 Register file: 0x2A – Transmitter Calibration block
+**/
+#define TX_CAL_ID               0x2A            /* Transmitter calibration block */
+#define TX_CAL_LEN              (52)
+/* offset from TX_CAL_ID in bytes */
+#define TC_SARL_SAR_C		        (0)         /* SAR control */
+/*cause bug in register block TX_CAL, we need to read 1 byte in a time*/
+#define TC_SARL_SAR_LVBAT_OFFSET    (3)         /* Latest SAR reading for Voltage level */
+#define TC_SARL_SAR_LTEMP_OFFSET    (4)         /* Latest SAR reading for Temperature level */
+#define TC_SARW_SAR_WTEMP_OFFSET    0x06            /* SAR reading of Temperature level taken at last wakeup event */
+#define TC_SARW_SAR_WVBAT_OFFSET    0x07            /* SAR reading of Voltage level taken at last wakeup event */
+/* offset from TX_CAL_ID in bytes */
+#define TC_PGDELAY_OFFSET       0x0B            /* Transmitter Calibration – Pulse Generator Delay */
+#define TC_PGDELAY_LEN          (1)
+#define TC_PGDELAY_CH1          0xC9            /* Recommended value for channel 1 */
+#define TC_PGDELAY_CH2          0xC2            /* Recommended value for channel 2 */
+#define TC_PGDELAY_CH3          0xC5            /* Recommended value for channel 3 */
+#define TC_PGDELAY_CH4          0x95            /* Recommended value for channel 4 */
+#define TC_PGDELAY_CH5          0xC0            /* Recommended value for channel 5 */
+#define TC_PGDELAY_CH7          0x93            /* Recommended value for channel 7 */
+/* offset from TX_CAL_ID in bytes */
+#define TC_PGTEST_OFFSET        0x0C            /* Transmitter Calibration – Pulse Generator Test */
+#define TC_PGTEST_LEN           (1)
+#define TC_PGTEST_NORMAL        0x00            /* Normal operation */
+#define TC_PGTEST_CW            0x13            /* Continuous Wave (CW) Test Mode */
+
+/****************************************************************************//**
+ * @brief Bit definitions for register
+ * Refer to section 7.2.44 Register file: 0x2B – Frequency synthesiser control block
+**/
+#define FS_CTRL_ID              0x2B            /* Frequency synthesiser control block */
+#define FS_CTRL_LEN             (21)
+/* offset from FS_CTRL_ID in bytes */
+#define FS_RES1_OFFSET          0x00            /* reserved area. Please take care not to write to this area as doing so may cause the DW1000 to malfunction. */
+#define FS_RES1_LEN             (7)
+/* offset from FS_CTRL_ID in bytes */
+#define FS_PLLCFG_OFFSET        0x07            /* Frequency synthesiser – PLL configuration */
+#define FS_PLLCFG_LEN           (5)
+#define FS_PLLCFG_CH1           0x09000407UL    /* Operating Channel 1 */
+#define FS_PLLCFG_CH2           0x08400508UL    /* Operating Channel 2 */
+#define FS_PLLCFG_CH3           0x08401009UL    /* Operating Channel 3 */
+#define FS_PLLCFG_CH4           FS_PLLCFG_CH2   /* Operating Channel 4 (same as 2) */
+#define FS_PLLCFG_CH5           0x0800041DUL    /* Operating Channel 5 */
+#define FS_PLLCFG_CH7           FS_PLLCFG_CH5   /* Operating Channel 7 (same as 5) */
+/* offset from FS_CTRL_ID in bytes */
+#define FS_PLLTUNE_OFFSET       0x0B            /* Frequency synthesiser – PLL Tuning */
+#define FS_PLLTUNE_LEN          (1)
+#define FS_PLLTUNE_CH1          0x1E            /* Operating Channel 1 */
+#define FS_PLLTUNE_CH2          0x26            /* Operating Channel 2 */
+#define FS_PLLTUNE_CH3          0x56            /* Operating Channel 3 */
+#define FS_PLLTUNE_CH4          FS_PLLTUNE_CH2  /* Operating Channel 4 (same as 2) */
+#define FS_PLLTUNE_CH5          0xBE            /* Operating Channel 5 */
+#define FS_PLLTUNE_CH7          FS_PLLTUNE_CH5  /* Operating Channel 7 (same as 5) */
+/* offset from FS_CTRL_ID in bytes */
+#define FS_RES2_OFFSET          0x0C    /* reserved area. Please take care not to write to this area as doing so may cause the DW1000 to malfunction. */
+#define FS_RES2_LEN             (2)
+/* offset from FS_CTRL_ID in bytes */
+#define FS_XTALT_OFFSET         0x0E    /* Frequency synthesiser – Crystal trim */
+#define FS_XTALT_LEN            (1)
+#define FS_XTALT_MASK           0x1F    /* Crystal Trim. Crystals may be trimmed using this register setting to tune out errors, see 8.1 – IC Calibration – Crystal Oscillator Trim. */
+#define FS_XTALT_MIDRANGE       0x10
+/* offset from FS_CTRL_ID in bytes */
+#define FS_RES3_OFFSET          0x0F    /* reserved area. Please take care not to write to this area as doing so may cause the DW1000 to malfunction. */
+#define FS_RES3_LEN             (6)
+
+/****************************************************************************//**
+ * @brief Bit definitions for register
+**/
+#define AON_ID                  0x2C            /* Always-On register set */
+#define AON_LEN                 (12)
+/* offset from AON_ID in bytes */
+#define AON_WCFG_OFFSET         0x00    /* used to control what the DW1000 IC does as it wakes up from low-power SLEEP or DEEPSLEEPstates. */
+#define AON_WCFG_LEN            (2)
+#define AON_WCFG_MASK           0x09CB  /* access mask to AON_WCFG register*/
+#define AON_WCFG_ONW_RADC       0x0001  /* On Wake-up Run the (temperature and voltage) Analog-to-Digital Convertors */
+#define AON_WCFG_ONW_RX         0x0002  /* On Wake-up turn on the Receiver */
+#define AON_WCFG_ONW_LEUI       0x0008  /* On Wake-up load the EUI from OTP memory into Register file: 0x01 – Extended Unique Identifier. */
+#define AON_WCFG_ONW_LDC        0x0040  /* On Wake-up load configurations from the AON memory into the host interface register set */
+#define AON_WCFG_ONW_L64P       0x0080  /* On Wake-up load the Length64 receiver operating parameter set */
+#define AON_WCFG_PRES_SLEEP     0x0100  /* Preserve Sleep. This bit determines what the DW1000 does with respect to the ARXSLP and ATXSLP sleep controls */
+#define AON_WCFG_ONW_LLDE       0x0800  /* On Wake-up load the LDE microcode. */
+#define AON_WCFG_ONW_LLDO       0x1000  /* On Wake-up load the LDO tune value. */
+/* offset from AON_ID in bytes */
+#define AON_CTRL_OFFSET         0x02    /* The bits in this register in general cause direct activity within the AON block with respect to the stored AON memory */
+#define AON_CTRL_LEN            (1)
+#define AON_CTRL_MASK           0x8F    /* access mask to AON_CTRL register */
+#define AON_CTRL_RESTORE        0x01    /* When this bit is set the DW1000 will copy the user configurations from the AON memory to the host interface register set. */
+#define AON_CTRL_SAVE           0x02    /* When this bit is set the DW1000 will copy the user configurations from the host interface register set into the AON memory */
+#define AON_CTRL_UPL_CFG        0x04    /* Upload the AON block configurations to the AON  */
+#define AON_CTRL_DCA_READ       0x08    /* Direct AON memory access read */
+#define AON_CTRL_DCA_ENAB       0x80    /* Direct AON memory access enable bit */
+/* offset from AON_ID in bytes */
+#define AON_RDAT_OFFSET         0x03    /* AON Direct Access Read Data Result */
+#define AON_RDAT_LEN            (1)
+/* offset from AON_ID in bytes */
+#define AON_ADDR_OFFSET         0x04    /* AON Direct Access Address */
+#define AON_ADDR_LEN            (1)
+#define AON_ADDR_LPOSC_CAL_0    117     /* Address of low-power oscillator calibration value (lower byte) */
+#define AON_ADDR_LPOSC_CAL_1    118     /* Address of low-power oscillator calibration value (lower byte) */
+
+/* offset from AON_ID in bytes */
+#define AON_CFG0_OFFSET         0x06    /* 32-bit configuration register for the always on block. */
+#define AON_CFG0_LEN            (4)
+#define AON_CFG0_SLEEP_EN           0x00000001UL    /* This is the sleep enable configuration bit */
+#define AON_CFG0_WAKE_PIN           0x00000002UL    /* Wake using WAKEUP pin */
+#define AON_CFG0_WAKE_SPI           0x00000004UL    /* Wake using SPI access SPICSn */
+#define AON_CFG0_WAKE_CNT           0x00000008UL    /* Wake when sleep counter elapses */
+#define AON_CFG0_LPDIV_EN           0x00000010UL    /* Low power divider enable configuration */
+#define AON_CFG0_LPCLKDIVA_MASK     0x0000FFE0UL    /* divider count for dividing the raw DW1000 XTAL oscillator frequency to set an LP clock frequency */
+#define AON_CFG0_LPCLKDIVA_SHIFT    (5)
+#define AON_CFG0_SLEEP_TIM          0xFFFF0000UL    /* Sleep time. This field configures the sleep time count elapse value */
+#define AON_CFG0_SLEEP_SHIFT        (16)
+#define AON_CFG0_SLEEP_TIM_OFFSET   2               /* In bytes */
+/* offset from AON_ID in bytes */
+#define AON_CFG1_OFFSET         0x0A
+#define AON_CFG1_LEN            (2)
+#define AON_CFG1_MASK           0x0007  /* aceess mask to AON_CFG1 */
+#define AON_CFG1_SLEEP_CEN      0x0001  /* This bit enables the sleep counter */
+#define AON_CFG1_SMXX           0x0002  /* This bit needs to be set to 0 for correct operation in the SLEEP state within the DW1000 */
+#define AON_CFG1_LPOSC_CAL      0x0004  /* This bit enables the calibration function that measures the period of the IC’s internal low powered oscillator */
+
+/****************************************************************************//**
+ * @brief Bit definitions for register OTP_IF
+ * Refer to section 7.2.46 Register file: 0x2D – OTP Memory Interface
+**/
+#define OTP_IF_ID               0x2D            /* One Time Programmable Memory Interface */
+#define OTP_IF_LEN              (18)
+/* offset from OTP_IF_ID in bytes */
+#define OTP_WDAT                0x00            /* 32-bit register. The data value to be programmed into an OTP location  */
+#define OTP_WDAT_LEN            (4)
+/* offset from OTP_IF_ID in bytes */
+#define OTP_ADDR                0x04            /* 16-bit register used to select the address within the OTP memory block */
+#define OTP_ADDR_LEN            (2)
+#define OTP_ADDR_MASK           0x07FF          /* This 11-bit field specifies the address within OTP memory that will be accessed read or written. */
+/* offset from OTP_IF_ID in bytes */
+#define OTP_CTRL                0x06            /* used to control the operation of the OTP memory */
+#define OTP_CTRL_LEN            (2)
+#define OTP_CTRL_MASK           0x8002
+#define OTP_CTRL_OTPRDEN        0x0001          /* This bit forces the OTP into manual read mode */
+#define OTP_CTRL_OTPREAD        0x0002          /* This bit commands a read operation from the address specified in the OTP_ADDR register */
+#define OTP_CTRL_LDELOAD        0x8000          /* This bit forces a load of LDE microcode */
+#define OTP_CTRL_OTPPROG        0x0040          /* Setting this bit will cause the contents of OTP_WDAT to be written to OTP_ADDR. */
+/* offset from OTP_IF_ID in bytes */
+#define OTP_STAT                0x08
+#define OTP_STAT_LEN            (2)
+#define OTP_STAT_MASK           0x0003
+#define OTP_STAT_OTPPRGD        0x0001          /* OTP Programming Done */
+#define OTP_STAT_OTPVPOK        0x0002          /* OTP Programming Voltage OK */
+/* offset from OTP_IF_ID in bytes */
+#define OTP_RDAT                0x0A            /* 32-bit register. The data value read from an OTP location will appear here */
+#define OTP_RDAT_LEN            (4)
+/* offset from OTP_IF_ID in bytes */
+#define OTP_SRDAT               0x0E            /* 32-bit register. The data value stored in the OTP SR (0x400) location will appear here after power up */
+#define OTP_SRDAT_LEN           (4)
+/* offset from OTP_IF_ID in bytes */
+#define OTP_SF                  0x12            /*8-bit special function register used to select and load special receiver operational parameter */
+#define OTP_SF_LEN              (1)
+#define OTP_SF_MASK             0x63
+#define OTP_SF_OPS_KICK         0x01            /* This bit when set initiates a load of the operating parameter set selected by the OPS_SEL */
+#define OTP_SF_LDO_KICK         0x02            /* This bit when set initiates a load of the LDO tune code */
+#define OTP_SF_OPS_SEL_SHFT     5
+#define OTP_SF_OPS_SEL_MASK     0x60
+#define OTP_SF_OPS_SEL_L64      0x00            /* Operating parameter set selection: Length64 */
+#define OTP_SF_OPS_SEL_TIGHT    0x40            /* Operating parameter set selection: Tight */
+
+/****************************************************************************//**
+ * @brief Bit definitions for register LDE_IF
+ * Refer to section 7.2.47 Register file: 0x2E – Leading Edge Detection Interface
+ * PLEASE NOTE: Other areas within the address space of Register file: 0x2E – Leading Edge Detection Interface
+ * are reserved. To ensure proper operation of the LDE algorithm (i.e. to avoid loss of performance or a malfunction),
+ * care must be taken not to write to any byte locations other than those defined in the sub-sections below.
+**/
+#define LDE_IF_ID               0x2E            /* Leading edge detection control block */
+#define LDE_IF_LEN              (0)
+/* offset from LDE_IF_ID in bytes */
+#define LDE_THRESH_OFFSET       0x0000  /* 16-bit status register reporting the threshold that was used to find the first path */
+#define LDE_THRESH_LEN          (2)
+/* offset from LDE_IF_ID in bytes */
+#define LDE_CFG1_OFFSET         0x0806  /*8-bit configuration register*/
+#define LDE_CFG1_LEN            (1)
+#define LDE_CFG1_NSTDEV_MASK    0x1F    /* Number of Standard Deviations mask. */
+#define LDE_CFG1_PMULT_MASK     0xE0    /* Peak Multiplier mask. */
+/* offset from LDE_IF_ID in bytes */
+#define LDE_PPINDX_OFFSET       0x1000  /* reporting the position within the accumulator that the LDE algorithm has determined to contain the maximum */
+#define LDE_PPINDX_LEN          (2)
+/* offset from LDE_IF_ID in bytes */
+#define LDE_PPAMPL_OFFSET       0x1002  /* reporting the magnitude of the peak signal seen in the accumulator data memory */
+#define LDE_PPAMPL_LEN          (2)
+/* offset from LDE_IF_ID in bytes */
+#define LDE_RXANTD_OFFSET       0x1804  /* 16-bit configuration register for setting the receive antenna delay */
+#define LDE_RXANTD_LEN          (2)
+/* offset from LDE_IF_ID in bytes */
+#define LDE_CFG2_OFFSET         0x1806  /* 16-bit LDE configuration tuning register */
+#define LDE_CFG2_LEN            (2)
+/* offset from LDE_IF_ID in bytes */
+#define LDE_REPC_OFFSET         0x2804  /* 16-bit configuration register for setting the replica avoidance coefficient */
+#define LDE_REPC_LEN            (2)
+#define LDE_REPC_PCODE_1        0x5998
+#define LDE_REPC_PCODE_2        0x5998
+#define LDE_REPC_PCODE_3        0x51EA
+#define LDE_REPC_PCODE_4        0x428E
+#define LDE_REPC_PCODE_5        0x451E
+#define LDE_REPC_PCODE_6        0x2E14
+#define LDE_REPC_PCODE_7        0x8000
+#define LDE_REPC_PCODE_8        0x51EA
+#define LDE_REPC_PCODE_9        0x28F4
+#define LDE_REPC_PCODE_10       0x3332
+#define LDE_REPC_PCODE_11       0x3AE0
+#define LDE_REPC_PCODE_12       0x3D70
+#define LDE_REPC_PCODE_13       0x3AE0
+#define LDE_REPC_PCODE_14       0x35C2
+#define LDE_REPC_PCODE_15       0x2B84
+#define LDE_REPC_PCODE_16       0x35C2
+#define LDE_REPC_PCODE_17       0x3332
+#define LDE_REPC_PCODE_18       0x35C2
+#define LDE_REPC_PCODE_19       0x35C2
+#define LDE_REPC_PCODE_20       0x47AE
+#define LDE_REPC_PCODE_21       0x3AE0
+#define LDE_REPC_PCODE_22       0x3850
+#define LDE_REPC_PCODE_23       0x30A2
+#define LDE_REPC_PCODE_24       0x3850
+
+/****************************************************************************//**
+ * @brief Bit definitions for register DIG_DIAG
+ * Digital Diagnostics interface.
+ * It contains a number of sub-registers that give diagnostics information.
+**/
+#define DIG_DIAG_ID             0x2F        /* Digital Diagnostics Interface */
+#define DIG_DIAG_LEN            (41)
+
+/* offset from DIG_DIAG_ID in bytes */
+#define EVC_CTRL_OFFSET         0x00        /* Event Counter Control */
+#define EVC_CTRL_LEN            (4)
+#define EVC_CTRL_MASK           0x00000003UL/* access mask to Register for bits should always be set to zero to avoid any malfunction of the device. */
+#define EVC_EN                  0x00000001UL/* Event Counters Enable bit */
+#define EVC_CLR                 0x00000002UL
+
+/* offset from DIG_DIAG_ID in bytes */
+#define EVC_PHE_OFFSET          0x04        /* PHR Error Event Counter */
+#define EVC_PHE_LEN             (2)
+#define EVC_PHE_MASK            0x0FFF
+/* offset from DIG_DIAG_ID in bytes */
+#define EVC_RSE_OFFSET          0x06        /* Reed Solomon decoder (Frame Sync Loss) Error Event Counter */
+#define EVC_RSE_LEN             (2)
+#define EVC_RSE_MASK            0x0FFF
+
+/* offset from DIG_DIAG_ID in bytes */
+#define EVC_FCG_OFFSET          0x08        /* The EVC_FCG field is a 12-bit counter of the frames received with good CRC/FCS sequence. */
+#define EVC_FCG_LEN             (2)
+#define EVC_FCG_MASK            0x0FFF
+/* offset from DIG_DIAG_ID in bytes */
+#define EVC_FCE_OFFSET          0x0A        /* The EVC_FCE field is a 12-bit counter of the frames received with bad CRC/FCS sequence. */
+#define EVC_FCE_LEN             (2)
+#define EVC_FCE_MASK            0x0FFF
+
+/* offset from DIG_DIAG_ID in bytes */
+#define EVC_FFR_OFFSET          0x0C        /* The EVC_FFR field is a 12-bit counter of the frames rejected by the receive frame filtering function. */
+#define EVC_FFR_LEN             (2)
+#define EVC_FFR_MASK            0x0FFF
+/* offset from DIG_DIAG_ID in bytes */
+#define EVC_OVR_OFFSET          0x0E        /* The EVC_OVR field is a 12-bit counter of receive overrun events */
+#define EVC_OVR_LEN             (2)
+#define EVC_OVR_MASK            0x0FFF
+
+/* offset from DIG_DIAG_ID in bytes */
+#define EVC_STO_OFFSET          0x10        /* The EVC_STO field is a 12-bit counter of SFD Timeout Error events */
+#define EVC_OVR_LEN             (2)
+#define EVC_OVR_MASK            0x0FFF
+/* offset from DIG_DIAG_ID in bytes */
+#define EVC_PTO_OFFSET          0x12        /* The EVC_PTO field is a 12-bit counter of Preamble detection Timeout events */
+#define EVC_PTO_LEN             (2)
+#define EVC_PTO_MASK            0x0FFF
+
+/* offset from DIG_DIAG_ID in bytes */
+#define EVC_FWTO_OFFSET         0x14        /* The EVC_FWTO field is a 12-bit counter of receive frame wait timeout events */
+#define EVC_FWTO_LEN            (2)
+#define EVC_FWTO_MASK           0x0FFF
+/* offset from DIG_DIAG_ID in bytes */
+#define EVC_TXFS_OFFSET         0x16        /* The EVC_TXFS field is a 12-bit counter of transmit frames sent. This is incremented every time a frame is sent */
+#define EVC_TXFS_LEN            (2)
+#define EVC_TXFS_MASK           0x0FFF
+
+/* offset from DIG_DIAG_ID in bytes */
+#define EVC_HPW_OFFSET          0x18        /* The EVC_HPW field is a 12-bit counter of “Half Period Warnings”. */
+#define EVC_HPW_LEN             (2)
+#define EVC_HPW_MASK            0x0FFF
+/* offset from DIG_DIAG_ID in bytes */
+#define EVC_TPW_OFFSET          0x1A        /* The EVC_TPW field is a 12-bit counter of “Transmitter Power-Up Warnings”. */
+#define EVC_TPW_LEN             (2)
+#define EVC_TPW_MASK            0x0FFF
+
+/* offset from DIG_DIAG_ID in bytes */
+#define EVC_RES1_OFFSET         0x1C        /* Please take care not to write to this register as doing so may cause the DW1000 to malfunction. */
+
+/* offset from DIG_DIAG_ID in bytes */
+#define DIAG_TMC_OFFSET         0x24
+#define DIAG_TMC_LEN            (2)
+#define DIAG_TMC_MASK           0x0010
+#define DIAG_TMC_TX_PSTM        0x0010      /* This test mode is provided to help support regulatory approvals spectral testing. When the TX_PSTM bit is set it enables a repeating transmission of the data from the TX_BUFFER */
+
+
+/****************************************************************************//**
+ * @brief Bit definitions for register 0x30-0x35
+ * Please take care not to write to these registers as doing so may cause the DW1000 to malfunction.
+**/
+#define REG_30_ID_RESERVED      0x30
+#define REG_31_ID_RESERVED      0x31
+#define REG_32_ID_RESERVED      0x32
+#define REG_33_ID_RESERVED      0x33
+#define REG_34_ID_RESERVED      0x34
+#define REG_35_ID_RESERVED      0x35
+
+/****************************************************************************//**
+ * @brief Bit definitions for register PMSC
+**/
+#define PMSC_ID                 0x36            /* Power Management System Control Block */
+#define PMSC_LEN                (48)
+/* offset from PMSC_ID in bytes */
+#define PMSC_CTRL0_OFFSET       0x00
+#define PMSC_CTRL0_LEN          (4)
+#define PMSC_CTRL0_MASK         0xF18F847FUL    /* access mask to register PMSC_CTRL0 */
+#define PMSC_CTRL0_SYSCLKS_AUTO 0x00000000UL    /* The system clock will run off the 19.2 MHz XTI clock until the PLL is calibrated and locked, then it will switch over the 125 MHz PLL clock */
+#define PMSC_CTRL0_SYSCLKS_19M  0x00000001UL    /* Force system clock to be the 19.2 MHz XTI clock. */
+#define PMSC_CTRL0_SYSCLKS_125M 0x00000002UL    /* Force system clock to the 125 MHz PLL clock. */
+#define PMSC_CTRL0_RXCLKS_AUTO  0x00000000UL    /* The RX clock will be disabled until it is required for an RX operation */
+#define PMSC_CTRL0_RXCLKS_19M   0x00000004UL    /* Force RX clock enable and sourced clock from the 19.2 MHz XTI clock */
+#define PMSC_CTRL0_RXCLKS_125M  0x00000008UL    /* Force RX clock enable and sourced from the 125 MHz PLL clock */
+#define PMSC_CTRL0_RXCLKS_OFF   0x0000000CUL    /* Force RX clock off. */
+#define PMSC_CTRL0_TXCLKS_AUTO  0x00000000UL    /* The TX clock will be disabled until it is required for a TX operation */
+#define PMSC_CTRL0_TXCLKS_19M   0x00000010UL    /* Force TX clock enable and sourced clock from the 19.2 MHz XTI clock */
+#define PMSC_CTRL0_TXCLKS_125M  0x00000020UL    /* Force TX clock enable and sourced from the 125 MHz PLL clock */
+#define PMSC_CTRL0_TXCLKS_OFF   0x00000030UL    /* Force TX clock off */
+#define PMSC_CTRL0_FACE         0x00000040UL    /* Force Accumulator Clock Enable */
+#define PMSC_CTRL0_GPDCE        0x00040000UL    /* GPIO De-bounce Clock Enable */
+#define PMSC_CTRL0_KHZCLEN      0x00800000UL    /* Kilohertz Clock Enable */
+#define PMSC_CTRL0_PLL2_SEQ_EN  0x01000000UL    /* Enable PLL2 on/off sequencing by SNIFF mode */
+#define PMSC_CTRL0_SOFTRESET_OFFSET 3           /* In bytes */
+#define PMSC_CTRL0_RESET_ALL    0x00            /* Assuming only 4th byte of the register is read */
+#define PMSC_CTRL0_RESET_RX     0xE0            /* Assuming only 4th byte of the register is read */
+#define PMSC_CTRL0_RESET_CLEAR  0xF0            /* Assuming only 4th byte of the register is read */
+/* offset from PMSC_ID in bytes */
+#define PMSC_CTRL1_OFFSET       0x04
+#define PMSC_CTRL1_LEN          (4)
+#define PMSC_CTRL1_MASK         0xFC02F802UL    /* access mask to register PMSC_CTRL1 */
+#define PMSC_CTRL1_ARX2INIT     0x00000002UL    /* Automatic transition from receive mode into the INIT state */
+#define PMSC_CTRL1_ATXSLP       0x00000800UL    /* If this bit is set then the DW1000 will automatically transition into SLEEP or DEEPSLEEP mode after transmission of a frame */
+#define PMSC_CTRL1_ARXSLP       0x00001000UL    /* this bit is set then the DW1000 will automatically transition into SLEEP mode after a receive attempt */
+#define PMSC_CTRL1_SNOZE        0x00002000UL    /* Snooze Enable */
+#define PMSC_CTRL1_SNOZR        0x00004000UL    /* The SNOZR bit is set to allow the snooze timer to repeat twice */
+#define PMSC_CTRL1_PLLSYN       0x00008000UL    /* This enables a special 1 GHz clock used for some external SYNC modes */
+#define PMSC_CTRL1_LDERUNE      0x00020000UL    /* This bit enables the running of the LDE algorithm */
+#define PMSC_CTRL1_KHZCLKDIV_MASK   0xFC000000UL    /* Kilohertz clock divisor */
+#define PMSC_CTRL1_PKTSEQ_DISABLE   0x00        /* writing this to PMSC CONTROL 1 register (bits 10-3) disables PMSC control of analog RF subsystems */
+#define PMSC_CTRL1_PKTSEQ_ENABLE    0xE7        /* writing this to PMSC CONTROL 1 register (bits 10-3) enables PMSC control of analog RF subsystems */
+/* offset from PMSC_ID in bytes */
+#define PMSC_RES1_OFFSET        0x08
+/* offset from PMSC_ID in bytes */
+#define PMSC_SNOZT_OFFSET       0x0C            /* PMSC Snooze Time Register */
+#define PMSC_SNOZT_LEN          (1)
+/* offset from PMSC_ID in bytes */
+#define PMSC_RES2_OFFSET        0x10
+/* offset from PMSC_ID in bytes */
+#define PMSC_RES3_OFFSET        0x24
+/* offset from PMSC_ID in bytes */
+#define PMSC_TXFINESEQ_OFFSET   0x26
+#define PMSC_TXFINESEQ_DISABLE  0x0             /* Writing this disables fine grain sequencing in the transmitter */
+#define PMSC_TXFINESEQ_ENABLE   0x0B74          /* Writing this enables fine grain sequencing in the transmitter */
+/* offset from PMSC_ID in bytes */
+#define PMSC_LEDC_OFFSET        0x28
+#define PMSC_LEDC_LEN           (4)
+#define PMSC_LEDC_MASK          0x000001FFUL    /* 32-bit LED control register. */
+#define PMSC_LEDC_BLINK_TIM_MASK 0x000000FFUL   /* This field determines how long the LEDs remain lit after an event that causes them to be set on. */
+#define PMSC_LEDC_BLNKEN        0x00000100UL    /* Blink Enable. When this bit is set to 1 the LED blink feature is enabled. */
+/* Default blink time. Blink time is expressed in multiples of 14 ms. The value defined here is ~225 ms. */
+#define PMSC_LEDC_BLINK_TIME_DEF 0x10
+/* Command a blink of all LEDs */
+#define PMSC_LEDC_BLINK_NOW_ALL 0x000F0000UL
+
+/****************************************************************************//**
+ * @brief Bit definitions for register 0x37-0x3F
+ * Please take care not to write to these registers as doing so may cause the DW1000 to malfunction.
+**/
+#define REG_37_ID_RESERVED      0x37
+#define REG_38_ID_RESERVED      0x38
+#define REG_39_ID_RESERVED      0x39
+#define REG_3A_ID_RESERVED      0x3A
+#define REG_3B_ID_RESERVED      0x3B
+#define REG_3C_ID_RESERVED      0x3C
+#define REG_3D_ID_RESERVED      0x3D
+#define REG_3E_ID_RESERVED      0x3E
+#define REG_3F_ID_RESERVED      0x3F
+
+/* END DW1000 REGISTER DEFINITION */
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/decadriver/deca_types.h	Wed Dec 06 21:35:45 2017 +0000
@@ -0,0 +1,73 @@
+/*! ----------------------------------------------------------------------------
+ *  @file   deca_types.h
+ *  @brief  Decawave general type definitions
+ *
+ * @attention
+ *
+ * Copyright 2013 (c) Decawave Ltd, Dublin, Ireland.
+ *
+ * All rights reserved.
+ *
+ */
+
+#ifndef _DECA_TYPES_H_
+#define _DECA_TYPES_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#ifndef uint8
+#ifndef _DECA_UINT8_
+#define _DECA_UINT8_
+typedef unsigned char uint8;
+#endif
+#endif
+
+#ifndef uint16
+#ifndef _DECA_UINT16_
+#define _DECA_UINT16_
+typedef unsigned short uint16;
+#endif
+#endif
+
+#ifndef uint32
+#ifndef _DECA_UINT32_
+#define _DECA_UINT32_
+typedef unsigned long uint32;
+#endif
+#endif
+
+#ifndef int8
+#ifndef _DECA_INT8_
+#define _DECA_INT8_
+typedef signed char int8;
+#endif
+#endif
+
+#ifndef int16
+#ifndef _DECA_INT16_
+#define _DECA_INT16_
+typedef signed short int16;
+#endif
+#endif
+
+#ifndef int32
+#ifndef _DECA_INT32_
+#define _DECA_INT32_
+typedef signed long int32;
+#endif
+#endif
+
+#ifndef NULL
+#define NULL ((void *)0UL)
+#endif
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* DECA_TYPES_H_ */
+
+
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/decadriver/deca_version.h	Wed Dec 06 21:35:45 2017 +0000
@@ -0,0 +1,33 @@
+/*! ----------------------------------------------------------------------------
+ * @file    deca_version.h
+ * @brief   Defines the version info for the DW1000 device driver including its API
+ *
+ * @attention
+ *
+ * Copyright 2013 (c) Decawave Ltd, Dublin, Ireland.
+ *
+ * All rights reserved.
+ *
+ */
+
+#ifndef _DECA_VERSION_H_
+#define _DECA_VERSION_H_
+
+//
+// The DW1000 device driver is separately version numbered to any version the application using it may have
+//
+// Two symbols are defined here: one hexadecimal value and one string that includes the hex bytes.
+// Both should be updated together in a consistent way when the software is being modified.
+//
+// The format of the hex version is 0xAABBCC and the string ends with AA.BB.CC, where...
+//
+// Quantity CC is updated for minor changes/bug fixes that should not need user code changes
+// Quantity BB is updated for changes/bug fixes that may need user code changes
+// Quantity AA is updated for major changes that will need user code changes
+//
+
+#define DW1000_DRIVER_VERSION               0x040001
+#define DW1000_DEVICE_DRIVER_VER_STRING     "DW1000 Device Driver Version 04.00.01"
+
+#endif
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/globals.h	Wed Dec 06 21:35:45 2017 +0000
@@ -0,0 +1,29 @@
+/*#include "PC.h" 
+#include "mbed.h"
+#include "DecaWave.h"*/
+#ifndef _GLOBALS_H
+#define _GLOBALS_H
+
+#include "DecaWave.h"
+#include "PC.h" 
+#include "mbed.h"/*
+#include "SMConfig.h"
+#include "DecaWave.h"                  // DW1000 functions      
+#include "LMMN2WR.h"                   // 2-Way-Ranging
+#include "PC.h"                        // Serial Port via USB for debugging with Terminal
+#include "Watchdog.h"                  // Resets Program if it gets stuck*/
+//#include "frames.h"                  // Resets Program if it gets stuck
+//#include "LMMN2WR.h"
+
+
+
+
+//Declarations for global Variables. They are all defined somewhere throughout the project.
+extern SPI decaWaveSpi;
+extern DigitalOut decaWaveCs;
+extern InterruptIn decaWaveIrq;
+extern PC pc;           // USB UART Terminal
+//extern LMMN2WR node;
+
+
+#endif
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main_minimal.cpp	Wed Dec 06 21:35:45 2017 +0000
@@ -0,0 +1,354 @@
+#include "mbed.h"
+
+
+#include "SMConfig.h"
+#include "DecaWave.h"                  // DW1000 functions      
+//#include "LMMN2WR.h"                   // 2-Way-Ranging
+#include "PC.h"                        // Serial Port via USB for debugging with Terminal
+#include "Watchdog.h"                  // Resets Program if it gets stuck
+#include "nodes.h"
+ #include <stddef.h>
+
+#define NELEMS(x)  (sizeof(x) / sizeof((x)[0]))
+
+// Function Prototypes
+void dwCallbackTx(const dwt_cb_data_t *rxd);
+void dwCallbackRx(const dwt_cb_data_t *rxd);
+int min (int a, int b);
+void configureDW1000(uint8_t dwMode);
+void rangeAndDisplayOne(uint8_t addr);
+void rangeAndDisplayAll();
+void executeOrder(char* command);
+
+
+// PA_7 MOSI, PA_6 MISO, PA_5 SCLK, PB_6 CS, PB_9 IRQ
+SPI         decaWaveSpi(PA_7, PA_6, PA_5);      // Instance of SPI connection to DW1000
+DigitalOut  decaWaveCs(PB_6);
+InterruptIn decaWaveIrq(PB_9);
+DecaWave    decaWave = DecaWave();              // Instance of the DW1000
+
+
+/* BIT and PINS
+BIT 1 = PA_10
+BIT 2 = PB_3
+BIT 3 = PB_5
+BIT 4 = PB_4
+BIT 5 = PB_10
+BIT 6 = PA_8
+BIT 7 = PA_9
+BIT 8 = PC_7
+*/
+
+
+BusIn adressInput(PA_9,PA_8,PB_10,PB_4,PB_5,PB_3,PA_10); // first seven 7 bit for ID settings, most left bit = most significant bit
+PC          pc(USBTX, USBRX, 921600);           // USB UART Terminal
+DigitalIn anchorInput(PC_7); // usage of last bit as deciding bit for: anchor or beacon
+Watchdog    wdt = Watchdog();
+BeaconNode  beaconNode(decaWave);
+AnchorNode  anchorNode(decaWave);
+BaseStationNode  baseStationNode(decaWave);
+Node        *node;
+
+
+int main() {
+  
+  decaWaveSpi.frequency(20000000); // Crank up the SPI frequency from 1Mhz to 3Mhz (maybe more?)
+
+  // Check if Reset was from Watchdog or externally
+  if(RCC->CSR&0x20000000)
+    pc.printf("\r\n\r\n --- !!!!WATCHDOG RESET!!!! --- \r\n\r\n", RCC->CSR); 
+  else if(RCC->CSR&0x4000000){
+    pc.printf("\r\n\r\n --- External Reset --- \r\n\r\n", RCC->CSR);
+  //wdt.kick();
+    }    
+    __HAL_RCC_CLEAR_RESET_FLAGS();
+
+  // Set all Switches Pull-Down so that it is zero if Switch is not set   
+
+    adressInput.mode(PullDown);    
+  anchorInput.mode(PullDown);
+  //modeInput.mode(PullDown);
+  wait_ms(50);
+  
+    baseStationNode.setAddress(adressInput & adressInput.mask());  
+    anchorNode.setAddress(adressInput & adressInput.mask());  
+    beaconNode.setAddress(adressInput & adressInput.mask());  
+    
+    if((adressInput & adressInput.mask()) == BASE_STATION_ADDR){
+        node = &baseStationNode; 
+        pc.printf("This node is the Base Station, Adress: %d \r\n \r\n \r\n", node->getAddress());        
+       // wdt.kick(2);            // Set up WatchDog         
+   
+    }
+    else if(anchorInput){
+        node = &anchorNode;  
+        pc.printf("This node is an Anchor node, Adress: %d \r\n \r\n \r\n", node->getAddress()); 
+        wdt.kick(2);            // Set up WatchDog         
+    }     
+    else{
+        node = &beaconNode; 
+        pc.printf("This node is a Beacon, Adress: %d \r\n \r\n \r\n", node->getAddress());
+        wdt.kick(2);            // Set up WatchDog   
+    }
+
+    //pc.printf(" Adress: %d \r\n \r\n \r\n", node->getAddress());
+       
+    configureDW1000(7); 
+ 
+    // TODO turn on RXMode regularly (every couple seconds)
+    
+    char command_str[30];
+    
+    while(1) {
+        
+        // Choose between what to execte based on whether this device is a:
+        // BEACON
+        // BASESTATION
+        // ANCHOR
+        if (!node->isAnchor() && !node->isBaseStation()){
+            
+            // THE BEACON EXECUTES THIS
+            if(beaconNode.getRepetitions() > 0){
+                switch(beaconNode.getMode()){
+                    case RANGE_ALL: rangeAndDisplayAll(); 
+                                    break;
+                    case RANGE_ONE: rangeAndDisplayOne(beaconNode.getDestination());
+                                    break;
+                    default:        break;
+                }
+                beaconNode.decreaseRepetitions();
+            }
+            else{
+                beaconNode.clearRec();
+                wait_ms(8);
+            }
+            wdt.kick(); 
+        } 
+        else if (node->isBaseStation()){
+             
+            //pc.readcommand(executeOrder);
+    
+             // EXECUTE THIS IF A BASE STATION
+            pc.readcommand(executeOrder);
+            wait_ms(10);
+            wdt.kick();
+            /*
+            while (1)
+            {
+                if (pc.readable())
+                {
+                    pc.scanf( "%s" , command_str );
+                    break;
+                }
+            }
+            */
+            //wait_ms(3);
+            //pc.printf("Command Received: %s", command_str);
+            //wait_ms(3);
+            //executeOrder(command_str);
+            //wait_ms(3);
+        }
+        else { // All Anchor Action is in the Interrupt functions!
+            // EXECUTE THIS IF AN ANCHOR
+            wait_ms(10);
+            wdt.kick(); 
+        }
+    }  
+
+    
+}
+
+
+void executeOrder(char* command){
+
+    int repetitions = command[3]*100 + command[4]*10 + command[5] - 5328;
+    //uint8_t dest1 = command[7] - 48;
+    uint8_t dest2 = command[8] - 48;
+    uint8_t dest3 = command[9] - 48;
+    uint8_t dest1=0;
+  
+    
+   
+    
+    if(command[7] == 0 && command[8] == 0)
+    {
+    dest1 =    command[9] - 48;
+    }
+    else if (command[7] == 0 && command[8] != 0)
+    {
+    dest1 =     command[8] * 10 + command[9] - 528;
+    }
+    else if (command[7] != 0 && command[8] != 0) 
+    {
+    dest1 = command[7] * 100 + command[8] * 10 + command[9] - 5328;
+    }
+    
+    if (strncmp(command, "reset", 5) == 0){ // This command is implemented in order to be able to reset BaseStation from Matlab. 
+        wdt.kick(2);            // Set up WatchDog   
+        pc.printf("Base Station is RESETTED  \r\n\r\n");
+    }
+    
+    
+    else if (strncmp(command, "all", 3) == 0){
+        baseStationNode.sendOrder(0, NOT_USED, RANGE_ALL, repetitions, Node::BASE_ORDER); 
+       // pc.printf("Mode: Range all \r\n");
+    }
+    else if (strncmp(command, "one", 3) == 0){
+        
+        
+        
+        if(dest1 != 15)
+        {
+           
+            baseStationNode.sendOrder(0, dest1, RANGE_ONE, repetitions, Node::BASE_ORDER); 
+        }
+        else
+        {
+            baseStationNode.sendOrder(0, 1, RANGE_ONE, repetitions, Node::BASE_ORDER); 
+        }
+    }
+    
+    else if (strncmp(command, "bea", 3) == 0){
+        if(dest1 < 15)        
+            baseStationNode.sendOrder(dest1, NOT_USED, BECOME_BEACON, NOT_USED, Node::SWITCH_TYPE);
+        else
+            baseStationNode.sendOrder(0, NOT_USED, BECOME_BEACON, NOT_USED, Node::SWITCH_TYPE);
+         
+    }
+    
+    else if (strncmp(command, "anc", 3) == 0){
+        if(dest1 < 15)   
+            baseStationNode.sendOrder(dest1, NOT_USED, BECOME_ANCHOR, NOT_USED, Node::SWITCH_TYPE); 
+        else
+            baseStationNode.sendOrder(0, NOT_USED, BECOME_ANCHOR, NOT_USED, Node::SWITCH_TYPE); 
+    }
+    
+    else if (strncmp(command, "tri", 3) == 0){
+        // Switch them in correct modes (should already be the case)
+        baseStationNode.sendOrder(dest1, NOT_USED, BECOME_BEACON, NOT_USED, Node::SWITCH_TYPE);
+        baseStationNode.sendOrder(dest2, NOT_USED, BECOME_ANCHOR, NOT_USED, Node::SWITCH_TYPE);  
+        baseStationNode.sendOrder(dest3, NOT_USED, BECOME_ANCHOR, NOT_USED, Node::SWITCH_TYPE);  
+        
+        // Ranging from first node
+        baseStationNode.sendOrder(dest1, dest2, RANGE_ONE, repetitions, Node::BASE_ORDER); 
+        wait_ms(10*repetitions);
+        baseStationNode.sendOrder(dest1, dest3, RANGE_ONE, repetitions, Node::BASE_ORDER); 
+        wait_ms(10*repetitions);
+        
+        // Mode Switches
+        baseStationNode.sendOrder(dest2, NOT_USED, BECOME_BEACON, NOT_USED, Node::SWITCH_TYPE);  
+        baseStationNode.sendOrder(dest1, NOT_USED, BECOME_ANCHOR, NOT_USED, Node::SWITCH_TYPE);
+        
+        // Rangings
+        baseStationNode.sendOrder(dest2, dest1, RANGE_ONE, repetitions, Node::BASE_ORDER); 
+        wait_ms(10*repetitions);        
+        baseStationNode.sendOrder(dest2, dest3, RANGE_ONE, repetitions, Node::BASE_ORDER); 
+        wait_ms(10*repetitions);
+        
+        // Mode Switches
+        baseStationNode.sendOrder(dest3, NOT_USED, BECOME_BEACON, NOT_USED, Node::SWITCH_TYPE);  
+        baseStationNode.sendOrder(dest2, NOT_USED, BECOME_ANCHOR, NOT_USED, Node::SWITCH_TYPE);
+        
+        // Rangings
+        baseStationNode.sendOrder(dest3, dest1, RANGE_ONE, repetitions, Node::BASE_ORDER); 
+        wait_ms(10*repetitions);        
+        baseStationNode.sendOrder(dest3, dest2, RANGE_ONE, repetitions, Node::BASE_ORDER); 
+        wait_ms(10*repetitions);        
+        
+        // Back to original modes
+        baseStationNode.sendOrder(dest1, NOT_USED, BECOME_BEACON, NOT_USED, Node::SWITCH_TYPE);
+        baseStationNode.sendOrder(dest2, NOT_USED, BECOME_ANCHOR, NOT_USED, Node::SWITCH_TYPE);  
+        baseStationNode.sendOrder(dest3, NOT_USED, BECOME_ANCHOR, NOT_USED, Node::SWITCH_TYPE);
+        
+    } 
+    else
+    {
+    pc.printf("ERROR ------  This is an invalid command\n"); 
+    }
+}
+
+#pragma Otime // Compiler optimize Runtime at the cost of image size
+// Called after Frame was received
+void dwCallbackRx(const dwt_cb_data_t *rxd) {
+  int64_t rxTimeStamp = 0;  
+  dwt_readrxdata(node->getRecFrameRef(), min(node->getRecFrameLength(), rxd->datalength), 0);  // Read Data Frame from Registers 
+
+  dwt_readrxtimestamp((uint8_t*) &rxTimeStamp);  // Read Timestamp when the frame was received exactly
+  rxTimeStamp &= MASK_40BIT; //Mask the 40 Bits of the timestamp
+  
+  node->callbackRX(rxTimeStamp); // Two Way Ranging Function
+}
+
+
+#pragma Otime // Compiler optimize Runtime at the cost of image size
+// Called after Frame was transmitted
+void dwCallbackTx(const dwt_cb_data_t *txd) {
+  int64_t txTimeStamp = 0;
+  dwt_readtxtimestamp((uint8_t*) &txTimeStamp);  // Read Timestamp when the frame was transmitted exactly
+  txTimeStamp &= MASK_40BIT; // Delete the most significant 8 Bits because the timestamp has only 40 Bits
+
+  node->callbackTX(txTimeStamp); // Two Way Ranging Function
+}
+
+
+int min (int a, int b){
+    if(a<=b) return a;
+    return b;
+}
+
+void configureDW1000(uint8_t dwMode){
+  dwt_config_t dwConfig;
+  dwt_txconfig_t dwConfigTx;
+    
+  SMsetconfig(dwMode, &dwConfig, &dwConfigTx);
+
+  decaWave.setup(dwConfig, dwConfigTx, rfDelays[dwConfig.prf - DWT_PRF_16M], dwCallbackTx, dwCallbackRx);
+
+  pc.printf("%s\r\n", DW1000_DEVICE_DRIVER_VER_STRING);
+  {
+    uint16_t tempvbat = dwt_readtempvbat(1);
+    if (tempvbat>0) {
+      float tempC = 1.13f * (float) (tempvbat >> 8) - 113.0f;
+      float vbatV = 0.0057f * (float) (tempvbat & 0xFF) + 2.3f;
+
+      pc.printf("  Voltage: %f, Temperature: %f\r\n", vbatV, tempC);
+    } else {
+      pc.printf("ERROR: Cannot read voltage/temperature\r\n");
+    }
+  }
+  pc.printf("  Device Lot ID %lu, Part ID %lu\r\n", dwt_getlotid(), dwt_getpartid());
+
+  uint16_t DWID = (dwt_getpartid() & 0xFFFF);
+
+  pc.printf(
+      "  Settings %i: \r\n    Channel %u (%1.3f GHz w/ %1.3f GHz BW), Datarate %s, \r\n    PRF %s, Preamble Code %u, PreambleLength %u symbols, \r\n    PAC Size %u, %s SFD, SDF timeout %ul symbols\r\n",
+      dwMode, dwConfig.chan, ChannelFrequency[dwConfig.chan],
+    ChannelBandwidth[dwConfig.chan], ChannelBitrate[dwConfig.dataRate], ChannelPRF[dwConfig.prf],
+      dwConfig.txCode, ChannelPLEN(dwConfig.txPreambLength), ChannelPAC[dwConfig.rxPAC],
+      dwConfig.nsSFD==0?"standard":"non-standard", dwConfig.sfdTO);
+  pc.printf("    Power: NORM %2.1f dB, BOOST: 0.125ms %2.1f dB, 0.25ms %2.1f dB, 0.5ms %2.1f dB\r\n",
+    SMgain(dwConfigTx.power& 0xFF), SMgain((dwConfigTx.power>>24)& 0xFF), SMgain((dwConfigTx.power>>16)& 0xFF),
+    SMgain((dwConfigTx.power>>8)& 0xFF));
+  pc.printf("  Frame length: %d us\r\n", decaWave.computeFrameLength_us());
+  pc.printf("  Antenna Delay set to: %d \r\n", decaWave.getAntennaDelay());
+
+  decaWave.turnonrx();   // start listening       
+}
+
+void rangeAndDisplayOne(uint8_t addr){
+    beaconNode.requestRanging(addr); 
+    pc.printf("o error2 ? %f(%f) \r\n", beaconNode.getDistance(addr), beaconNode.getSignalStrength(addr));
+}
+
+
+
+void rangeAndDisplayAll(){
+    beaconNode.requestRangingAll(); 
+    for(int i = 0; i < ADRESSES_COUNT; i++){
+        /*if(beaconNode.getDistance(i) > -10){
+            pc.printf("#%d %f(%f), ", i, beaconNode.getDistance(i), beaconNode.getSignalStrength(i));
+        }*/
+    }
+   // pc.printf("o error3 ? \r\n");
+}
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-dev.lib	Wed Dec 06 21:35:45 2017 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed-dev/#447f873cad2f
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/nodes/frames.h	Wed Dec 06 21:35:45 2017 +0000
@@ -0,0 +1,27 @@
+// Lukas Bieri, Matthias, Manuel Meier & Noa Melchior 
+// based on Work by Matthias Grob & Manuel Stalder 
+// ETH 2017
+
+#ifndef _FRAMES_H
+#define _FRAMES_H
+
+//the packed attribute makes sure the types only use their respective size in memory (8 bit for uint8_t), otherwise they would always use 32 bit
+//IT IS A GCC SPECIFIC DIRECTIVE
+struct __attribute__((packed, aligned(1))) RangingFrame {
+    uint8_t source;
+    uint8_t destination;
+    uint8_t type;
+};
+
+struct __attribute__((packed, aligned(1))) ExtendedRangingFrame : RangingFrame{
+    int signedTime;
+};
+
+struct __attribute__((packed, aligned(1))) StreamFrame : RangingFrame{
+    uint8_t anchor_adress;
+    float distance;
+    float signalStrength;
+    float FPLevel;
+};
+
+#endif /* _FRAMES_H */
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/nodes/nodes.cpp	Wed Dec 06 21:35:45 2017 +0000
@@ -0,0 +1,504 @@
+#include "nodes.h"
+
+extern Node   *node;
+extern BeaconNode  beaconNode;
+extern AnchorNode  anchorNode;
+extern BaseStationNode  baseStationNode;
+
+Node::Node(DecaWave& DW) : dw(DW) {
+    address = 0;      
+}
+void Node::setAnchor(bool anc){
+    Anchor = anc;
+}
+
+bool Node::isAnchor(){
+    return Anchor;
+}
+bool Node::isBaseStation(){
+    return address == BASE_STATION_ADDR;
+}   
+void Node::setAddress(uint8_t adr){
+    address = adr;
+}
+
+uint8_t Node::getAddress(){
+    return address;
+}
+
+
+
+//------- BeaconNode Class -------------------
+BeaconNode::BeaconNode(DecaWave& DW) : Node(DW) {
+    Anchor = false;
+    for (int i = 0; i < 3; i++)
+        acknowledgement[i] = true;
+    LocalTimer.start();
+    mode=0;   
+}
+
+#pragma Otime // Compiler optimize Runtime at the cost of image size
+// Two Way Ranging Actions when Frame was received
+void BeaconNode::callbackRX(uint64_t RxTime) {
+   //pc.printf("Got a frame, adress: %d source: %d \r\n", receivedFrame.destination, receivedFrame.source);   
+        if (receivedFrame.destination == address){
+            switch (receivedFrame.type) {
+                case SWITCH_TYPE:
+                    sendBaseAck();
+                    if((receivedFrame.signedTime >> 24) == BECOME_BEACON){
+                        pc.printf("\r\n \r\n ---This Node is already a beacon ---\r\n \r\n");
+                    } 
+                    else if((receivedFrame.signedTime >> 24) == BECOME_ANCHOR){
+                        node = &anchorNode;
+                        pc.printf("\r\n \r\n ---This Node is an anchor now ---\r\n \r\n");
+                    }
+                    
+                    break;
+                case BASE_ORDER:
+                    repetitions = receivedFrame.signedTime&0xffff;
+                    mode = receivedFrame.signedTime >> 24;
+                    destination = (receivedFrame.signedTime&0xff0000) >> 16;
+                    sendBaseAck();
+                    break;
+                case ANCHOR_RESPONSE:
+                {
+                    sendAnswer(receivedFrame.source, BEACON_RESPONSE);  
+                    senderTimestamps[receivedFrame.source][1] = RxTime;        //Save the second timestamp on the sending node/beacon (T_rr)
+                    acknowledgement[0] = true;
+                    
+                     // Get Signal Level
+                    dwt_rxdiag_t diagnostic;
+                    dwt_readdiagnostics(&diagnostic);
+                    signalStrength[receivedFrame.source] = dw.getRXLevel(&diagnostic);               
+                    
+                    break;
+                }
+                case TRANSFER_FRAME:
+                {
+                    //timediffSend = 2 * senderTimestamps[receivedFrame.source][1] - senderTimestamps[receivedFrame.source][0] - senderTimestamps[receivedFrame.source][2];
+                    tofs[receivedFrame.source] = receivedFrame.signedTime + 2 * senderTimestamps[receivedFrame.source][1] - senderTimestamps[receivedFrame.source][0] - senderTimestamps[receivedFrame.source][2];   
+                    
+                    // Get Signal Level
+                    dwt_rxdiag_t diagnostic2;
+                    dwt_readdiagnostics(&diagnostic2);
+                    float level = dw.getRXLevel(&diagnostic2);
+                    if(level < signalStrength[receivedFrame.source])
+                        signalStrength[receivedFrame.source] = level;
+                                 
+                    acknowledgement[1] = true;
+                  
+                    //dw.turnonrx();   // start listening again
+                    break;
+                }
+                default : break;
+            }
+        }
+        else{
+          dw.turnonrx();   // start listening again
+        }
+}
+
+
+#pragma Otime // Compiler optimize Runtime at the cost of image size
+// Two Way Ranging Actions when Frame was transmitted
+void BeaconNode::callbackTX(uint64_t TxTime) {
+    //pc.printf("TXCallback: %d %d %d \r\n", acknowledgement[0], acknowledgement[1], acknowledgement[2]);    
+    dw.turnonrx();   // start listening again
+    
+    switch (rangingFrame.type) {
+        case PING:
+            senderTimestamps[rangingFrame.destination][0] = TxTime;    //Save the first timestamp on the sending node/beacon (T_sp)
+            break;
+        case BEACON_RESPONSE:
+            senderTimestamps[rangingFrame.destination][2] = TxTime;    //Save the third timestamp on the sending node/beacon (T_sr)
+            correctSenderTimestamps(rangingFrame.destination);         //Correct the timestamps for the case of a counter overflow        
+            break;   
+        default:
+            break;
+    }
+    if(acknowledgement[1] == true){
+        acknowledgement[2] = true; 
+       // pc.printf("Ack edited %d \r\n",  acknowledgement[2]);
+    }      
+}
+
+
+#pragma Otime // Compiler optimize Runtime at the cost of image size
+/**
+ *  Get the distance to the Anchor with address @param destination.
+ *
+ *   @param destination The address of the anchor
+ */
+void BeaconNode::requestRanging(uint8_t destination) {
+    if(noRec[destination] <= MAX_TRIES){
+        
+        
+        float time_before = LocalTimer.read();
+        
+        while(!acknowledgement[2] && LocalTimer.read() < time_before + 0.001f); // Wait until previous StreamFrame is sent 
+         
+        acknowledgement[0] = false;
+        acknowledgement[1] = false;
+        acknowledgement[2] = false;
+        time_before = LocalTimer.read();
+    
+        sendPingFrame(destination);
+    
+        while(!acknowledgement[1] && (LocalTimer.read() < time_before + 0.001f + 0.003f*acknowledgement[0])); // One Ranging normaly takes less than 1.5 miliseconds
+    
+        if(acknowledgement[1]){
+            distances[destination] = calibratedDistance(destination);
+            noRec[destination] = 0;
+            // Stream Data to Basestation                
+            sendStreamFrame(destination);         
+        } else {
+            distances[destination] = -10;
+            noRec[destination]++;
+        }     
+    }     
+}
+
+#pragma Otime // Compiler optimize Runtime at the cost of image size
+inline float BeaconNode::calibratedDistance(uint8_t destination) {
+
+    float rawDistance = (tofs[destination] * 300 * TIMEUNITS_TO_US / 4);
+    //float correctDistance = rawDistance + dwt_getrangebias(7, rawDistance, DWT_PRF_64M);
+
+    //if(rawDistance <= 8.458)
+    //    rawDistance -= 0.0541*rawDistance; // Correction Term 22-03-2017
+    //else
+    if(rawDistance >= 22.7)
+        rawDistance += -0.0004*rawDistance - 0.3971;
+    else if (rawDistance >= 14.3)
+        rawDistance += -0.0015*rawDistance - 0.372;
+    else if (rawDistance >= 8)
+        rawDistance += -0.0029*rawDistance - 0.352;
+    else if (rawDistance >= 3.93)
+        rawDistance += 0.001*rawDistance - 0.370;
+    else
+        rawDistance += -0.0235*rawDistance - 0.273;
+        
+    //else if (rawDistance >= 3)
+    //    rawDistance += 0.0004*rawDistance - 0.5556
+   /* else
+        rawDistance += -0.01799*rawDistance - 0.2724;
+        
+    else if ()*/
+        
+        
+        //Non-Correction-Term: rawDistance -= 0.458;
+    
+    
+ // Calibration for Nucleo 0 (and 1)
+
+ //   if (this->address == 1) rawDistance+= 10;
+//    switch(destination){
+//        case 2:
+//            return  rawDistance * 0.9754 - 0.5004;
+//        case 3:
+//            return  rawDistance * 0.9759 - 0.4103;
+//        case 4:
+//            return  rawDistance * 0.9798 - 0.5499;
+//        case 5:
+//            return  rawDistance * 0.9765 - 0.5169;
+//        }
+
+    return rawDistance;
+
+}
+
+#pragma Otime // Compiler optimize Runtime at the cost of image size
+void BeaconNode::requestRangingAll() {
+    for (int i = 0; i < ADRESSES_COUNT; i++) {  // Request ranging to all anchors
+        if(i != address){
+            requestRanging(i);  
+        }
+        else
+            distances[i] = -10;
+    }
+}
+
+#pragma Otime // Compiler optimize Runtime at the cost of image size
+void BeaconNode::sendPingFrame(uint8_t destination) {
+    rangingFrame.source = address;
+    rangingFrame.destination = destination;
+    rangingFrame.type = PING;
+    //dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame));
+    dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), 0, 0);
+}
+
+#pragma Otime // Compiler optimize Runtime at the cost of image size
+void BeaconNode::sendBaseAck() {
+    rangingFrame.source = address;
+    rangingFrame.destination = BASE_STATION_ADDR;
+    rangingFrame.type = BASE_ORDER_ACK;
+    //dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame));
+    dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), 0, 0);
+}
+
+#pragma Otime // Compiler optimize Runtime at the cost of image size
+void BeaconNode::sendStreamFrame(uint8_t anchor_addr) {    
+    StreamFrame streamFrame;
+        
+    streamFrame.source = address;
+    streamFrame.destination = BASE_STATION_ADDR;
+    streamFrame.type = STREAM_TO_BASE;
+    streamFrame.anchor_adress =  anchor_addr;
+    streamFrame.distance = getDistance(anchor_addr);
+    streamFrame.signalStrength = getSignalStrength(anchor_addr);
+    streamFrame.FPLevel = dw.getFPLevel();
+    
+    dw.sendFrame((uint8_t*)&streamFrame, sizeof(streamFrame), 0, 0);
+}
+
+#pragma Otime // Compiler optimize Runtime at the cost of image size
+void BeaconNode::sendAnswer(uint8_t destination, uint8_t type) {
+
+    rangingFrame.source = address;
+    rangingFrame.destination = destination;
+    rangingFrame.type = type;
+    
+    dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), 0, 0);
+}
+
+#pragma Otime // Compiler optimize Runtime at the cost of image size
+void BeaconNode::correctSenderTimestamps(uint8_t source){
+
+    if (senderTimestamps[source][0] - dw.getAntennaDelay() > senderTimestamps[source][1]) {
+        senderTimestamps[source][1] += MMRANGING_2POWER40;
+        senderTimestamps[source][2] += MMRANGING_2POWER40;
+    }
+    if (senderTimestamps[source][1] > senderTimestamps[source][2]) {
+        senderTimestamps[source][2] += MMRANGING_2POWER40;
+    }
+
+}
+
+float BeaconNode::getSignalStrength(uint8_t index){
+    return signalStrength[index];
+}  
+
+float BeaconNode::getDistance(uint8_t index){
+    return distances[index];
+} 
+
+uint8* BeaconNode::getRecFrameRef(){
+   return (uint8 *) &receivedFrame;           
+}
+uint16 BeaconNode::getRecFrameLength(){
+    return sizeof(receivedFrame);
+} 
+
+int BeaconNode::getMode(){
+    return mode;
+}  
+
+uint16_t BeaconNode::getRepetitions(){
+    return repetitions;
+}
+void BeaconNode::decreaseRepetitions(){
+    repetitions--;
+}
+uint8_t BeaconNode::getDestination(){
+    return destination;
+}
+
+void BeaconNode::clearRec(){
+    for(int j = 0; j < ADRESSES_COUNT; j++)
+        noRec[j] = 0;
+}
+
+
+//------- AnchorNode Class -------------------
+AnchorNode::AnchorNode(DecaWave& DW) : Node(DW) {
+    Anchor = true;
+}
+
+#pragma Otime // Compiler optimize Runtime at the cost of image size
+// Two Way Ranging Actions when Frame was received
+void AnchorNode::callbackRX(uint64_t RxTime) {
+   // pc.printf("Got a frame, adress: %d source: %d \r\n", receivedFrame.destination, receivedFrame.source);
+   // if(!isBaseStation()){    
+        if (receivedFrame.destination == address){
+            switch (receivedFrame.type) {
+                case SWITCH_TYPE:
+                    sendBaseAck();
+                    if((receivedFrame.signedTime >> 24) == BECOME_BEACON){
+                        node = &beaconNode; 
+                        pc.printf("\r\n \r\n ---This Node is a beacon now ---\r\n \r\n");
+                    } 
+                    else if((receivedFrame.signedTime >> 24) == BECOME_ANCHOR){
+                        pc.printf("\r\n \r\n ---This Node is already an anchor ---\r\n \r\n");
+                    }
+                    break;
+                case PING:
+                    sendAnswer(receivedFrame.source, ANCHOR_RESPONSE);  
+                    receiverTimestamps[receivedFrame.source][0] = RxTime;      //Save the first timestamp on the receiving node/anchor (T_rp)             
+                    break;
+                case BEACON_RESPONSE:
+                    {
+                    receiverTimestamps[receivedFrame.source][2] = RxTime;      //Save the third timestamp on the receiving node/anchor (T_rf)
+                    correctReceiverTimestamps(receivedFrame.source);                //Correct the timestamps for the case of a counter overflow
+                    //timediffRec = receiverTimestamps[receivedFrame.source][0] + receiverTimestamps[receivedFrame.source][2] - 2*receiverTimestamps[receivedFrame.source][1];
+                    //if(timediffRec < 0)
+                    //    timediffRec = 0;
+                    sendTransferFrame(receivedFrame.source, receiverTimestamps[receivedFrame.source][0] + receiverTimestamps[receivedFrame.source][2] - 2*receiverTimestamps[receivedFrame.source][1]);
+                    break;
+                    }
+                default : break;
+            }
+        }
+        else{
+          dw.turnonrx();   // start listening again
+        }
+}
+
+
+#pragma Otime // Compiler optimize Runtime at the cost of image size
+// Two Way Ranging Actions when Frame was transmitted
+void AnchorNode::callbackTX(uint64_t TxTime) {
+    dw.turnonrx();   // start listening again
+    
+    switch (rangingFrame.type) {
+        case ANCHOR_RESPONSE:
+            receiverTimestamps[rangingFrame.destination][1] = TxTime;  //Save the second timestamp on the receiving node/anchor (T_sr)
+            break;  
+        default:
+            break;
+    }   
+}
+
+
+
+
+#pragma Otime // Compiler optimize Runtime at the cost of image size
+void AnchorNode::sendBaseAck() {
+    rangingFrame.source = address;
+    rangingFrame.destination = BASE_STATION_ADDR;
+    rangingFrame.type = BASE_ORDER_ACK;
+    //dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame));
+    dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), 0, 0);
+}
+
+
+
+
+
+#pragma Otime // Compiler optimize Runtime at the cost of image size
+void AnchorNode::sendTransferFrame(uint8_t destination, int timeDiffsReceiver) {
+    ExtendedRangingFrame transferFrame;
+        
+    transferFrame.source = address;
+    transferFrame.destination = destination;
+    transferFrame.type = TRANSFER_FRAME;
+    transferFrame.signedTime =  timeDiffsReceiver;                      //cast the time difference
+    dw.sendFrame((uint8_t*)&transferFrame, sizeof(transferFrame), 0, 0);
+}
+
+#pragma Otime // Compiler optimize Runtime at the cost of image size
+void AnchorNode::sendAnswer(uint8_t destination, uint8_t type) {
+
+    rangingFrame.source = address;
+    rangingFrame.destination = destination;
+    rangingFrame.type = type;
+    
+    dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), 0, 0);
+}
+
+#pragma Otime // Compiler optimize Runtime at the cost of image size
+void AnchorNode::correctReceiverTimestamps(uint8_t source){
+
+    if(receiverTimestamps[source][0] > receiverTimestamps[source][1]){
+        receiverTimestamps[source][1] += MMRANGING_2POWER40;
+        receiverTimestamps[source][2] += MMRANGING_2POWER40;
+    }
+
+    if(receiverTimestamps[source][1] - dw.getAntennaDelay() > receiverTimestamps[source][2]){
+        receiverTimestamps[source][2] += MMRANGING_2POWER40;
+    }
+
+}
+
+
+
+uint8* AnchorNode::getRecFrameRef(){
+   return (uint8 *) &receivedFrame;           
+}
+uint16 AnchorNode::getRecFrameLength(){
+   return sizeof(receivedFrame);
+} 
+
+
+//------- BaseStationNode Class -------------------
+BaseStationNode::BaseStationNode(DecaWave& DW) : Node(DW) {
+    Anchor = false;
+    LocalTimer.start();    
+}
+
+uint8* BaseStationNode::getRecFrameRef(){
+   return (uint8 *) &receivedStreamFrame;           
+}
+uint16 BaseStationNode::getRecFrameLength(){
+   return sizeof(receivedStreamFrame);
+} 
+
+
+#pragma Otime // Compiler optimize Runtime at the cost of image size
+// Two Way Ranging Actions when Frame was received
+void BaseStationNode::callbackRX(uint64_t RxTime) {
+    //pc.printf("Got a frame, adress: %d source: %d \r\n", receivedStreamFrame.destination, receivedStreamFrame.source);
+    if (receivedStreamFrame.destination == address){
+        switch(receivedStreamFrame.type){
+            case STREAM_TO_BASE:
+                
+                //pc.printf("#%d to #%d %f(%f) \r\n", receivedStreamFrame.source, receivedStreamFrame.anchor_adress, receivedStreamFrame.distance, receivedStreamFrame.signalStrength);
+                pc.printf("#%03d/#%03d/%+011.6f/%+011.6f/%+011.6f/ \r\n", receivedStreamFrame.source, receivedStreamFrame.anchor_adress, receivedStreamFrame.distance, receivedStreamFrame.signalStrength, receivedStreamFrame.FPLevel/*dw.getFPLevel()*/);
+                break;
+            case BASE_ORDER_ACK:
+               
+                ack = true;
+                break;
+                
+            
+                
+        }
+    }
+    dw.turnonrx();   // start listening again
+    
+    
+}
+
+#pragma Otime // Compiler optimize Runtime at the cost of image size
+// Two Way Ranging Actions when Frame was transmitted
+void BaseStationNode::callbackTX(uint64_t TxTime) {  
+    dw.turnonrx();   // start listening again      
+}
+
+#pragma Otime // Compiler optimize Runtime at the cost of image size
+void BaseStationNode::sendOrder(uint8_t beacon_destination, uint8_t anchor_destination, uint8_t action, uint16_t repetitions, uint8_t type) {
+    ExtendedRangingFrame orderFrame;
+    ack = false;
+        
+    orderFrame.source = address;
+    orderFrame.destination = beacon_destination;
+    orderFrame.type = type;
+    orderFrame.signedTime =  action << 24 | anchor_destination << 16 | repetitions;           
+ 
+    int i = 0;     
+    for(i = 0; i < 10 && !ack; i++){
+        float time_before = LocalTimer.read(); 
+        dw.sendFrame((uint8_t*)&orderFrame, sizeof(orderFrame), 0, 0);
+        while(!ack && (LocalTimer.read() < time_before + 0.010 + 0.01*i)); // One Ranging normaly takes less than 1.5 miliseconds
+    }
+    if(!ack)
+    {
+        pc.printf("ERROR: Tag #%d did not respond \r\n", beacon_destination);
+    }
+    else
+    {
+        //pc.printf("Order sent, %d tries \r\n", i);
+    }
+        
+   
+        
+}
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/nodes/nodes.h	Wed Dec 06 21:35:45 2017 +0000
@@ -0,0 +1,168 @@
+// Lukas Bieri, Matthias, Manuel Meier & Noa Melchior 
+// based on Work by Matthias Grob & Manuel Stalder 
+// ETH 2017
+
+#ifndef _NODES_H
+#define _NODES_H
+
+#include "mbed.h"
+#include "DecaWave.h"
+#include "frames.h"
+
+#include "globals.h"
+
+#define TIMEUNITS_TO_US       (1/(128*499.2))    // conversion between the decawave timeunits (ca 15.65ps) to microseconds.
+#define US_TO_TIMEUNITS       (128*499.2)        // conversion between microseconds to the decawave timeunits (ca 15.65ps).
+#define MMRANGING_2POWER40     1099511627776     // decimal value of 2^40 to correct timeroverflow between timestamps
+#define ADRESSES_COUNT         10                // Defines the Adress Space that is covered when Ranging for all
+  
+  //static int ADRESSES_COUNT=1;                                           // Adress Space: 0 - (ADRESSES_COUNT - 1)
+#define MAX_TRIES              5                 // Number of times a Anchor is pinged until determined it is not in range
+#define BASE_STATION_ADDR      15                // Defines the Adress of the Base Station (reserved Adress)
+
+// Constants for Base_Orders
+#define RANGE_ALL               1                
+#define RANGE_ONE               0           
+#define NOT_USED                0xff
+
+// Constant for Switch Types Orders
+#define BECOME_BEACON           1                
+#define BECOME_ANCHOR           0
+
+
+class Node {
+    public:
+        Node(DecaWave& DW);
+    
+        void setAnchor(bool anc);
+        bool isAnchor();  
+        bool isBaseStation();      
+        void setAddress(uint8_t adr);
+        uint8_t getAddress(); 
+        
+        virtual uint8* getRecFrameRef() = 0;         
+        virtual uint16 getRecFrameLength() = 0;
+        
+        virtual void callbackRX(uint64_t RxTime) = 0;
+        virtual void callbackTX(uint64_t TxTime) = 0; 
+        
+        //Frametype of the form 1001XXXX. First 4 Bits for uniqueness
+        enum FrameType{
+            PING = 0x91,
+            ANCHOR_RESPONSE = 0x92,
+            BEACON_RESPONSE = 0x93,
+            TRANSFER_FRAME = 0x94,
+            DISTANCES_FRAME = 0x95,
+            STREAM_TO_BASE = 0x96,
+            BASE_ORDER = 0x97,
+            SWITCH_TYPE = 0x98,  
+            BASE_ORDER_ACK = 0x99         
+        };              
+        
+    protected:
+        DecaWave& dw;    
+        uint8_t address; 
+        bool Anchor;                 
+};
+
+class BaseStationNode : public Node {
+    public:
+        BaseStationNode(DecaWave& DW);
+    
+        virtual uint8* getRecFrameRef();         
+        virtual uint16 getRecFrameLength();
+            
+        virtual void callbackRX(uint64_t RxTime);
+        virtual void callbackTX(uint64_t TxTime);  
+        
+        void sendOrder(uint8_t beacon_destination, uint8_t anchor_destination, uint8_t action, uint16_t repetitions, uint8_t type);
+        
+    private:     
+        Timer LocalTimer;   
+        bool ack;    
+        RangingFrame rangingFrame;                  // buffer in class for sending a frame (not made locally because then we can recall in the interrupt what was sent)
+    public:    
+        StreamFrame receivedStreamFrame; 
+};
+
+class AnchorNode : public Node {
+    public:    
+        AnchorNode(DecaWave& DW);
+        
+        virtual uint8* getRecFrameRef();
+        virtual uint16 getRecFrameLength();  
+         
+        virtual void callbackRX(uint64_t RxTime);
+        virtual void callbackTX(uint64_t TxTime);    
+         
+    private:
+        uint64_t receiverTimestamps[ADRESSES_COUNT][3];  
+
+        void sendAnswer(uint8_t destination, uint8_t type);
+        void sendTransferFrame(uint8_t destination, int timestamp); 
+        void sendBaseAck();   
+
+        void correctReceiverTimestamps(uint8_t source);
+
+        RangingFrame rangingFrame;                  // buffer in class for sending a frame (not made locally because then we can recall in the interrupt what was sent)
+        
+    public:    
+        ExtendedRangingFrame receivedFrame;   
+};
+
+
+
+class BeaconNode : public Node {
+    public:
+        BeaconNode(DecaWave& DW);
+    
+        virtual void requestRanging(uint8_t destination);
+        virtual void requestRangingAll();
+     
+        float getDistance(uint8_t index);  
+        float getSignalStrength(uint8_t index);         
+        int getMode();
+                  
+        virtual uint8* getRecFrameRef();
+        virtual uint16 getRecFrameLength();
+        
+        virtual void callbackRX(uint64_t RxTime);
+        virtual void callbackTX(uint64_t TxTime); 
+        
+        uint16_t getRepetitions();
+        void decreaseRepetitions();
+        uint8_t getDestination();
+        void clearRec();
+        
+         
+    private:  
+        Timer LocalTimer;
+        float distances[ADRESSES_COUNT];          // Array containing the finally calculated Distances to the anchors 
+        float signalStrength[ADRESSES_COUNT];          // Array containing the finally calculated Distances to the anchors 
+        uint64_t senderTimestamps[ADRESSES_COUNT][3];
+        volatile bool acknowledgement[3];                                // flag to indicate if ranging has started (0) and succeeded (1)
+        int32_t tofs[ADRESSES_COUNT];                          // Array containing time of flights for each node (index is address of node)
+        int8_t noRec[ADRESSES_COUNT];
+    
+        void sendPingFrame(uint8_t destination);
+        void sendAnswer(uint8_t destination, uint8_t type);
+        void sendStreamFrame(uint8_t anchor_addr);
+        void sendBaseAck();
+        
+        inline float calibratedDistance(uint8_t destination);
+        void correctSenderTimestamps(uint8_t source);
+       
+        RangingFrame rangingFrame;                  // buffer in class for sending a frame (not made locally because then we can recall in the interrupt what was sent)
+    
+        uint8_t mode;      
+        uint8_t destination;   
+        uint16_t repetitions;    
+    
+    public:    
+        ExtendedRangingFrame receivedFrame;    
+    
+    
+};
+
+#endif
+