Diff: main.cpp

Revision:

44:2e0045042a59

Parent:

43:d89fe237a684

Child:

45:01a33363bc21

--- a/main.cpp	Thu Mar 05 10:26:51 2015 +0000
+++ b/main.cpp	Thu Mar 05 12:18:37 2015 +0000
@@ -2,118 +2,45 @@
 #include "mbed.h"
 #include "PC.h"                                     // Serial Port via USB for debugging with Terminal
 #include "DW1000.h"                                 // our DW1000 device driver
-#include "MMRanging.h"                              // our self developed raning application
+#include "MM2WayRanging.h"                          // our self developed ranging application
+
+#define myprintf    pc.printf                       // to make the code adaptable to other outputs that support printf
+
+PC              pc(USBTX, USBRX, 921600);           // USB UART Terminal
+DW1000          dw(PA_7, PA_6, PA_5, PB_6, PB_9);   // Device driver instanceSPI pins: (MOSI, MISO, SCLK, CS, IRQ)
+MM2WayRanging   node(dw);                           // Instance of the two way ranging algorithm
 
-PC          pc(USBTX, USBRX, 921600);               // USB UART Terminal
-DW1000      dw(PA_7, PA_6, PA_5, PB_6, PB_9);       // Device driver instanceSPI pins: (MOSI, MISO, SCLK, CS, IRQ)
-MMRanging   r(dw);                                  // Ranging class for getting distances and later positions
-
-int average_index = 0;
+void rangeAndDisplayAll(){
+    node.requestRangingAll();                       // Request ranging to all anchors
+    for (int i = 1; i <= 4; i++) {                  // Output Results
+        myprintf("D: %f, ", node.distances[i]);
+        myprintf("T:%f", node.roundtriptimes[i]);
+        myprintf("\r\n");
+    }
+    myprintf("\r\n\n");
+}
 
 int main() {
-    pc.printf("\r\nDecaWave 1.0   up and running!\r\n");  
+    pc.printf("\r\nDecaWave 1.0   up and running!\r\n");            // Splashscreen
     dw.setEUI(0xFAEDCD01FAEDCD01);                                  // basic methods called to check if we have a working SPI connection
     pc.printf("DEVICE_ID register: 0x%X\r\n", dw.getDeviceID());
     pc.printf("EUI register: %016llX\r\n", dw.getEUI());
     pc.printf("Voltage: %fV\r\n", dw.getVoltage());
-    pc.printf("System Configuration: %llX\r\n", dw.readRegister40(DW1000_SYS_CFG, 0));
-    pc.printf("Size of Rangingframe: %d\r\n", sizeof(r.TX));
     
-    pc.printf("Antenna Delay TX: %d\r\n", dw.readRegister16(DW1000_TX_ANTD, 0));
-    pc.printf("Antenna Delay RX: %d\r\n", dw.readRegister16(DW1000_LDE_CTRL, 0x1804));
-    pc.printf("TX Power: %llX\r\n", dw.readRegister40(DW1000_TX_POWER, 0));
-    //r.receiver = true;
-    if (r.receiver)
-        r.address = 1;
-    else
-        r.address = 0; // sender node has address 0
-    pc.printf("Address: %d\r\n", r.address);
-    //wait(1);
+    node.isAnchor = true;                                           // declare as anchor or beacon
+    if (node.isAnchor) {
+        node.address = 1;
+        myprintf("This node is Anchor node %d \r\n", node.address);
+    } else {
+        node.address = 0;
+        myprintf("This node is a Beacon. ");
+    }
     
     while(1) {
-        if (!r.receiver) {
-            #if 0 // normal operation
-                r.requestRangingAll();
-                for (int i = 1; i <= 4; i++) {  // Request ranging to all anchors
-                    pc.printf("%f, ", r.distances[i]);
-                    //pc.printf("%f s\r\n", r.roundtriptimes[i]);
-                }
-                pc.printf("\r\n");
-            #endif
-            #if 0 // calibration of antennadelay
-                float rangings[1000];
-                int index = 0;
-            
-                r.requestRanging(1);
-                if (r.distances[1] < 100) {
-                    rangings[index] = r.distances[1];
-                    //pc.printf("%f, ", r.distances[1]);
-                    index++;
-                }
-                if (index == 1000) {
-                    for(int i = 0; i < 999; i++)
-                        rangings[999] += rangings[i];
-                    float mean = rangings[999] / 1000;
-                    pc.printf("\r\n\r\nMean: %f Delay: %d\r\n\r\n", mean, setdelay);
-                    wait(2);
-                    if(mean > 5)
-                        setdelay += 10;
-                    else
-                        setdelay--;
-                    if(abs(mean-5) < 0.003f) {
-                        pc.printf("\r\n\r\nDELAY: %d\r\n\r\n", setdelay);
-                        return 0;
-                    }
-                    dw.writeRegister16(DW1000_TX_ANTD, 0, setdelay / 2);
-                    dw.writeRegister16(DW1000_LDE_CTRL, 0x1804, setdelay / 2);
-                    index = 0; 
-                }
-            #endif
-        } else {
-            //pc.printf("%lld\r\n", r.timeDifference40Bit(r.rangingtimingsReceiver[0][0], r.rangingtimingsReceiver[0][1]));  // debuging output to find 40Bit overflows on receiver side
-        }
-        
-        #if 1 // averaging the distance with a ringbuffer
-            float averaged = 0;
-            float average[1000];
-            
-            int i = 1;
-            r.requestRanging(i);
-            average[average_index] = r.distances[i];
-            average_index++;
-            if(average_index == 1000) {
-                average_index = 0;
-                for(int j = 0; j < 1000; j++)
-                    averaged += average[j];
-                averaged /= 1000;
-                pc.printf("Distance: %f\r\n", averaged);
-                pc.printf("Calibrated: %f\r\n", -0.123 *averaged*averaged + 2.564 * averaged - 5.332 );
-            }
-        #endif
-        
-        #if 0 // Output bars on console
-            for(int i = 1; i < 3; i++) {
-                pc.printf("%lld [", r.tofs[i]);
-                int dots = r.tofs[i]*70/1400;
-                if (abs(dots) < 10000)
-                    for(int j = 0; j < dots; j++)
-                        pc.printf("=");
-                pc.printf("]\r\n");
-            }
-        #endif
-        
-        #ifdef EVENTS   // Output interrupt callback events for debugging (defined in MMRanging.h)
-            for(int j = 0; j < 10; j++)
-                if(r.event[j][0] == '!') {
-                    pc.printf("%s\r\n", r.event[j]);
-                    r.event[j][0] = 'X';
-                }
-            r.event_i = 0;
-        #endif
-        //pc.printf("Status: %llX\r\n", dw.getStatus());
-        //pc.printf("TX Control: %llX\r\n", dw.readRegister40(DW1000_TX_FCTRL, 0));
-        //pc.printf("\r\n");
-        wait(0.002);
-        //wait(0.2);
+        if (!node.isAnchor)
+            rangeAndDisplayAll();
+        else
+            myprintf(".\r");
+        wait(0.3);
     }
 }
\ No newline at end of file