File content as of revision 58:8d4a354816b1:

#include "MM2WayRanging.h"
#include "NRFuart.h"


MM2WayRanging::MM2WayRanging(DW1000& DW) : dw(DW) {
    isBeacon = true;
    overflow = false;
    address = 0;
    
    LocalTimer.start();
    dw.startRX();   
}


bool MM2WayRanging::waitForFrameRX(float time_before) {
    bool frameReceived = false;
    while(!frameReceived && (LocalTimer.read() < time_before + 0.02f)) {
        frameReceived = dw.hasReceivedFrame();
    }; // wait for succeeding or timeout
    if (frameReceived) {
        //debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "Frame Received\n");debug_exe();
        callbackRX();
        dw.clearReceivedFlag();
    } else {
        //debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "Receive Timeout\n");debug_exe();
    }
    return frameReceived;
}


bool MM2WayRanging::waitForFrameTX(float time_before) {
    bool frameSent = false;
    while(!frameSent && (LocalTimer.read() < time_before + 0.02f)) {
        frameSent = dw.hasSentFrame();
    }; // wait for succeeding or timeout
    if (frameSent) {
        //debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "Frame Sent\n");debug_exe();
        callbackTX();
        dw.clearSentFlag();
    } else {
        //debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "Send Timeout\n");debug_exe();
    }
    return frameSent;
}

 

void MM2WayRanging::callbackRX() {
    dw.readRegister(DW1000_RX_BUFFER, 0, (uint8_t*)&receivedFrame, dw.getFramelength());
    
    //MESSAGES ADDRESSED FOR ALL DETECTORS
    if (receivedFrame.destination == 0) {  
        switch (receivedFrame.type) {
            case BEACON_READY:
                anchor_to_beacon_Send(receivedFrame.source);
                //TODO - WE COULD ALSO TIME THIS FRAME AND THEN DIVIDE BY THREE TO GET MORE ACCURACY?
                break;
            default : break;
        }
    }
    
    //MESSAGES ADDRESSED FOR ME
    if (receivedFrame.destination == address) 
        switch (receivedFrame.type) {
            case ANCHOR_TO_BEACON_PING:
                RxTimestamp = dw.getRXTimestamp();
                beacon_to_anchor_response_Send(receivedFrame.source, RxTimestamp);
                break;
            case BEACON_TO_ANCHOR_RESPONSE:
                rangingRxTimestamp[receivedFrame.destination] = dw.getRXTimestamp();
                //Calulate time/distance
                rangingTOF[receivedFrame.source] = (rangingRxTimestamp[receivedFrame.source] - rangingTxTimestamp[receivedFrame.source]); //TODO need to remove ANSWER_DELAY_TIMEUNITS from this
                rangingDistance[receivedFrame.source] = (rangingTOF[receivedFrame.source] * 300 * TIMEUNITS_TO_US / 4); //TODO should this be divide by 2?
                debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "Distance(%d): %d\n",receivedFrame.source, rangingDistance[receivedFrame.source]);debug_exe();
                break;
            default : break;
        }
 
    //debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "Received From: %d\n",receivedFrame.destination);debug_exe();
    dw.startRX();
}

 

void MM2WayRanging::callbackTX() {
    //dw.readRegister(DW1000_TX_BUFFER, 0, (uint8_t*)&sentFrame, dw.getFramelength());
    switch (rangingFrame.type) {
        case BEACON_READY:
            //No Need to do anything
            break;
        case ANCHOR_TO_BEACON_PING:
            rangingTxTimestamp[rangingFrame.destination] = dw.getTXTimestamp();
            break;
        case BEACON_TO_ANCHOR_RESPONSE:
            //No Need to do anything
            break;
        default: break;
    }
}



bool MM2WayRanging::beacon_requestRanging() {
    float time_before = LocalTimer.read();
    beacon_ready_Send();
    bool sendSuccess = waitForFrameTX(time_before);
    if (sendSuccess) {
        //WAIT FOR FIRST RANGING FROM AN ANCHOR - TIMEOUT AFTER NO RANGING FOR CERTAIN TIME
        float lastRangingTime = LocalTimer.read();
        while((LocalTimer.read() - lastRangingTime) < 0.02f) {
            if (waitForFrameRX(lastRangingTime)) {
                lastRangingTime = LocalTimer.read();
            }
        }
    } else {
        //Send Fail   
    }
    //debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "Range End - %d\n",destination);debug_exe();
}



void MM2WayRanging::anchor_standbyToRange() {
    float time_before = LocalTimer.read();
    waitForFrameRX(time_before);
}


void MM2WayRanging::beacon_ready_Send() {
    rangingFrame.source = address;
    rangingFrame.destination = 0;
    rangingFrame.type = BEACON_READY;
    dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame));
}
 
void MM2WayRanging::anchor_to_beacon_Send(uint8_t destination) {
    rangingFrame.source = address;
    rangingFrame.destination = destination;
    rangingFrame.type = ANCHOR_TO_BEACON_PING;
    dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame));
}

void MM2WayRanging::beacon_to_anchor_response_Send(uint8_t destination, uint64_t rxTimestamp) {
    rangingFrame.source = address;
    rangingFrame.destination = destination;
    rangingFrame.type = BEACON_TO_ANCHOR_RESPONSE;
    if(rxTimestamp + ANSWER_DELAY_TIMEUNITS > MMRANGING_2POWER40) {
        dw.sendDelayedFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), rxTimestamp + ANSWER_DELAY_TIMEUNITS - MMRANGING_2POWER40);
    } else {
        dw.sendDelayedFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), rxTimestamp + ANSWER_DELAY_TIMEUNITS);
    }
}












/*
void MM2WayRanging::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] > receiverTimestamps[source][2]){
        receiverTimestamps[source][2] += MMRANGING_2POWER40;
    }
}
 
void MM2WayRanging::correctSenderTimestamps(uint8_t source){
    if (senderTimestamps[source][0] > senderTimestamps[source][1]) {
        senderTimestamps[source][1] += MMRANGING_2POWER40;
        senderTimestamps[source][2] += MMRANGING_2POWER40;
        overflow = true;
    } else if (senderTimestamps[source][1] > senderTimestamps[source][2]) {
        senderTimestamps[source][2] += MMRANGING_2POWER40;
        overflow = true;
    } else { 
        overflow = false;
    }
}
*/