Diff: MM2WayRanging/MM2WayRanging.cpp

Revision:

0:99928431bb44

diff -r 000000000000 -r 99928431bb44 MM2WayRanging/MM2WayRanging.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MM2WayRanging/MM2WayRanging.cpp	Tue Jul 07 09:36:12 2015 +0000
@@ -0,0 +1,178 @@
+#include "MM2WayRanging.h"
+
+MM2WayRanging::MM2WayRanging(DW1000& DW) : dw(DW) {
+    overflow = false;
+    address = 0;
+    rxTimestamp = 0;
+    timediffRec = 0;
+    timediffSend = 0;
+    for (int i = 0; i < 10; i++)
+        acknowledgement[i] = true;
+
+    dw.setCallbacks(this, &MM2WayRanging::callbackRX, &MM2WayRanging::callbackTX);
+
+    LocalTimer.start();
+
+    dw.startRX();
+}
+
+void MM2WayRanging::callbackRX() {
+    dw.readRegister(DW1000_RX_BUFFER, 0, (uint8_t*)&receivedFrame, dw.getFramelength());
+
+    if (receivedFrame.destination == address)
+        switch (receivedFrame.type) {
+            case PING:
+                rxTimestamp = dw.getRXTimestamp();
+                receiverTimestamps[receivedFrame.source][0] = rxTimestamp;      //Save the first timestamp on the receiving node/anchor (T_rp)
+                sendDelayedAnswer(receivedFrame.source, ANCHOR_RESPONSE, rxTimestamp);
+                break;
+            case ANCHOR_RESPONSE:
+                rxTimestamp = dw.getRXTimestamp();
+                senderTimestamps[receivedFrame.source][1] = rxTimestamp;        //Save the second timestamp on the sending node/beacon (T_rr)
+                sendDelayedAnswer(receivedFrame.source, 3, rxTimestamp);
+                break;
+            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 = - 2*receiverTimestamps[receivedFrame.source][1] + receiverTimestamps[receivedFrame.source][0] + receiverTimestamps[receivedFrame.source][2];
+                sendTransferFrame(receivedFrame.source, timediffRec );
+                break;
+            case TRANSFER_FRAME:
+                //calculation of the summand on the sending node/beacon
+                timediffSend = 2 * senderTimestamps[receivedFrame.source][1] - senderTimestamps[receivedFrame.source][0] - senderTimestamps[receivedFrame.source][2];
+                //calculation of the resulting sum of all four ToFs.
+                tofs[receivedFrame.source] = receivedFrame.signedTime + timediffSend;
+                acknowledgement[receivedFrame.source] = true;
+                break;
+            default : break;
+        }
+
+    dw.startRX();
+}
+
+void MM2WayRanging::callbackTX() {
+    switch (rangingFrame.type) {
+    case PING:
+        senderTimestamps[rangingFrame.destination][0] = dw.getTXTimestamp();    //Save the first timestamp on the sending node/beacon (T_sp)
+        break;
+    case ANCHOR_RESPONSE:
+        receiverTimestamps[rangingFrame.destination][1] = dw.getTXTimestamp();  //Save the second timestamp on the receiving node/anchor (T_sr)
+        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;
+    }
+
+}
+
+/**
+ *  Get the distance to the Anchor with address @param destination.
+ *
+ *   @param destination The address of the anchor
+ */
+void MM2WayRanging::requestRanging(uint8_t destination) {
+    acknowledgement[destination] = false;
+    float time_before = LocalTimer.read();
+
+    sendPingFrame(destination);
+
+    // TODO: Here 0.02 seconds is hardcoded as a timeout. Should be added as an argument.
+    while(!acknowledgement[destination] && (LocalTimer.read() < time_before + 0.02f)); // wait for succeeding ranging or timeout
+
+    roundtriptimes[destination] = LocalTimer.read() - time_before;
+
+    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);
+
+
+
+ // Calibration for Nucleo 0 (and 1)
+
+ //   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;
+//        }
+
+    return rawDistance;
+
+}
+
+void MM2WayRanging::requestRangingAll() {
+    for (int i = 1; i <= 4; i++) {  // Request ranging to all anchors
+        requestRanging(i);
+    }
+}
+
+void MM2WayRanging::sendPingFrame(uint8_t destination) {
+    rangingFrame.source = address;
+    rangingFrame.destination = destination;
+    rangingFrame.type = PING;
+    dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame));
+}
+
+void MM2WayRanging::sendTransferFrame(uint8_t destination, int timeDiffsReceiver) {
+    transferFrame.source = address;
+    transferFrame.destination = destination;
+    transferFrame.type = TRANSFER_FRAME;
+    transferFrame.signedTime =  timeDiffsReceiver;                      //cast the time difference
+    dw.sendFrame((uint8_t*)&transferFrame, sizeof(transferFrame));
+}
+
+void MM2WayRanging::sendDelayedAnswer(uint8_t destination, uint8_t type, uint64_t rxTimestamp) {
+
+    rangingFrame.source = address;
+    rangingFrame.destination = destination;
+    rangingFrame.type = type;
+
+    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;
+
+}