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.
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) ; }