Diff: MM2WayRanging/MM2WayRanging.cpp

Revision:

44:2e0045042a59

Child:

45:01a33363bc21

diff -r d89fe237a684 -r 2e0045042a59 MM2WayRanging/MM2WayRanging.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MM2WayRanging/MM2WayRanging.cpp	Thu Mar 05 12:18:37 2015 +0000
@@ -0,0 +1,112 @@
+#include "MM2WayRanging.h"
+
+MM2WayRanging::MM2WayRanging(DW1000& DW) : dw(DW) {
+    isAnchor = true;
+    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();     // Timer for timout during acknowledgement
+
+    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)
+                //calculation of the summand on the receiving node/anchor
+                timediffRec = receiverTimestamps[receivedFrame.source][0] + receiverTimestamps[receivedFrame.source][2] - 2*receiverTimestamps[receivedFrame.source][1];
+                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)
+        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);
+
+    while(!acknowledgement[destination] && (LocalTimer.read() < time_before + 0.1f)); // wait for succeeding ranging or timeout
+
+    roundtriptimes[destination] = LocalTimer.read() - time_before;
+    distances[destination] = (tofs[destination] * 300 * TIMEUNITS_TO_US / 4);
+}
+
+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;
+    dw.sendDelayedFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), rxTimestamp + ANSWER_DELAY_TIMEUNITS);
+}
\ No newline at end of file