Diff: DW1000.cpp

Revision:

9:326bf149c8bc

Parent:

8:0b408e77b701

Child:

10:f1e3c04080d6

Child:

11:b4ea3e12f15e

diff -r 0b408e77b701 -r 326bf149c8bc DW1000.cpp
--- 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);