Diff: MMRanging/MMRanging.cpp

Revision:

41:0a3bb028d4ba

Parent:

40:5ce51b7e3118

Child:

43:d89fe237a684

--- a/MMRanging/MMRanging.cpp	Fri Feb 27 11:47:51 2015 +0000
+++ b/MMRanging/MMRanging.cpp	Sun Mar 01 20:19:38 2015 +0000
@@ -6,6 +6,7 @@
     dw.setCallbacks(this, &MMRanging::callbackRX, &MMRanging::callbackTX);
     for (int i = 0; i < 10; i++)
         acknowledgement[i] = true;
+    LocalTimer.start();
     dw.startRX();
 }
 
@@ -66,7 +67,18 @@
 }
 
 void MMRanging::requestRanging(uint8_t destination) {
+    acknowledgement[destination] = false;
+    float time_before = LocalTimer.read();
     sendRangingframe(destination, counter, 1, 0);
+    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 / MMRANGING_TIMEUNIT_US / 2);
+}
+
+void MMRanging::requestRangingAll() {
+    for (int i = 1; i <= 4; i++) {  // Request ranging to all anchors
+        requestRanging(i);
+    }
 }
 
 void MMRanging::sendRangingframe(uint8_t destination, uint8_t sequence_number, uint8_t type, uint64_t time_difference_receiver) {