Huseyin Berkay Berabi / GA-Test_copy

Dependencies:   mbed-dev

Fork of GA-Test_copy by Alejandro Ungria Hirte

DecaWave/DecaWave.cpp

Committer:
aungriah
Date:
2017-12-06
Revision:
0:a3b83d366423

File content as of revision 0:a3b83d366423:

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