DW1000 UWB driver based on work of Matthias Grob & Manuel Stalder - ETH Zürich - 2015
Diff: DW1000.cpp
- Revision:
- 9:326bf149c8bc
- Parent:
- 8:0b408e77b701
- Child:
- 10:f1e3c04080d6
- Child:
- 11:b4ea3e12f15e
--- a/DW1000.cpp Mon Apr 18 16:58:27 2016 +0000 +++ b/DW1000.cpp Wed Apr 20 11:03:41 2016 +0000 @@ -3,33 +3,46 @@ #define SPIRATE_PLL (10*1000*1000) #define SPIRATE_OSC (2*1000*1000) -DW1000::DW1000(UWBMode setup, PinName MOSI, PinName MISO, PinName SCLK, PinName CS, PinName IRQ) : irq(IRQ), spi(MOSI, MISO, SCLK), cs(CS) +DW1000::DW1000(PinName MOSI, PinName MISO, PinName SCLK, PinName CS, PinName IRQ) : irq(IRQ), spi(MOSI, MISO, SCLK), cs(CS) { setCallbacks(NULL, NULL); - DW1000Setup newSetup(DW1000Setup::fastLocationC5); + DW1000Setup newSetup(DW1000Setup::tunedDefault); systemConfig.applyConfig(&newSetup); deselect(); // Chip must be deselected first spi.format(8,0); // Setup the spi for standard 8 bit data and SPI-Mode 0 (GPIO5, GPIO6 open circuit or ground on DW1000) spi.frequency(SPIRATE_PLL); // with a 1MHz clock rate (worked up to 49MHz in our Test) - resetAll(); // we do a soft reset of the DW1000 everytime the driver starts -setupRadio(); - setTxPower(250,250,250+30,250+60); // power = 23dB gain + setupRadio(); - setRxDelay(0); - setTxDelay(0); - - irq.rise(this, &DW1000::ISR); // attach interrupt handler to rising edge of interrupt pin from DW1000 + setRxDelay(0); + setTxDelay(0); } -DW1000Setup* DW1000::getSetup() { +DW1000Setup* DW1000::getSetup() +{ return &systemConfig; +} + +bool DW1000::applySetup(DW1000Setup *setup) +{ + + if (setup->check()) { + systemConfig.applyConfig(setup); + setupRadio(); + spi.frequency(SPIRATE_PLL); // with a 1MHz clock rate (worked up to 49MHz in our Test) + return true; } + return false; +} void DW1000::setupRadio() { + irq.rise(NULL); // attach interrupt handler to rising edge of interrupt pin from DW1000 + + stopTRX(); + resetAll(); // we do a soft reset of the DW1000 to get to a known state. Without this we lose comms. setupAGC(); setupRxConfig(); setupLDE(); @@ -39,7 +52,51 @@ setupTxCalibration(); setupTxFrameCtrl(); setupSystemConfig(); + setupGPIO(); + setupPower(); + irq.rise(this, &DW1000::ISR); // attach interrupt handler to rising edge of interrupt pin from DW1000 +} + + +void DW1000::setupGPIO() +{ +// not done in a loop because bits 7 and 8 are the inverse, a value of 01 indicates GPIO + uint32_t value = 0; + uint32_t pinMask = systemConfig.getGPIO(); + if (pinMask & (0x01<<0)) + value |= 1<<6; + + if (pinMask & (0x01<<1)) + value |= 1<<8; + + if (pinMask & (0x01<<2)) + value |= 1<<10; + + if (pinMask & (0x01<<3)) + value |= 1<<12; + + if (pinMask & (0x01<<4)) + value |= 1<<14; + + if (pinMask & (0x01<<5)) + value |= 1<<16; + + if (pinMask & (0x01<<6)) + value |= 1<<18; + + if (!(pinMask & (0x01<<7))) + value |= 1<<20; + + if (!(pinMask & (0x01<<8))) + value |= 1<<22; + + writeRegister32(DW1000_GPIO_CTRL, 0, value); // set time to 400ms, enable blink and flash all LEDs + + if (pinMask & 0x000f) { // some LEDs are active + writeRegister32(DW1000_PMSC,DWPMSC_PMSC_CTRL0,readRegister32(DW1000_PMSC,DWPMSC_PMSC_CTRL0) | 1<<23 | 1<<18); + writeRegister32(DW1000_PMSC, DWPMSC_PMSC_LEDC, 0x00000120); // set time to 400ms, enable blink and flash all LEDs + } } void DW1000::setupAGC() @@ -278,65 +335,105 @@ } -uint8_t DW1000::powerToRegValue(uint16_t powercB) +uint8_t DW1000::powerToRegValue(float powerdB) { - // course power control - 0 = 18dB, 6 = 0dB in 3dB steps. - uint8_t course = powercB / 30; + uint8_t course = powerdB / 3; if(course > 6) course = 6; // remaining power - powercB -= course * 30; + powerdB -= course * 3; // value in reg is inverse. course = 6-course; // fine control in steps of 0.5dB - uint8_t fine = powercB / 5; + uint8_t fine = powerdB / 0.5f; if (fine > 31) fine = 31; + return (course << 5) | fine; +} - return (course << 5) | fine; + +float DW1000::regToPowerValue(uint8_t powerVal) +{ + + int course = powerVal >> 5; + if (course==7) // off + return 0; + + course = (6-course)*3; + + int fine = (powerVal & 0x1f); + return course + fine/2.0f; +} + + +void DW1000::setupPower() +{ + const float *powerPtr = systemConfig.getTxPowers(); + + uint32_t powerReg = powerToRegValue(*powerPtr); + powerReg |= powerToRegValue(*(powerPtr+1)) << 8; + powerReg |= powerToRegValue(*(powerPtr+2)) << 16; + powerReg |= powerToRegValue(*(powerPtr+3)) << 24; + writeRegister32(DW1000_TX_POWER,0,powerReg); } // transmit power: 0 to 33.5 dB gain in steps of 0.5. Inputs are in 10ths of a dB (0 to 335) -void DW1000::setTxPower(uint16_t normalPowercB, uint16_t boost500, uint16_t boost250, uint16_t boost125) +void DW1000::setTxPower(float normalPowerdB, float boost500, float boost250, float boost125) { + + if(normalPowerdB > 33.5) + normalPowerdB = 33.5; - if(normalPowercB > 335) - normalPowercB = 335; - - if (boost500 < normalPowercB) - boost500 = normalPowercB; - if(boost500 > 335) - boost500 = 335; + if (boost500 < normalPowerdB) + boost500 = normalPowerdB; + if(boost500 > 33.5) + boost500 = 33.5; if (boost250 < boost500) boost250 = boost500; - if(boost250 > 335) - boost250 = 335; + if(boost250 > 33.5) + boost250 = 33.5; if (boost125 < boost250) boost125 = boost250; - if(boost125 > 335) - boost125 = 335; + if(boost125 > 33.5) + boost125 = 33.5; if (systemConfig.getSmartPower() == false) { - boost500 = normalPowercB; - boost250 = normalPowercB; - boost125 = normalPowercB; + boost500 = normalPowerdB; + boost250 = normalPowerdB; + boost125 = normalPowerdB; } - uint32_t powerReg = powerToRegValue(normalPowercB); + uint32_t powerReg = powerToRegValue(normalPowerdB); powerReg |= powerToRegValue(boost500) << 8; powerReg |= powerToRegValue(boost250) << 16; powerReg |= powerToRegValue(boost125) << 24; - writeRegister32(DW1000_TX_POWER, 0, powerReg); + writeRegister32(DW1000_TX_POWER,0,powerReg); + + systemConfig.setSmartTxPower(normalPowerdB,boost500,boost250,boost125); // update the systemConfig +} + +uint32_t DW1000::getTxPower(float *power,float *boost500, float *boost250, float*boost125) +{ + uint32_t value = readRegister32(DW1000_TX_POWER,0); + if (power) + *power = regToPowerValue(value&0x000000ff); + if (boost500) + *boost500 = regToPowerValue((value&0x0000ff00)>>8); + if (boost250) + *boost250 = regToPowerValue((value&0x00ff0000)>>16); + if (boost125) + *boost125 = regToPowerValue((value&0xff000000)>>24); + return value; } void DW1000::setupAnalogRF() @@ -491,6 +588,68 @@ writeRegister32(DW1000_TX_FCTRL,0,frameCtrlValue); } + +#define SQR(x) ((float)(x) * (float)(x)) + +void DW1000::getRxSignalPower(float *direct, float *total) +{ + uint16_t firstPathAmp1 = readRegister16(DW1000_RX_TIME,7); + uint16_t firstPathAmp2 = readRegister16(DW1000_RX_FQUAL,2); + uint16_t firstPathAmp3 = readRegister16(DW1000_RX_FQUAL,4); + uint16_t preambleAcc = readRegister16(DW1000_RX_FINFO,4) >> 4; + uint16_t preambleAccNoSat = readRegister16(DW1000_DRX_CONF,DWDRX_RXPAC_NOSAT); + uint16_t channelImpulse = readRegister16(DW1000_RX_FQUAL,6); + + if (preambleAcc == preambleAccNoSat) { + if (systemConfig.getSfd() == DW1000Setup::standard) { + if (systemConfig.getDataRate() == DW1000Setup::kbps110) + preambleAcc += -64; + else + preambleAcc += -5; + } else { + if (systemConfig.getDataRate() == DW1000Setup::kbps110) + preambleAcc += -82; + else + preambleAcc += -10; + } + } + + + float directPower = 10*log10( (SQR(firstPathAmp1) + SQR(firstPathAmp2) + SQR(firstPathAmp3))/SQR(preambleAcc)); + + float rxSignalPower = 10*log10( ((float)channelImpulse * (1<<17))/SQR(preambleAcc) ); + + if (systemConfig.getPRF() == DW1000Setup::prf16MHz) { + directPower -= 113.77; + rxSignalPower -= 113.77; + } else { + directPower -= 121.74; + rxSignalPower -= 121.74; + } + + *direct = directPower; + *total = rxSignalPower; + +} + + +float DW1000::getRxQuality(uint16_t *sigAmp, uint16_t *noiseAmp) +{ + uint16_t firstPathAmp2 = readRegister16(DW1000_RX_FQUAL,2); + uint16_t noise = readRegister16(DW1000_RX_FQUAL,0); + if (sigAmp) + *sigAmp = firstPathAmp2; + if (noiseAmp) + *noiseAmp = noise; + + if (noise > 0) + return SQR(firstPathAmp2)/(noise*100000.0f); + else + return 100; +} +#undef SQR + + void DW1000::setRxDelay(uint16_t ticks) { writeRegister16(DW1000_LDE_CTRL, DWLDE_LDE_RXANTD, ticks);