Diff: MM2WayRanging/MM2WayRanging.cpp

Revision:

45:01a33363bc21

Parent:

44:2e0045042a59

Child:

46:6398237672a0

--- a/MM2WayRanging/MM2WayRanging.cpp	Thu Mar 05 12:18:37 2015 +0000
+++ b/MM2WayRanging/MM2WayRanging.cpp	Sun Mar 08 15:59:14 2015 +0000
@@ -2,6 +2,7 @@
 
 MM2WayRanging::MM2WayRanging(DW1000& DW) : dw(DW) {
     isAnchor = true;
+    overflow = false;
     address = 0;
     rxTimestamp = 0;
     timediffRec = 0;
@@ -11,7 +12,8 @@
 
     dw.setCallbacks(this, &MM2WayRanging::callbackRX, &MM2WayRanging::callbackTX);
 
-    LocalTimer.start();     // Timer for timout during acknowledgement
+    //Timer for debugging purposes
+    LocalTimer.start();
 
     dw.startRX();
 }
@@ -34,8 +36,10 @@
             case BEACON_RESPONSE:
                 rxTimestamp = dw.getRXTimestamp();
                 receiverTimestamps[receivedFrame.source][2] = rxTimestamp;      //Save the third timestamp on the receiving node/anchor (T_rf)
+
+                correctReceiverTimestamps(receivedFrame.source);                //Correct the timestamps for the case of a counter overflow
                 //calculation of the summand on the receiving node/anchor
-                timediffRec = receiverTimestamps[receivedFrame.source][0] + receiverTimestamps[receivedFrame.source][2] - 2*receiverTimestamps[receivedFrame.source][1];
+                timediffRec = - 2*receiverTimestamps[receivedFrame.source][1] + receiverTimestamps[receivedFrame.source][0] + receiverTimestamps[receivedFrame.source][2];
                 sendTransferFrame(receivedFrame.source, timediffRec );
                 break;
             case TRANSFER_FRAME:
@@ -47,6 +51,7 @@
                 break;
             default : break;
         }
+    
     dw.startRX();
 }
 
@@ -60,10 +65,12 @@
         break;
     case BEACON_RESPONSE:
         senderTimestamps[rangingFrame.destination][2] = dw.getTXTimestamp();    //Save the third timestamp on the sending node/beacon (T_sr)
+        correctSenderTimestamps(rangingFrame.destination);                      //Correct the timestamps for the case of a counter overflow
         break;
     default:
         break;
     }
+
 }
 
 /**
@@ -77,14 +84,39 @@
 
     sendPingFrame(destination);
 
-    while(!acknowledgement[destination] && (LocalTimer.read() < time_before + 0.1f)); // wait for succeeding ranging or timeout
+    while(!acknowledgement[destination] && (LocalTimer.read() < time_before + 0.5f)); // wait for succeeding ranging or timeout
 
     roundtriptimes[destination] = LocalTimer.read() - time_before;
-    distances[destination] = (tofs[destination] * 300 * TIMEUNITS_TO_US / 4);
+
+    if(acknowledgement[destination]){
+    distances[destination] = calibratedDistance(destination);
+    } else {
+        distances[destination] = -1;
+    }
+}
+
+inline float MM2WayRanging::calibratedDistance(uint8_t destination) {
+    
+    float rawDistance = (tofs[destination] * 300 * TIMEUNITS_TO_US / 4);
+    
+    if (this->address == 1) rawDistance+= 10;
+    
+    switch(destination){
+        case 2:
+            return  rawDistance * 0.9754 - 0.5004;              
+        case 3:
+            return  rawDistance * 0.9759 - 0.4103;
+        case 4:
+            return  rawDistance * 0.9798 - 0.5499;
+        case 5:
+            return  rawDistance * 0.9765 - 0.5169;
+        }
+    
+            
 }
 
 void MM2WayRanging::requestRangingAll() {
-    for (int i = 1; i <= 4; i++) {  // Request ranging to all anchors
+    for (int i = 1; i <= 5; i++) {  // Request ranging to all anchors
         requestRanging(i);
     }
 }
@@ -105,8 +137,39 @@
 }
 
 void MM2WayRanging::sendDelayedAnswer(uint8_t destination, uint8_t type, uint64_t rxTimestamp) {
+
     rangingFrame.source = address;
     rangingFrame.destination = destination;
     rangingFrame.type = type;
-    dw.sendDelayedFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), rxTimestamp + ANSWER_DELAY_TIMEUNITS);
-}
\ No newline at end of file
+
+    if(rxTimestamp + ANSWER_DELAY_TIMEUNITS > MMRANGING_2POWER40)
+        dw.sendDelayedFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), rxTimestamp + ANSWER_DELAY_TIMEUNITS - MMRANGING_2POWER40);
+    else
+        dw.sendDelayedFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), rxTimestamp + ANSWER_DELAY_TIMEUNITS);
+}
+
+void MM2WayRanging::correctReceiverTimestamps(uint8_t source){
+
+    if(receiverTimestamps[source][0] > receiverTimestamps[source][1]){
+        receiverTimestamps[source][1] += MMRANGING_2POWER40;
+        receiverTimestamps[source][2] += MMRANGING_2POWER40;
+    }
+
+    if(receiverTimestamps[source][1] > receiverTimestamps[source][2]){
+            receiverTimestamps[source][2] += MMRANGING_2POWER40;
+        }
+
+}
+
+void MM2WayRanging::correctSenderTimestamps(uint8_t source){
+
+    if (senderTimestamps[source][0] > senderTimestamps[source][1]) {
+        senderTimestamps[source][1] += MMRANGING_2POWER40;
+        senderTimestamps[source][2] += MMRANGING_2POWER40;
+        overflow = true;
+    } else if (senderTimestamps[source][1] > senderTimestamps[source][2]) {
+        senderTimestamps[source][2] += MMRANGING_2POWER40;
+        overflow = true;
+    }else overflow = false;
+
+}