Diff: DecaWave/DecaWave.cpp

Revision:

0:a3b83d366423

--- /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) ;
+}
+
+