Diff: nodes/nodes.cpp

Revision:

3:8bee1711d186

Parent:

2:5adf0b785944

--- a/nodes/nodes.cpp	Wed Jan 10 16:14:49 2018 +0000
+++ b/nodes/nodes.cpp	Wed Feb 28 16:10:21 2018 +0000
@@ -42,6 +42,7 @@
 void BeaconNode::callbackRX(uint64_t RxTime) {
    //pc.printf("Got a frame, adress: %d source: %d \r\n", receivedFrame.destination, receivedFrame.source);   
         if (receivedFrame.destination == address){
+
             switch (receivedFrame.type) {
                 case SWITCH_TYPE:
                     sendBaseAck();
@@ -84,9 +85,10 @@
                     float level = dw.getRXLevel(&diagnostic2);
                     if(level < signalStrength[receivedFrame.source])
                         signalStrength[receivedFrame.source] = level;
+                    
                                  
                     acknowledgement[1] = true;
-                  
+                 
                    // dw.turnonrx();   // start listening again
                     break;
                 }
@@ -120,7 +122,8 @@
     if(acknowledgement[1] == true){
         acknowledgement[2] = true; 
        // pc.printf("Ack edited %d \r\n",  acknowledgement[2]);
-    }      
+    }    
+      
 }
 
 
@@ -135,7 +138,7 @@
         pc.printf(" max try %d\n\r",MAX_TRIES);
         float time_before = LocalTimer.read();
         
-        while(!acknowledgement[2] && LocalTimer.read() < time_before + 0.001f); // Wait until previous StreamFrame is sent 
+        while(!acknowledgement[2] && LocalTimer.read() < time_before + 0.0025f); // Wait until previous StreamFrame is sent 
          
         acknowledgement[0] = false;
         acknowledgement[1] = false;
@@ -144,7 +147,7 @@
     
         sendPingFrame(destination);
     
-        while(!acknowledgement[1] && (LocalTimer.read() < time_before + 0.001f + 0.003f*acknowledgement[0])); // One Ranging normaly takes less than 1.5 miliseconds
+        while(!acknowledgement[1] && (LocalTimer.read() < time_before + 0.0025f + 0.003f*acknowledgement[0])); // One Ranging normaly takes less than 1.5 miliseconds
     
         if(acknowledgement[1]){
             distances[destination] = calibratedDistance(destination);
@@ -152,7 +155,7 @@
             // Stream Data to Basestation                
             sendStreamFrame(destination);         
         } else {
-            acknowledgement[1]=1;
+            //acknowledgement[1]=1;
             distances[destination] = -10;
             noRec[destination] = 0;
             sendStreamFrame(destination);
@@ -172,16 +175,16 @@
     //if(rawDistance <= 8.458)
     //    rawDistance -= 0.0541*rawDistance; // Correction Term 22-03-2017
     //else
-    if(rawDistance >= 22.7)
-        rawDistance += -0.0004*rawDistance - 0.3971;
-    else if (rawDistance >= 14.3)
-        rawDistance += -0.0015*rawDistance - 0.372;
-    else if (rawDistance >= 8)
-        rawDistance += -0.0029*rawDistance - 0.352;
-    else if (rawDistance >= 3.93)
-        rawDistance += 0.001*rawDistance - 0.370;
-    else
-        rawDistance += -0.0235*rawDistance - 0.273;
+    //if(rawDistance >= 22.7)
+    //    rawDistance += -0.0004*rawDistance - 0.3971;
+    //else if (rawDistance >= 14.3)
+    //    rawDistance += -0.0015*rawDistance - 0.372;
+    //else if (rawDistance >= 8)
+    //    rawDistance += -0.0029*rawDistance - 0.352;
+    //else if (rawDistance >= 3.93)
+    //    rawDistance += 0.001*rawDistance - 0.370;
+    //else
+    //    rawDistance += -0.0235*rawDistance - 0.273;
         
     //else if (rawDistance >= 3)
     //    rawDistance += 0.0004*rawDistance - 0.5556
@@ -209,7 +212,7 @@
 //        }
 
     return rawDistance;
-
+    //return tofs[destination];
 }
 
 #pragma Otime // Compiler optimize Runtime at the cost of image size
@@ -222,7 +225,15 @@
             distances[i] = -10;
     }
 }
-
+void BeaconNode::requestRangingInt(uint8_t from,uint8_t to) {
+    for (int i=from; i <= to; i++){
+        if (i!= address) {
+            requestRanging(i);
+        }
+        else
+        distances[i] = -10;
+    }
+}
 #pragma Otime // Compiler optimize Runtime at the cost of image size
 void BeaconNode::sendPingFrame(uint8_t destination) {
     rangingFrame.source = address;
@@ -339,6 +350,7 @@
                 case PING:
                     sendAnswer(receivedFrame.source, ANCHOR_RESPONSE);  
                     receiverTimestamps[receivedFrame.source][0] = RxTime;      //Save the first timestamp on the receiving node/anchor (T_rp)             
+                    //dw.turnonrx();
                     break;
                 case BEACON_RESPONSE:
                     {
@@ -348,9 +360,12 @@
                     //if(timediffRec < 0)
                     //    timediffRec = 0;
                     sendTransferFrame(receivedFrame.source, receiverTimestamps[receivedFrame.source][0] + receiverTimestamps[receivedFrame.source][2] - 2*receiverTimestamps[receivedFrame.source][1]);
+                    //dw.turnonrx();
                     break;
                     }
-                default : break;
+                default : 
+                dw.turnonrx(); 
+                break;
             }
         }
         else{
@@ -463,7 +478,10 @@
                
                 ack = true;
                 break;
-                
+            
+            default: 
+            //dw.turnonrx(); 
+            break;   
 
                 
             
@@ -481,6 +499,8 @@
     dw.turnonrx();   // start listening again      
 }
 
+
+
 #pragma Otime // Compiler optimize Runtime at the cost of image size
 void BaseStationNode::sendOrder(uint8_t beacon_destination, uint8_t anchor_destination, uint8_t action, uint16_t repetitions, uint8_t type) {
     ExtendedRangingFrame orderFrame;
@@ -489,7 +509,11 @@
     orderFrame.source = address;
     orderFrame.destination = beacon_destination;
     orderFrame.type = type;
-    orderFrame.signedTime =  action << 24 | anchor_destination << 16 | repetitions;           
+    if(action == 1){
+        orderFrame.signedTime =  action << 24 | anchor_destination << 16 | 1;  
+    }
+    else
+        orderFrame.signedTime =  action << 24 | anchor_destination << 16 | repetitions;           
  
     int i = 0;     
     for(i = 0; i < 3 && !ack; i++){
@@ -500,12 +524,13 @@
     if(!ack)
     {
         pc.printf("ERROR: Tag #%d did not respond \r\n", beacon_destination);
+
     }
     else
     {
        // pc.printf("Order sent, %d tries \r\n", i);
     }
-        
+       
    
         
 }