Diff: MMRanging/MMRanging.cpp

Revision:

31:6f76f3d518ac

Parent:

29:019ff388ed76

Child:

32:041dd02e0e3b

--- a/MMRanging/MMRanging.cpp	Fri Nov 28 17:06:51 2014 +0000
+++ b/MMRanging/MMRanging.cpp	Mon Dec 01 13:04:18 2014 +0000
@@ -3,6 +3,8 @@
 MMRanging::MMRanging(DW1000& DW) : dw(DW){
     MMRanging::dw = dw;
     
+    ranging_i = 0;
+    
     message[0] = '\0';
     messageRX[0] = '\0';
     event_i = 0;
@@ -14,13 +16,33 @@
 
 void MMRanging::callbackRX() {
     RX_timestamp = dw.getRXTimestamp();
+    
     dw.receiveString(messageRX);
-    if (receiver) {
-        message[0] = 'A';                               // acknowledge messages
-        for(int i = 0; i < 10; i++)
-            message[i+1] = messageRX[i];
-        dw.sendString(message);
+    
+    rangingframe* RX = (rangingframe*)messageRX;
+    switch (RX->type) {
+        case 1:
+            rangingtimingsReceiver[RX->sequence_number][0] = dw.getRXTimestamp();
+            TX.type = 2;
+            TX.sequence_number = RX->sequence_number;
+            TX.time_difference_receiver = 0;
+            dw.sendFrame((uint8_t*)&TX, 10);
+            break;
+        case 2:
+            rangingtimingsSender[RX->sequence_number][1] = dw.getRXTimestamp();
+            break;
+        case 3:
+            TX.type = 4;
+            TX.time_difference_receiver = rangingtimingsReceiver[TX.sequence_number][1] - rangingtimingsReceiver[TX.sequence_number][0];
+            dw.sendFrame((uint8_t*)&TX, 10);
+            break;
+        case 4:
+            tofs[RX->sequence_number] = rangingtimingsSender[RX->sequence_number][1] - rangingtimingsSender[RX->sequence_number][0] - RX->time_difference_receiver;
+            //tofs[RX->sequence_number] = (rangingtimingsSender[RX->sequence_number][1] - rangingtimingsSender[RX->sequence_number][0] - RX->time_difference_receiver) >> 1;
+            break;
+        default : break;
     }
+    
     eventtimes[event_i] = RX_timestamp - TX_timestamp;                      // TODO: can give some wrong values because of timer reset after 17 seconds
     event[event_i][0] = '!';
     event[event_i][1] = 'R';
@@ -31,17 +53,30 @@
         event_i = 0;
     else
         event_i++;
+    
     dw.startRX();
 }
 
 void MMRanging::callbackTX() {
     TX_timestamp = dw.getTXTimestamp();
+    
+    switch (TX.type) {
+        case 1:
+            rangingtimingsSender[TX.sequence_number][0] = dw.getTXTimestamp();
+            break;
+        case 2:
+            rangingtimingsReceiver[TX.sequence_number][1] = dw.getTXTimestamp();
+            break;
+        default: break;
+    }
+    
     eventtimes[event_i] = 0;
     event[event_i][0] = '!';
     event[event_i][1] = 'S';
     event[event_i][2] = ' ';
-    for(int i = 0; i < 10; i++)
-        event[event_i][i+3] = message[i];
+    sprintf(&event[event_i][3], "%d %d", TX.sequence_number, TX.type);
+    //for(int i = 0; i < 10; i++)
+    //    event[event_i][i+3] = message[i];
     if (event_i == 8)
         event_i = 0;
     else
@@ -49,7 +84,16 @@
 }
 
 void MMRanging::requestRanging() {
-    sprintf(message, "%d", counter);                  // send numbers to acknowledge
+    TX.type = 1;
+    TX.sequence_number = counter;
+    TX.time_difference_receiver = 0;
+    dw.sendFrame((uint8_t*)&TX, 10);
+}
+
+void MMRanging::requestTime() {
+    TX.type = 3;
+    TX.sequence_number = counter;
+    TX.time_difference_receiver = 0;
+    dw.sendFrame((uint8_t*)&TX, 10);
     counter++;
-    dw.sendString(message);
 }
\ No newline at end of file