Alejandro Ungria Hirte
/
GA-Test
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Diff: DecaWave/DecaWave.cpp
- Revision:
- 0:3333b6066adf
diff -r 000000000000 -r 3333b6066adf DecaWave/DecaWave.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/DecaWave/DecaWave.cpp Wed Dec 06 21:42:54 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) ;
+}
+
+