Diff: MMRanging/MMRanging.cpp

Revision:

36:883de6f9a73b

Parent:

35:95d838d33bf5

Child:

37:40f94c634c3e

--- a/MMRanging/MMRanging.cpp	Tue Feb 17 10:58:58 2015 +0000
+++ b/MMRanging/MMRanging.cpp	Fri Feb 20 09:07:52 2015 +0000
@@ -12,30 +12,31 @@
     
     rangingframe RX;
     dw.readRegister(DW1000_RX_BUFFER, 0, (uint8_t*)&RX, dw.getFramelength());  // get data from buffer
-    uint8_t Index = RX.sequence_number % 10;
-
-    switch (RX.type) {
-        case 1:
-            rangingtimingsReceiver[Index][0] = dw.getRXTimestamp();
-            sendRangingframe(2, RX.sequence_number, 0);
-            break;
-        case 2:
-            rangingtimingsSender[Index][1] = dw.getRXTimestamp();
-            break;
-        case 3:
-            sendRangingframe(4, TX.sequence_number, rangingtimingsReceiver[Index][1] - rangingtimingsReceiver[Index][0]); // TODO: sequence number???
-            break;
-        case 4:
-            tofs = (rangingtimingsSender[Index][1] - rangingtimingsSender[Index][0] - RX.time_difference_receiver);// - 65536;
-            break;
-        default : break;
-    }
     
-    eventtimes[event_i] = RX_timestamp - TX_timestamp;                      // TODO: can give some wrong values because of timer reset after 17 seconds
+    if (RX.destination == address)                                                  // only if received packet is for me
+        switch (RX.type) {
+            case 1:
+                rangingtimingsReceiver[RX.source][0] = dw.getRXTimestamp();
+                sendRangingframe(RX.source, RX.sequence_number, 2, 0);
+                break;
+            case 2:
+                rangingtimingsSender[RX.source][1] = dw.getRXTimestamp();
+                sendRangingframe(RX.source, counter, 3, 0);
+                counter++;
+                break;
+            case 3:
+                sendRangingframe(RX.source, RX.sequence_number, 4, rangingtimingsReceiver[RX.source][1] - rangingtimingsReceiver[RX.source][0]);
+                break;
+            case 4:
+                tofs = (rangingtimingsSender[RX.source][1] - rangingtimingsSender[RX.source][0] - RX.time_difference_receiver);
+                break;
+            default : break;
+        }
+    
     event[event_i][0] = '!';
     event[event_i][1] = 'R';
     event[event_i][2] = ' ';
-    sprintf(&event[event_i][3], "%d %d", RX.sequence_number, RX.type);
+    sprintf(&event[event_i][3], "%d>%d / %d %d", RX.source, RX.destination, RX.sequence_number, RX.type);
     if (event_i == 8)
         event_i = 0;
     else
@@ -46,40 +47,35 @@
 
 void MMRanging::callbackTX() {
     TX_timestamp = dw.getTXTimestamp();
-    uint8_t Index = TX.sequence_number % 10;
     switch (TX.type) {
         case 1:
-            rangingtimingsSender[Index][0] = dw.getTXTimestamp();
+            rangingtimingsSender[TX.destination][0] = dw.getTXTimestamp();
             break;
         case 2:
-            rangingtimingsReceiver[Index][1] = dw.getTXTimestamp();
+            rangingtimingsReceiver[TX.destination][1] = dw.getTXTimestamp();
             break;
         default: break;
     }
     
-    eventtimes[event_i] = 0;
     event[event_i][0] = '!';
     event[event_i][1] = 'S';
     event[event_i][2] = ' ';
-    sprintf(&event[event_i][3], "%d %d", TX.sequence_number, TX.type);
+    sprintf(&event[event_i][3], "%d>%d / %d %d", TX.source, TX.destination, TX.sequence_number, TX.type);
     if (event_i == 8)
         event_i = 0;
     else
         event_i++;
 }
 
-void MMRanging::requestRanging() {
-    sendRangingframe(1, counter, 0);
+void MMRanging::requestRanging(uint8_t destination) {
+    sendRangingframe(destination, counter, 1, 0);
 }
 
-void MMRanging::requestTime() {
-    sendRangingframe(3, counter, 0);
-    counter++;
-}
-
-void MMRanging::sendRangingframe(uint8_t type, uint8_t sequence_number, uint64_t time_difference_receiver) {
+void MMRanging::sendRangingframe(uint8_t destination, uint8_t sequence_number, uint8_t type, uint64_t time_difference_receiver) {
+    TX.source = address;
+    TX.destination = destination;
+    TX.sequence_number = sequence_number;
     TX.type = type;
-    TX.sequence_number = sequence_number;
     TX.time_difference_receiver = time_difference_receiver;
     dw.sendFrame((uint8_t*)&TX, sizeof(TX));
 }
\ No newline at end of file