Diff: MM2WayRanging/MM2WayRanging.cpp

Revision:

48:5999e510f154

Parent:

47:b6120c152ad1

Child:

50:50b8aea54a51

diff -r b6120c152ad1 -r 5999e510f154 MM2WayRanging/MM2WayRanging.cpp
--- a/MM2WayRanging/MM2WayRanging.cpp	Thu Mar 19 12:54:28 2015 +0000
+++ b/MM2WayRanging/MM2WayRanging.cpp	Tue Nov 24 16:41:23 2015 +0000
@@ -1,6 +1,15 @@
 #include "MM2WayRanging.h"
 
+//#include "PC.h"
+//static PC pc(USBTX, USBRX, 115200);           // USB UART Terminal
+
 MM2WayRanging::MM2WayRanging(DW1000& DW) : dw(DW) {
+    for (int i = 0; i < 3; ++i) {
+        memset(reception_stats[i], 0, sizeof(ReceptionStats));
+    }
+    memset(roundtriptimes, 0, sizeof(roundtriptimes));
+    memset(distances, 0, sizeof(distances));
+
     isAnchor = true;
     overflow = false;
     address = 0;
@@ -18,53 +27,104 @@
 }
 
 void MM2WayRanging::callbackRX() {
+    //pc.printf("Received frame\r\n");
     dw.readRegister(DW1000_RX_BUFFER, 0, (uint8_t*)&receivedFrame, dw.getFramelength());
 
-    if (receivedFrame.destination == address)
+    if (receivedFrame.remote_address == address) {
+        uint16_t std_noise = dw.getStdNoise();
+        uint16_t preamble_acc_count = dw.getPACC();
+        uint16_t first_path_index = dw.getFPINDEX();
+        uint16_t first_path_amp_1 = dw.getFPAMPL1();
+        uint16_t first_path_amp_2 = dw.getFPAMPL2();
+        uint16_t first_path_amp_3 = dw.getFPAMPL3();
+        uint16_t channel_impulse_response_power = dw.getCIRPWR();
+        uint8_t prf = dw.getPRF();
         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);
+                receiverTimestamps[receivedFrame.address][0] = rxTimestamp;      //Save the first timestamp on the receiving node/anchor (T_rp)
+                sendDelayedAnswer(receivedFrame.address, ANCHOR_RESPONSE, rxTimestamp);
+                reception_stats[receivedFrame.address][0].std_noise = std_noise;
+                reception_stats[receivedFrame.address][0].preamble_acc_count = preamble_acc_count;
+                reception_stats[receivedFrame.address][0].first_path_index = first_path_index;
+                reception_stats[receivedFrame.address][0].first_path_amp_1 = first_path_amp_1;
+                reception_stats[receivedFrame.address][0].first_path_amp_2 = first_path_amp_2;
+                reception_stats[receivedFrame.address][0].first_path_amp_3 = first_path_amp_3;
+                reception_stats[receivedFrame.address][0].channel_impulse_response_power = channel_impulse_response_power;
+                reception_stats[receivedFrame.address][0].prf = prf;
+                //pc.printf("Received ping. Noise 0x%0x %d\r\n", std_noise, prf);
                 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);
+                senderTimestamps[receivedFrame.address][1] = rxTimestamp;        //Save the second timestamp on the sending node/beacon (T_rr)
+                sendDelayedAnswer(receivedFrame.address, 3, rxTimestamp);
+                //pc.printf("Received anchor response\r\n");
+                reception_stats[receivedFrame.address][1].std_noise = std_noise;
+                reception_stats[receivedFrame.address][1].preamble_acc_count = preamble_acc_count;
+                reception_stats[receivedFrame.address][1].first_path_index = first_path_index;
+                reception_stats[receivedFrame.address][1].first_path_amp_1 = first_path_amp_1;
+                reception_stats[receivedFrame.address][1].first_path_amp_2 = first_path_amp_2;
+                reception_stats[receivedFrame.address][1].first_path_amp_3 = first_path_amp_3;
+                reception_stats[receivedFrame.address][1].channel_impulse_response_power = channel_impulse_response_power;
+                reception_stats[receivedFrame.address][1].prf = prf;
                 break;
             case BEACON_RESPONSE:
                 rxTimestamp = dw.getRXTimestamp();
-                receiverTimestamps[receivedFrame.source][2] = rxTimestamp;      //Save the third timestamp on the receiving node/anchor (T_rf)
+                receiverTimestamps[receivedFrame.address][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
+                correctReceiverTimestamps(receivedFrame.address);                //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 );
+                timediffRec = - 2*receiverTimestamps[receivedFrame.address][1] + receiverTimestamps[receivedFrame.address][0] + receiverTimestamps[receivedFrame.address][2];
+                reception_stats[receivedFrame.address][2].std_noise = std_noise;
+                reception_stats[receivedFrame.address][2].preamble_acc_count = preamble_acc_count;
+                reception_stats[receivedFrame.address][2].first_path_index = first_path_index;
+                reception_stats[receivedFrame.address][2].first_path_amp_1 = first_path_amp_1;
+                reception_stats[receivedFrame.address][2].first_path_amp_2 = first_path_amp_2;
+                reception_stats[receivedFrame.address][2].first_path_amp_3 = first_path_amp_3;
+                reception_stats[receivedFrame.address][2].channel_impulse_response_power = channel_impulse_response_power;
+                reception_stats[receivedFrame.address][2].prf = prf;
+                sendTransferFrame(receivedFrame.address, timediffRec);
+                //pc.printf("%x %d\r\n", noise_levels[receivedFrame.address][0]);
+                //pc.printf("%x %d\r\n", noise_levels[receivedFrame.address][1]);
+                //pc.printf("%x %d\r\n", noise_levels[receivedFrame.address][2]);
+                //pc.printf("Received beacon response. Noise %d\r\n", noise_level);
                 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];
+                timediffSend = 2 * senderTimestamps[receivedFrame.address][1] - senderTimestamps[receivedFrame.address][0] - senderTimestamps[receivedFrame.address][2];
                 //calculation of the resulting sum of all four ToFs.
-                tofs[receivedFrame.source] = receivedFrame.signedTime + timediffSend;
-                acknowledgement[receivedFrame.source] = true;
+                tofs[receivedFrame.address] = receivedFrame.signedTime + timediffSend;
+                acknowledgement[receivedFrame.address] = true;
+                memcpy(&reception_stats[receivedFrame.address][0], &receivedFrame.stats1, sizeof(ReceptionStats));
+                memcpy(&reception_stats[receivedFrame.address][2], &receivedFrame.stats2, sizeof(ReceptionStats));
+                //reception_stats[receivedFrame.address][0].std_noise = receivedFrame.stats1.std_noise;
+                //reception_stats[receivedFrame.address][2].std_noise = receivedFrame.stats2.std_noise;
+                //noise_levels[receivedFrame.address][0] = receivedFrame.std_noise1;
+                //noise_levels[receivedFrame.address][2] = receivedFrame.std_noise2;
+                //pc.printf("Received transfer frame\r\n");
                 break;
             default : break;
         }
+    }
 
     dw.startRX();
 }
 
 void MM2WayRanging::callbackTX() {
+    //pc.printf("Sent frame\r\n");
     switch (rangingFrame.type) {
     case PING:
-        senderTimestamps[rangingFrame.destination][0] = dw.getTXTimestamp();    //Save the first timestamp on the sending node/beacon (T_sp)
+        senderTimestamps[rangingFrame.remote_address][0] = dw.getTXTimestamp();    //Save the first timestamp on the sending node/beacon (T_sp)
+        //pc.printf("Sent ping\r\n");
         break;
     case ANCHOR_RESPONSE:
-        receiverTimestamps[rangingFrame.destination][1] = dw.getTXTimestamp();  //Save the second timestamp on the receiving node/anchor (T_sr)
+        receiverTimestamps[rangingFrame.remote_address][1] = dw.getTXTimestamp();  //Save the second timestamp on the receiving node/anchor (T_sr)
+        //pc.printf("Sent anchor response\r\n");
         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
+        senderTimestamps[rangingFrame.remote_address][2] = dw.getTXTimestamp();    //Save the third timestamp on the sending node/beacon (T_sr)
+        correctSenderTimestamps(rangingFrame.remote_address);                      //Correct the timestamps for the case of a counter overflow
+        //pc.printf("Sent beacon response\r\n");
         break;
     default:
         break;
@@ -73,37 +133,43 @@
 }
 
 /**
- *  Get the distance to the Anchor with address @param destination.
+ *  Get the distance to the Anchor with address @param remote_address.
  *
- *   @param destination The address of the anchor
+ *   @param remote_address The address of the anchor
  */
-void MM2WayRanging::requestRanging(uint8_t destination) {
-    acknowledgement[destination] = false;
+void MM2WayRanging::requestRanging(uint8_t remote_address) {
+//    pc.printf("Request range %d\r\n", remote_address);
+    dw.enable_irq();
+//    pc.printf("Enabled IRQ %d\r\n", remote_address);
+    acknowledgement[remote_address] = false;
     float time_before = LocalTimer.read();
 
-    sendPingFrame(destination);
+//    pc.printf("Sending ping\r\n");
+    sendPingFrame(remote_address);
 
-    while(!acknowledgement[destination] && (LocalTimer.read() < time_before + 0.02f)); // wait for succeeding ranging or timeout
-
-    roundtriptimes[destination] = LocalTimer.read() - time_before;
+//    pc.printf("Waiting for ack\r\n");
+    while(!acknowledgement[remote_address] && (LocalTimer.read() < time_before + 0.5f)); // wait for succeeding ranging or timeout
 
-    if(acknowledgement[destination]){
-    distances[destination] = calibratedDistance(destination);
+    roundtriptimes[remote_address] = LocalTimer.read() - time_before;
+
+    if(acknowledgement[remote_address]){
+        distances[remote_address] = calibratedDistance(remote_address);
     } else {
-        distances[destination] = -1;
+        distances[remote_address] = -1;
     }
+    dw.disable_irq();
 }
 
-inline float MM2WayRanging::calibratedDistance(uint8_t destination) {
+inline float MM2WayRanging::calibratedDistance(uint8_t remote_address) {
 
-    float rawDistance = (tofs[destination] * 300 * TIMEUNITS_TO_US / 4);
+    float rawDistance = (tofs[remote_address] * 300 * TIMEUNITS_TO_US / 4);
 
 
 
  // Calibration for Nucleo 0 (and 1)
 
  //   if (this->address == 1) rawDistance+= 10;
-//    switch(destination){
+//    switch(remote_address){
 //        case 2:
 //            return  rawDistance * 0.9754 - 0.5004;
 //        case 3:
@@ -118,31 +184,27 @@
 
 }
 
-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;
+void MM2WayRanging::sendPingFrame(uint8_t remote_address) {
+    rangingFrame.address = address;
+    rangingFrame.remote_address = remote_address;
     rangingFrame.type = PING;
     dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame));
 }
 
-void MM2WayRanging::sendTransferFrame(uint8_t destination, int timeDiffsReceiver) {
-    transferFrame.source = address;
-    transferFrame.destination = destination;
+void MM2WayRanging::sendTransferFrame(uint8_t remote_address, int timeDiffsReceiver) {
+    transferFrame.address = address;
+    transferFrame.remote_address = remote_address;
     transferFrame.type = TRANSFER_FRAME;
     transferFrame.signedTime =  timeDiffsReceiver;                      //cast the time difference
+    memcpy(&transferFrame.stats1, &reception_stats[receivedFrame.address][0], sizeof(ReceptionStats));
+    memcpy(&transferFrame.stats2, &reception_stats[receivedFrame.address][2], sizeof(ReceptionStats));
     dw.sendFrame((uint8_t*)&transferFrame, sizeof(transferFrame));
 }
 
-void MM2WayRanging::sendDelayedAnswer(uint8_t destination, uint8_t type, uint64_t rxTimestamp) {
+void MM2WayRanging::sendDelayedAnswer(uint8_t remote_address, uint8_t type, uint64_t rxTimestamp) {
 
-    rangingFrame.source = address;
-    rangingFrame.destination = destination;
+    rangingFrame.address = address;
+    rangingFrame.remote_address = remote_address;
     rangingFrame.type = type;
 
     if(rxTimestamp + ANSWER_DELAY_TIMEUNITS > MMRANGING_2POWER40)