Diff: MMRanging/MMRanging.cpp

Revision:

39:bb57aa77b015

Parent:

38:8ef3b8d8b908

Child:

40:5ce51b7e3118

--- a/MMRanging/MMRanging.cpp	Sun Feb 22 11:41:18 2015 +0000
+++ b/MMRanging/MMRanging.cpp	Sun Feb 22 17:40:38 2015 +0000
@@ -8,8 +8,6 @@
 }
 
 void MMRanging::callbackRX() {
-    RX_timestamp = dw.getRXTimestamp();
-    
     rangingframe RX;
     dw.readRegister(DW1000_RX_BUFFER, 0, (uint8_t*)&RX, dw.getFramelength());  // get data from buffer
     
@@ -25,28 +23,26 @@
                 counter++;
                 break;
             case 3:
-                sendRangingframe(RX.source, RX.sequence_number, 4, rangingtimingsReceiver[RX.source][1] - rangingtimingsReceiver[RX.source][0]);
+                //sendRangingframe(RX.source, RX.sequence_number, 4, rangingtimingsReceiver[RX.source][1] - rangingtimingsReceiver[RX.source][0]);
+                sendRangingframe(RX.source, RX.sequence_number, 4, timeDifference40Bit(rangingtimingsReceiver[RX.source][0], rangingtimingsReceiver[RX.source][1]));
                 break;
             case 4:
-                tofs[RX.source] = (rangingtimingsSender[RX.source][1] - rangingtimingsSender[RX.source][0] - RX.time_difference_receiver);
+                //tofs[RX.source] = (rangingtimingsSender[RX.source][1] - rangingtimingsSender[RX.source][0] - RX.time_difference_receiver);
+                tofs[RX.source] = timeDifference40Bit(rangingtimingsSender[RX.source][0], rangingtimingsSender[RX.source][1]) - 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 / %d %d", RX.source, RX.destination, RX.sequence_number, RX.type);
+    sprintf(event[event_i], "!R %d>%d / %d %d", RX.source, RX.destination, RX.sequence_number, RX.type);
     if (event_i == 8)
         event_i = 0;
     else
-        event_i++;*/
+        event_i++;
     
     dw.startRX();
 }
 
 void MMRanging::callbackTX() {
-    TX_timestamp = dw.getTXTimestamp();
     switch (TX.type) {
         case 1:
             rangingtimingsSender[TX.destination][0] = dw.getTXTimestamp();
@@ -57,14 +53,11 @@
         default: break;
     }
     
-    /*event[event_i][0] = '!';
-    event[event_i][1] = 'S';
-    event[event_i][2] = ' ';
-    sprintf(&event[event_i][3], "%d>%d / %d %d", TX.source, TX.destination, TX.sequence_number, TX.type);
+    sprintf(event[event_i], "!S %d>%d / %d %d", TX.source, TX.destination, TX.sequence_number, TX.type);
     if (event_i == 8)
         event_i = 0;
     else
-        event_i++;*/
+        event_i++;
 }
 
 void MMRanging::requestRanging(uint8_t destination) {
@@ -78,4 +71,13 @@
     TX.type = type;
     TX.time_difference_receiver = time_difference_receiver;
     dw.sendFrame((uint8_t*)&TX, sizeof(TX));
+}
+
+uint64_t MMRanging::timeDifference40Bit(uint64_t early, uint64_t late) {
+    int64_t difference = late - early;
+    if ((difference < -MMRANGING_2POWER40+10000000000) && (difference > -MMRANGING_2POWER40-10000000000)) // if the timestamps differ a negative word length +- ~1sec that was potentially measured, correct it
+        return difference + MMRANGING_2POWER40;
+    if ((difference < 0) || (difference > 10000000000))
+        return 10000000000;
+    return (uint64_t)difference;
 }
\ No newline at end of file