Revision 2:e28f4414ca2c, committed 2016-08-19

Comitter:

Kekehoho

Date:

Fri Aug 19 17:38:43 2016 +0000

Parent:

1:4523d7cda75e

Commit message:

k

Changed in this revision

--- a/DW1000/DW1000.cpp	Fri Jun 17 20:35:28 2016 +0000
+++ b/DW1000/DW1000.cpp	Fri Aug 19 17:38:43 2016 +0000
@@ -1,4 +1,11 @@
 #include "DW1000.h"
+#include "math.h"
+#include "stdio.h"
+#include "stdlib.h"
+
+
+#define LAST(k, n) ((k)&((1<<(n))-1))
+#define MID(k,m,n) LAST((k)>>(m),((n)-(m)))
 
 DW1000::DW1000(PinName MOSI, PinName MISO, PinName SCLK, PinName CS, PinName IRQ) : irq(IRQ), spi(MOSI, MISO, SCLK), cs(CS) {
     setCallbacks(NULL, NULL);
@@ -97,9 +104,36 @@
     return result;
     }
     
+uint32_t DW1000::getFP_AMPL1(){
+    uint32_t result;
+    uint32_t result1;
+    uint32_t result2;
+    uint32_t result3;
+    uint32_t result4;
+    readRegister(DW1000_RX_TIME, 4,(uint8_t*)&result, 4);
+    readRegister(DW1000_RX_TIME, 8,(uint8_t*)&result1, 4);
+    result2 = MID(result, 24,32);
+    result3 = MID(result1, 0, 8);
+    result4 = (result3<<8)|result2; 
+    return result4;
+    }
+    
 uint32_t DW1000::getRXPACC() {
     uint32_t result;
-    readRegister(DW1000_RX_FINFO, 0, (uint8_t*)&result, 8);
+    readRegister(DW1000_RX_FINFO, 0, (uint8_t*)&result, 4);
+    return result;    
+    }
+
+uint32_t DW1000::getSignalStrength() {
+    uint64_t C;
+    readRegister(DW1000_RX_FQUAL, 0,(uint8_t*)&C, 8);
+    uint32_t N;
+    readRegister(DW1000_RX_FINFO, 0, (uint8_t*)&N, 4);
+    
+    float Param;
+    
+    Param = MID(C,48,64)*131072/(MID(N,20,32)*MID(N,20,32));
+    uint32_t result = 10*log( (float)Param) - 113.77;
     return result;    
     }
     

--- a/DW1000/DW1000.h	Fri Jun 17 20:35:28 2016 +0000
+++ b/DW1000/DW1000.h	Fri Aug 19 17:38:43 2016 +0000
@@ -67,7 +67,10 @@
         uint64_t getEUI();                                                                      // gets 64 bit Extended Unique Identifier according to IEEE standard
         void setEUI(uint64_t EUI);  
         uint64_t getCIR_PWR();
-        uint32_t getRXPACC();                                                            // sets 64 bit Extended Unique Identifier according to IEEE standard
+        uint64_t getCIR_PWR2();
+        uint32_t getRXPACC();
+        uint32_t getFP_AMPL1();
+        uint32_t getSignalStrength();                                                            // sets 64 bit Extended Unique Identifier according to IEEE standard
         float getVoltage();                                                                     // gets the current chip voltage measurement form the A/D converter
         uint64_t getStatus();                                                                   // get the 40 bit device status
         uint64_t getRXTimestamp();

--- a/MM2WayRanging/MM2WayRanging.cpp	Fri Jun 17 20:35:28 2016 +0000
+++ b/MM2WayRanging/MM2WayRanging.cpp	Fri Aug 19 17:38:43 2016 +0000
@@ -64,7 +64,7 @@
 
 float MM2WayRanging::rangeAndDisplayAll(){
     requestRangingAll();                       // Request ranging to all anchors
-    return distances[2], distances[3], distances[4];
+    return 0.1;
 }
 
 void MM2WayRanging::callbackTX() {

--- a/main.cpp	Fri Jun 17 20:35:28 2016 +0000
+++ b/main.cpp	Fri Aug 19 17:38:43 2016 +0000
@@ -50,7 +50,7 @@
     BMP180 bmp180(p28, p27);                    // sda, scl
     
     // DWM1000
-    DW1000          dw(p5, p6, p7, p11, p17);   // Device driver instanceSPI pins: (MOSI, MISO, SCLK, CS, IRQ)
+    DW1000          dw(p5, p6, p7, p11, p15);   // Device driver instanceSPI pins: (MOSI, MISO, SCLK, CS, IRQ)
     MM2WayRanging   node(dw);                   // Ranging algorithm
     
     //USB 
@@ -85,26 +85,17 @@
   // Connection Test of the DecaWave Ranging Measurement Unit 
     
     dw.setEUI(0xFAEDCD01FAEDCD01);                                  // basic methods called to check if we have a working SPI connection
-    if (!(dw.getEUI() == 0xFAEDCD01FAEDCD01 && dw.getDeviceID() == 0xDECA0130))
-    { myprintf("DWM1000 Identification error:\n DeviceID = %d\n EUI = %d", dw.getDeviceID(), dw.getEUI());
+    if (!(dw.getEUI() == 0xFAEDCD01FAEDCD01 && dw.getDeviceID() == 0xDECA0130)) //&& dw2.getEUI() == 0xFAEDCD01FAEDCD01 && dw2.getDeviceID() == 0xDECA0130))
+    { myprintf("\n\rDWM1000 Identification error:\n DeviceID1 = %X\n EUI1 = %016llX", dw.getDeviceID(), dw.getEUI()); //dw2.getDeviceID(), dw2.getEUI());
     while(1);}
     else 
     {myprintf("DWM100 Connection established\n\r");}
-    
+
     // Anchor or Beacon?
-myprintf("Set the Chip as an Anchor: Up\n\r Set the Chip as a Beacon: Down\n\r");   
-while (joystick.read() != 1 && joystick.read() != 2){wait(0.001);}
-
-if (joystick.read() == 1) {
-        node.isAnchor = true;
-        node.address = 2;
-        myprintf("This node is Anchor node %d \r\n", node.address);
-        while(1);
-    } else {
+   
         node.isAnchor = false;
-        node.address = 0;
-        myprintf("This node is a Beacon.\n\r ");
-        }
+        node.address = 1;
+        myprintf("This node is Beacon1      address: %d \r\n", node.address);
         
   // Read the WHO_AM_I register, this is a good test of communication
   uint8_t whoami = MPU9150.readByte(MPU9150_ADDRESS, WHO_AM_I_MPU9150);  // Read WHO_AM_I register for MPU-9250
@@ -142,15 +133,33 @@
     magbias[0] = -5.;   // User environmental x-axis correction in milliGauss
     magbias[1] = -95.;  // User environmental y-axis correction in milliGauss
     magbias[2] = -260.; // User environmental z-axis correction in milliGauss
+
 FILE *fic=  fopen("/fs/test.txt","w");
-int button = 0; 
-float Start; 
-float pressure, temperature;
-float Distance[3] = {7,8,9};
+int button = 0;
+int a = -2; 
+int First_Start = 1;
+float Start;
+float tnodetoanchor2, tnodetoanchor3, tnodetoanchor4, tnode2toanchor4;
+
 int Points = 0;
  while(joystick.read() != 4) {
   // Get Ranging measurements 
-  (Distance[0], Distance[1], Distance[2]) = node.rangeAndDisplayAll();
+  node.requestRanging(2);
+  tnodetoanchor2 = t.read();  
+  
+  node.requestRanging(3);
+  tnodetoanchor3 = t.read();  
+
+  node.requestRanging(4);
+  tnodetoanchor4 = t.read();  
+  
+  //node2.requestRanging(2);
+  //tnode2toanchor2 = t.read();
+  
+  //node2.requestRanging(4);
+  //tnode2toanchor4 = t.read();
+  
+   float temps;
   
   // If intPin goes high, all data registers have new data
   if(MPU9150.readByte(MPU9150_ADDRESS, INT_STATUS) & 0x01) {  // On interrupt, check if data ready interrupt
@@ -160,6 +169,7 @@
     ax = (float)accelCount[0]*aRes;  - accelBias[0];  // get actual g value, this depends on scale being set
     ay = (float)accelCount[1]*aRes;  - accelBias[1];   
     az = (float)accelCount[2]*aRes;  - accelBias[2];  
+   temps = t.read();
    
     MPU9150.readGyroData(gyroCount);  // Read the x/y/z adc values
     // Calculate the gyro value into actual degrees per second
@@ -168,7 +178,7 @@
     gz = (float)gyroCount[2]*gRes; // - gyroBias[2];   
   
     mcount++;
-    if (mcount > 200/MagRate) {  // this is a poor man's way of setting the magnetometer read rate (see below) 
+    //if (mcount > 200/MagRate) {  // this is a poor man's way of setting the magnetometer read rate (see below) 
     MPU9150.readMagData(magCount);  // Read the x/y/z adc values
     // Calculate the magnetometer values in milliGauss
     // Include factory calibration per data sheet and user environmental corrections
@@ -176,16 +186,16 @@
     my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];  
     mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];   
     mcount = 0;
-    }
+    //}
   }
   
    
-    //Now = t.read_us();
-    //deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
-    //lastUpdate = Now;
+    Now = t.read_us();
+    deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
+    lastUpdate = Now;
     
-    //sum += deltat;
-    //sumCount++;
+    sum += deltat;
+    sumCount++;
     
 //    if(lastUpdate - firstUpdate > 10000000.0f) {
 //     beta = 0.04;  // decrease filter gain after stabilized
@@ -193,7 +203,7 @@
  //   }
     
    // Pass gyro rate as rad/s
-//  MPU9150.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f,  my,  mx, mz);
+  //MPU9150.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f,  my,  mx, mz);
   //MPU9150.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
 
     // Serial print and/or display at 0.5 s rate independent of data rates
@@ -206,60 +216,41 @@
     
     delt_t = t.read_ms() - count;
     timer.start();
-        while (fire)
+        if (fire)
         { Start = timer.read();
-        myprintf("#");
-          fprintf(fic, "#\n");
-          wait(0.3);}
+          myprintf("#");
+          fprintf(fic, "%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d\n", a, a, a, a, a, a, a, a, a, a, a, a, a);
+          wait(0.4);}
         
-        if (Start>0.1)
-        { button = !button;
-          Points = 0;}
+        if (Start>0.01)
+        { button = 1;
+          Points = 0;
+          myprintf("\n\rNouvelle Aquisition");
+          wait(1);}
         
-        if (fic != NULL && button && Points < 501)
+        if (fic != NULL && button && Points < 2000)//&& bmp180.ReadData(&temperature, &pressure)
         {
-         //fprintf(fic, "%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f\n", t.read(),1000*ax, 1000*ay, 1000*az, gx, gy, gz, mx, my, mz, pressure, temperature, Distance);
-         myprintf("%d", Points);
-         fprintf(fic, "%.2f %.2f %.2f %u %u\n", Distance[0], Distance[1], Distance[2], MID(dw.getRXPACC(), 20,32), MID(dw.getCIR_PWR(),48,64));
-         myprintf("        Distance 2 = %.2f  Distance 3 = %.2f  Distance 4 = %.2f     %u %u\n\r", node.distances[2], node.distances[3], node.distances[4], MID(dw.getRXPACC(), 20,32), MID(dw.getCIR_PWR(),48,64));
+         fprintf(fic, "%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f\n",temps, 1000*ax, 1000*ay, 1000*az, gx, gy, gz,tnodetoanchor2, node.distances[2],tnodetoanchor3, node.distances[3],tnodetoanchor4,node.distances[4],mx,my,mz);
+         myprintf("%f       %f    %f   \n\r",node.distances[2], node.distances[3], node.distances[4]);// Points, node.distances[2],node.distances[3],node.distances[4],1000*ax, 1000*ay, 1000*az );
+         //fprintf(fic, ";%u;%u;%u", MID(dw.getRXPACC(), 20,32),dw.getFP_AMPL1(), MID(dw.getCIR_PWR(), 16, 32));
+         //fprintf(fic, ";%u;%.2f", MID(dw.getCIR_PWR(), 32, 48), node.distances[2]);
          Start = 0;
-         wait(0.0005);
          Points ++;} 
-        else 
-            //{if (fic != NULL && button)
-            //{fprintf(fic, "%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;%.2f;No data;No data\n",t.read(),1000*ax, 1000*ay, 1000*az, gx, gy, gz, mx, my, mz);
-            //    Start = 0;}
-            //    else {             
-              {          
-                        myprintf(".");
-                        Start = 0;}
+        else              
+              {myprintf(".");
+               Start = 0;}
 
     if (delt_t > 500) { // update LCD once per half-second independent of read rate        
-
-        
-
-    //lcd.locate(0,0);
-    //lcd.printf("ax = %.3f", 1000*ax); 
-    //myprintf(" ay = %f", 1000*ay); 
-    //pc.printf(" az = %f  mg\n\r", 1000*az); 
-    //lcd.locate(0,9);
-    //lcd.printf("gx = %f", gx); 
-   // myprintf(" gy = %f", gy); 
-    //pc.printf(" gz = %f  deg/s\n\r", gz); 
-    //lcd.locate(0,18);
-    //lcd.printf("gx = %f", mx); 
-    //myprintf(" gy = %f", my); 
-    //pc.printf(" gz = %f  mG\n\r", mz); 
     
     tempCount = MPU9150.readTempData();  // Read the adc values
     temperature = ((float) tempCount) / 340.0f + 36.53f; // Temperature in degrees Centigrade
     //myprintf(" temperature = %f  C", temperature); 
     //myprintf("Ranging = %f\n\r ", Distance);
     
-    //pc.printf("q0 = %f\n\r", q[0]);
-    //pc.printf("q1 = %f\n\r", q[1]);
-    //pc.printf("q2 = %f\n\r", q[2]);
-    //pc.printf("q3 = %f\n\r", q[3]);      
+    //myprintf("q0 = %f\n\r", q[0]);
+    //myprintf("q1 = %f\n\r", q[1]);
+    //myprintf("q2 = %f\n\r", q[2]);
+    //myprintf("q3 = %f\n\r", q[3]);      
     
 /*    lcd.clear();
     lcd.printString("MPU9150", 0, 0);
@@ -288,8 +279,8 @@
     //yaw   -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
     //roll  *= 180.0f / PI;
 
-    //pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
-    //pc.printf("average rate = %f\n\r", (float) sumCount/sum);
+    //myprintf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
+    //myprintf("average rate = %f\n\r", (float) sumCount/sum);
 //    sprintf(buffer, "YPR: %f %f %f", yaw, pitch, roll);
 //    lcd.printString(buffer, 0, 4);
 //    sprintf(buffer, "rate = %f", (float) sumCount/sum);
@@ -303,11 +294,13 @@
         count = 0;
         deltat= 0;
         lastUpdate = t.read_us();
+        
         }
-    //sum = 0;
-    //sumCount = 0; *
+    sum = 0;
+    sumCount = 0; 
 }
 }
+fprintf(fic, "%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d;%d", a, a, a, a, a, a, a, a, a, a, a, a, a);
 fclose(fic);
  
  }
\ No newline at end of file