Diff: nodes/nodes.cpp

Revision:

0:3333b6066adf

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/nodes/nodes.cpp	Wed Dec 06 21:42:54 2017 +0000
@@ -0,0 +1,506 @@
+#include "nodes.h"
+
+extern Node   *node;
+extern BeaconNode  beaconNode;
+extern AnchorNode  anchorNode;
+extern BaseStationNode  baseStationNode;
+
+Node::Node(DecaWave& DW) : dw(DW) {
+    address = 0;      
+}
+void Node::setAnchor(bool anc){
+    Anchor = anc;
+}
+
+bool Node::isAnchor(){
+    return Anchor;
+}
+bool Node::isBaseStation(){
+    return address == BASE_STATION_ADDR;
+}   
+void Node::setAddress(uint8_t adr){
+    address = adr;
+}
+
+uint8_t Node::getAddress(){
+    return address;
+}
+
+
+
+//------- BeaconNode Class -------------------
+BeaconNode::BeaconNode(DecaWave& DW) : Node(DW) {
+    Anchor = false;
+    for (int i = 0; i < 3; i++)
+        acknowledgement[i] = true;
+    LocalTimer.start();
+    mode=0;   
+}
+
+#pragma Otime // Compiler optimize Runtime at the cost of image size
+// Two Way Ranging Actions when Frame was received
+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();
+                    if((receivedFrame.signedTime >> 24) == BECOME_BEACON){
+                        pc.printf("\r\n \r\n ---This Node is already a beacon ---\r\n \r\n");
+                    } 
+                    else if((receivedFrame.signedTime >> 24) == BECOME_ANCHOR){
+                        node = &anchorNode;
+                        pc.printf("\r\n \r\n ---This Node is an anchor now ---\r\n \r\n");
+                    }
+                    
+                    break;
+                case BASE_ORDER:
+                    repetitions = receivedFrame.signedTime&0xffff;
+                    mode = receivedFrame.signedTime >> 24;
+                    destination = (receivedFrame.signedTime&0xff0000) >> 16;
+                    sendBaseAck();
+                    break;
+                case ANCHOR_RESPONSE:
+                {
+                    sendAnswer(receivedFrame.source, BEACON_RESPONSE);  
+                    senderTimestamps[receivedFrame.source][1] = RxTime;        //Save the second timestamp on the sending node/beacon (T_rr)
+                    acknowledgement[0] = true;
+                    
+                     // Get Signal Level
+                    dwt_rxdiag_t diagnostic;
+                    dwt_readdiagnostics(&diagnostic);
+                    signalStrength[receivedFrame.source] = dw.getRXLevel(&diagnostic);               
+                    
+                    break;
+                }
+                case TRANSFER_FRAME:
+                {
+                    //timediffSend = 2 * senderTimestamps[receivedFrame.source][1] - senderTimestamps[receivedFrame.source][0] - senderTimestamps[receivedFrame.source][2];
+                    tofs[receivedFrame.source] = receivedFrame.signedTime + 2 * senderTimestamps[receivedFrame.source][1] - senderTimestamps[receivedFrame.source][0] - senderTimestamps[receivedFrame.source][2];   
+                    
+                    // Get Signal Level
+                    dwt_rxdiag_t diagnostic2;
+                    dwt_readdiagnostics(&diagnostic2);
+                    float level = dw.getRXLevel(&diagnostic2);
+                    if(level < signalStrength[receivedFrame.source])
+                        signalStrength[receivedFrame.source] = level;
+                                 
+                    acknowledgement[1] = true;
+                  
+                    //dw.turnonrx();   // start listening again
+                    break;
+                }
+                default : break;
+            }
+        }
+        else{
+          dw.turnonrx();   // start listening again
+        }
+}
+
+
+#pragma Otime // Compiler optimize Runtime at the cost of image size
+// Two Way Ranging Actions when Frame was transmitted
+void BeaconNode::callbackTX(uint64_t TxTime) {
+    //pc.printf("TXCallback: %d %d %d \r\n", acknowledgement[0], acknowledgement[1], acknowledgement[2]);    
+    dw.turnonrx();   // start listening again
+    
+    switch (rangingFrame.type) {
+        case PING:
+            senderTimestamps[rangingFrame.destination][0] = TxTime;    //Save the first timestamp on the sending node/beacon (T_sp)
+            break;
+        case BEACON_RESPONSE:
+            senderTimestamps[rangingFrame.destination][2] = TxTime;    //Save the third timestamp on the sending node/beacon (T_sr)
+            correctSenderTimestamps(rangingFrame.destination);         //Correct the timestamps for the case of a counter overflow        
+            break;   
+        default:
+            break;
+    }
+    if(acknowledgement[1] == true){
+        acknowledgement[2] = true; 
+       // pc.printf("Ack edited %d \r\n",  acknowledgement[2]);
+    }      
+}
+
+
+#pragma Otime // Compiler optimize Runtime at the cost of image size
+/**
+ *  Get the distance to the Anchor with address @param destination.
+ *
+ *   @param destination The address of the anchor
+ */
+void BeaconNode::requestRanging(uint8_t destination) {
+    if(noRec[destination] <= MAX_TRIES){
+        
+        
+        float time_before = LocalTimer.read();
+        
+        while(!acknowledgement[2] && LocalTimer.read() < time_before + 0.001f); // Wait until previous StreamFrame is sent 
+         
+        acknowledgement[0] = false;
+        acknowledgement[1] = false;
+        acknowledgement[2] = false;
+        time_before = LocalTimer.read();
+    
+        sendPingFrame(destination);
+    
+        while(!acknowledgement[1] && (LocalTimer.read() < time_before + 0.001f + 0.003f*acknowledgement[0])); // One Ranging normaly takes less than 1.5 miliseconds
+    
+        if(acknowledgement[1]){
+            distances[destination] = calibratedDistance(destination);
+            noRec[destination] = 0;
+            // Stream Data to Basestation                
+            sendStreamFrame(destination);         
+        } else {
+            distances[destination] = -10;
+            noRec[destination]++;
+        }     
+    }     
+}
+
+#pragma Otime // Compiler optimize Runtime at the cost of image size
+inline float BeaconNode::calibratedDistance(uint8_t destination) {
+
+    float rawDistance = (tofs[destination] * 300 * TIMEUNITS_TO_US / 4);
+    //float correctDistance = rawDistance + dwt_getrangebias(7, rawDistance, DWT_PRF_64M);
+
+    //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;
+        
+    //else if (rawDistance >= 3)
+    //    rawDistance += 0.0004*rawDistance - 0.5556
+   /* else
+        rawDistance += -0.01799*rawDistance - 0.2724;
+        
+    else if ()*/
+        
+        
+        //Non-Correction-Term: rawDistance -= 0.458;
+    
+    
+ // Calibration for Nucleo 0 (and 1)
+
+ //   if (this->address == 1) rawDistance+= 10;
+//    switch(destination){
+//        case 2:
+//            return  rawDistance * 0.9754 - 0.5004;
+//        case 3:
+//            return  rawDistance * 0.9759 - 0.4103;
+//        case 4:
+//            return  rawDistance * 0.9798 - 0.5499;
+//        case 5:
+//            return  rawDistance * 0.9765 - 0.5169;
+//        }
+
+    return rawDistance;
+
+}
+
+#pragma Otime // Compiler optimize Runtime at the cost of image size
+void BeaconNode::requestRangingAll() {
+    for (int i = 0; i < ADRESSES_COUNT; i++) {  // Request ranging to all anchors
+        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;
+    rangingFrame.destination = destination;
+    rangingFrame.type = PING;
+    //dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame));
+    dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), 0, 0);
+}
+
+#pragma Otime // Compiler optimize Runtime at the cost of image size
+void BeaconNode::sendBaseAck() {
+    rangingFrame.source = address;
+    rangingFrame.destination = BASE_STATION_ADDR;
+    rangingFrame.type = BASE_ORDER_ACK;
+    //dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame));
+    dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), 0, 0);
+}
+
+#pragma Otime // Compiler optimize Runtime at the cost of image size
+void BeaconNode::sendStreamFrame(uint8_t anchor_addr) {    
+    StreamFrame streamFrame;
+        
+    streamFrame.source = address;
+    streamFrame.destination = BASE_STATION_ADDR;
+    streamFrame.type = STREAM_TO_BASE;
+    streamFrame.anchor_adress =  anchor_addr;
+    streamFrame.distance = getDistance(anchor_addr);
+    streamFrame.signalStrength = getSignalStrength(anchor_addr);
+    streamFrame.FPLevel = dw.getFPLevel();
+    
+    dw.sendFrame((uint8_t*)&streamFrame, sizeof(streamFrame), 0, 0);
+}
+
+#pragma Otime // Compiler optimize Runtime at the cost of image size
+void BeaconNode::sendAnswer(uint8_t destination, uint8_t type) {
+
+    rangingFrame.source = address;
+    rangingFrame.destination = destination;
+    rangingFrame.type = type;
+    
+    dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), 0, 0);
+}
+
+#pragma Otime // Compiler optimize Runtime at the cost of image size
+void BeaconNode::correctSenderTimestamps(uint8_t source){
+
+    if (senderTimestamps[source][0] - dw.getAntennaDelay() > senderTimestamps[source][1]) {
+        senderTimestamps[source][1] += MMRANGING_2POWER40;
+        senderTimestamps[source][2] += MMRANGING_2POWER40;
+    }
+    if (senderTimestamps[source][1] > senderTimestamps[source][2]) {
+        senderTimestamps[source][2] += MMRANGING_2POWER40;
+    }
+
+}
+
+float BeaconNode::getSignalStrength(uint8_t index){
+    return signalStrength[index];
+}  
+
+float BeaconNode::getDistance(uint8_t index){
+    return distances[index];
+} 
+
+uint8* BeaconNode::getRecFrameRef(){
+   return (uint8 *) &receivedFrame;           
+}
+uint16 BeaconNode::getRecFrameLength(){
+    return sizeof(receivedFrame);
+} 
+
+int BeaconNode::getMode(){
+    return mode;
+}  
+
+uint16_t BeaconNode::getRepetitions(){
+    return repetitions;
+}
+void BeaconNode::decreaseRepetitions(){
+    repetitions--;
+}
+uint8_t BeaconNode::getDestination(){
+    return destination;
+}
+
+void BeaconNode::clearRec(){
+    for(int j = 0; j < ADRESSES_COUNT; j++)
+        noRec[j] = 0;
+}
+
+
+//------- AnchorNode Class -------------------
+AnchorNode::AnchorNode(DecaWave& DW) : Node(DW) {
+    Anchor = true;
+}
+
+#pragma Otime // Compiler optimize Runtime at the cost of image size
+// Two Way Ranging Actions when Frame was received
+void AnchorNode::callbackRX(uint64_t RxTime) {
+   // pc.printf("Got a frame, adress: %d source: %d \r\n", receivedFrame.destination, receivedFrame.source);
+   // if(!isBaseStation()){    
+        if (receivedFrame.destination == address){
+            switch (receivedFrame.type) {
+                case SWITCH_TYPE:
+                    sendBaseAck();
+                    if((receivedFrame.signedTime >> 24) == BECOME_BEACON){
+                        node = &beaconNode; 
+                        pc.printf("\r\n \r\n ---This Node is a beacon now ---\r\n \r\n");
+                    } 
+                    else if((receivedFrame.signedTime >> 24) == BECOME_ANCHOR){
+                        pc.printf("\r\n \r\n ---This Node is already an anchor ---\r\n \r\n");
+                    }
+                    break;
+                case PING:
+                    sendAnswer(receivedFrame.source, ANCHOR_RESPONSE);  
+                    receiverTimestamps[receivedFrame.source][0] = RxTime;      //Save the first timestamp on the receiving node/anchor (T_rp)             
+                    break;
+                case BEACON_RESPONSE:
+                    {
+                    receiverTimestamps[receivedFrame.source][2] = RxTime;      //Save the third timestamp on the receiving node/anchor (T_rf)
+                    correctReceiverTimestamps(receivedFrame.source);                //Correct the timestamps for the case of a counter overflow
+                    //timediffRec = receiverTimestamps[receivedFrame.source][0] + receiverTimestamps[receivedFrame.source][2] - 2*receiverTimestamps[receivedFrame.source][1];
+                    //if(timediffRec < 0)
+                    //    timediffRec = 0;
+                    sendTransferFrame(receivedFrame.source, receiverTimestamps[receivedFrame.source][0] + receiverTimestamps[receivedFrame.source][2] - 2*receiverTimestamps[receivedFrame.source][1]);
+                    break;
+                    }
+                default : break;
+            }
+        }
+        else{
+          dw.turnonrx();   // start listening again
+        }
+}
+
+
+#pragma Otime // Compiler optimize Runtime at the cost of image size
+// Two Way Ranging Actions when Frame was transmitted
+void AnchorNode::callbackTX(uint64_t TxTime) {
+    dw.turnonrx();   // start listening again
+    
+    switch (rangingFrame.type) {
+        case ANCHOR_RESPONSE:
+            receiverTimestamps[rangingFrame.destination][1] = TxTime;  //Save the second timestamp on the receiving node/anchor (T_sr)
+            break;  
+        default:
+            break;
+    }   
+}
+
+
+
+
+#pragma Otime // Compiler optimize Runtime at the cost of image size
+void AnchorNode::sendBaseAck() {
+    rangingFrame.source = address;
+    rangingFrame.destination = BASE_STATION_ADDR;
+    rangingFrame.type = BASE_ORDER_ACK;
+    //dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame));
+    dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), 0, 0);
+}
+
+
+
+
+
+#pragma Otime // Compiler optimize Runtime at the cost of image size
+void AnchorNode::sendTransferFrame(uint8_t destination, int timeDiffsReceiver) {
+    ExtendedRangingFrame transferFrame;
+        
+    transferFrame.source = address;
+    transferFrame.destination = destination;
+    transferFrame.type = TRANSFER_FRAME;
+    transferFrame.signedTime =  timeDiffsReceiver;                      //cast the time difference
+    dw.sendFrame((uint8_t*)&transferFrame, sizeof(transferFrame), 0, 0);
+}
+
+#pragma Otime // Compiler optimize Runtime at the cost of image size
+void AnchorNode::sendAnswer(uint8_t destination, uint8_t type) {
+
+    rangingFrame.source = address;
+    rangingFrame.destination = destination;
+    rangingFrame.type = type;
+    
+    dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), 0, 0);
+}
+
+#pragma Otime // Compiler optimize Runtime at the cost of image size
+void AnchorNode::correctReceiverTimestamps(uint8_t source){
+
+    if(receiverTimestamps[source][0] > receiverTimestamps[source][1]){
+        receiverTimestamps[source][1] += MMRANGING_2POWER40;
+        receiverTimestamps[source][2] += MMRANGING_2POWER40;
+    }
+
+    if(receiverTimestamps[source][1] - dw.getAntennaDelay() > receiverTimestamps[source][2]){
+        receiverTimestamps[source][2] += MMRANGING_2POWER40;
+    }
+
+}
+
+
+
+uint8* AnchorNode::getRecFrameRef(){
+   return (uint8 *) &receivedFrame;           
+}
+uint16 AnchorNode::getRecFrameLength(){
+   return sizeof(receivedFrame);
+} 
+
+
+//------- BaseStationNode Class -------------------
+BaseStationNode::BaseStationNode(DecaWave& DW) : Node(DW) {
+    Anchor = false;
+    LocalTimer.start();    
+}
+
+uint8* BaseStationNode::getRecFrameRef(){
+   return (uint8 *) &receivedStreamFrame;           
+}
+uint16 BaseStationNode::getRecFrameLength(){
+   return sizeof(receivedStreamFrame);
+} 
+
+
+#pragma Otime // Compiler optimize Runtime at the cost of image size
+// Two Way Ranging Actions when Frame was received
+void BaseStationNode::callbackRX(uint64_t RxTime) {
+    pc.printf("Got a frame, adress: %d source: %d \r\n", receivedStreamFrame.destination, receivedStreamFrame.source);
+    if (receivedStreamFrame.destination == address){
+        switch(receivedStreamFrame.type){
+            case STREAM_TO_BASE:
+                pc.printf("Debug Point 8\r\n");
+                //pc.printf("#%d to #%d %f(%f) \r\n", receivedStreamFrame.source, receivedStreamFrame.anchor_adress, receivedStreamFrame.distance, receivedStreamFrame.signalStrength);
+                pc.printf("#%03d/#%03d/%+011.6f/%+011.6f/%+011.6f/ \r\n", receivedStreamFrame.source, receivedStreamFrame.anchor_adress, receivedStreamFrame.distance, receivedStreamFrame.signalStrength, receivedStreamFrame.FPLevel/*dw.getFPLevel()*/);
+                break;
+            case BASE_ORDER_ACK:
+                pc.printf("Debug Point 9\r\n");
+                ack = true;
+                break;
+                
+            default:
+                pc.printf("Debug Point 10\r\n");
+                break;
+                
+        }
+    }
+    dw.turnonrx();   // start listening again
+    
+    pc.printf("Debug Point 11\r\n");
+}
+
+#pragma Otime // Compiler optimize Runtime at the cost of image size
+// Two Way Ranging Actions when Frame was transmitted
+void BaseStationNode::callbackTX(uint64_t TxTime) {  
+    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;
+    ack = false;
+        
+    orderFrame.source = address;
+    orderFrame.destination = beacon_destination;
+    orderFrame.type = type;
+    orderFrame.signedTime =  action << 24 | anchor_destination << 16 | repetitions;           
+ 
+    int i = 0;     
+    for(i = 0; i < 10 && !ack; i++){
+        float time_before = LocalTimer.read(); 
+        dw.sendFrame((uint8_t*)&orderFrame, sizeof(orderFrame), 0, 0);
+        while(!ack && (LocalTimer.read() < time_before + 0.010 + 0.01*i)); // One Ranging normaly takes less than 1.5 miliseconds
+    }
+    if(!ack)
+    {
+        pc.printf("ERROR: Tag #%d did not respond \r\n", beacon_destination);
+    }
+    else
+    {
+        pc.printf("Order sent, %d tries \r\n", i);
+    }
+        
+    pc.printf("Debug Point 7\r\n");
+        
+}