Diff: MMRanging/MMRanging.cpp

Revision:

34:f56962030c5c

Parent:

33:6ea4c3dd504d

Child:

35:95d838d33bf5

--- a/MMRanging/MMRanging.cpp	Tue Dec 02 13:55:01 2014 +0000
+++ b/MMRanging/MMRanging.cpp	Sat Dec 27 14:37:12 2014 +0000
@@ -1,6 +1,6 @@
 #include "MMRanging.h"
 
-MMRanging::MMRanging(DW1000& DW) : dw(DW){
+MMRanging::MMRanging(DW1000& DW) : dw(DW) {
     MMRanging::dw = dw;
     
     ranging_i = 0;
@@ -19,27 +19,27 @@
     
     rangingframe RX;
     dw.readRegister(DW1000_RX_BUFFER, 0, (uint8_t*)&RX, dw.getFramelength());  // get data from buffer
-    
+    uint8_t Index = RX.sequence_number % 10;
+
     switch (RX.type) {
         case 1:
-            rangingtimingsReceiver[RX.sequence_number][0] = dw.getRXTimestamp();
+            rangingtimingsReceiver[Index][0] = dw.getRXTimestamp();
             TX.type = 2;
             TX.sequence_number = RX.sequence_number;
             TX.time_difference_receiver = 0;
-            TX.dummy = 1;
-            dw.sendFrame((uint8_t*)&TX, 12);
+            dw.sendFrame((uint8_t*)&TX, sizeof(TX));
             break;
         case 2:
-            rangingtimingsSender[RX.sequence_number][1] = dw.getRXTimestamp();
+            rangingtimingsSender[Index][1] = dw.getRXTimestamp();
             break;
         case 3:
             TX.type = 4;
-            TX.time_difference_receiver = rangingtimingsReceiver[TX.sequence_number][1] - rangingtimingsReceiver[TX.sequence_number][0];
-            TX.dummy = 1;
-            dw.sendFrame((uint8_t*)&TX, 12);
+            TX.time_difference_receiver = rangingtimingsReceiver[Index][1] - rangingtimingsReceiver[Index][0];
+            dw.sendFrame((uint8_t*)&TX, sizeof(TX));
             break;
         case 4:
-            tofs[RX.sequence_number % 10] = rangingtimingsSender[RX.sequence_number][1] - rangingtimingsSender[RX.sequence_number][0] - (uint32_t)RX.time_difference_receiver;
+            //tofs[Index] = RX.time_difference_receiver;
+            tofs[Index] = (rangingtimingsSender[Index][1] - rangingtimingsSender[Index][0] - RX.time_difference_receiver);// - 65536;
             //tofs[RX->sequence_number] = (rangingtimingsSender[RX->sequence_number][1] - rangingtimingsSender[RX->sequence_number][0] - RX->time_difference_receiver) >> 1;
             break;
         default : break;
@@ -60,13 +60,13 @@
 
 void MMRanging::callbackTX() {
     TX_timestamp = dw.getTXTimestamp();
-    
+    uint8_t Index = TX.sequence_number % 10;
     switch (TX.type) {
         case 1:
-            rangingtimingsSender[TX.sequence_number][0] = dw.getTXTimestamp();
+            rangingtimingsSender[Index][0] = dw.getTXTimestamp();
             break;
         case 2:
-            rangingtimingsReceiver[TX.sequence_number][1] = dw.getTXTimestamp();
+            rangingtimingsReceiver[Index][1] = dw.getTXTimestamp();
             break;
         default: break;
     }
@@ -86,15 +86,20 @@
     TX.type = 1;
     TX.sequence_number = counter;
     TX.time_difference_receiver = 0;
-    TX.dummy = 1;
-    dw.sendFrame((uint8_t*)&TX, 12);
+    dw.sendFrame((uint8_t*)&TX, sizeof(TX));
 }
 
 void MMRanging::requestTime() {
     TX.type = 3;
     TX.sequence_number = counter;
     TX.time_difference_receiver = 0;
-    TX.dummy = 1;
-    dw.sendFrame((uint8_t*)&TX, 12);
+    dw.sendFrame((uint8_t*)&TX, sizeof(TX));
     counter++;
+}
+
+void MMRanging::sendRangingframe(uint8_t type, uint8_t sequence_number, uint64_t time_difference_receiver, uint16_t dummy) {
+    TX.type = type;
+    TX.sequence_number = sequence_number;
+    TX.time_difference_receiver = time_difference_receiver;
+    dw.sendFrame((uint8_t*)&TX, sizeof(TX));
 }
\ No newline at end of file