File content as of revision 0:a3b83d366423:

#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("#%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:
               
                ack = true;
                break;
                
            
                
        }
    }
    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 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);
    }
        
   
        
}