Diff: MM2WayRanging.cpp

Revision:

58:8d4a354816b1

diff -r 066dfbe8b4b9 -r 8d4a354816b1 MM2WayRanging.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MM2WayRanging.cpp	Mon Feb 17 23:24:52 2020 +0000
@@ -0,0 +1,188 @@
+#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;
+    }
+}
+*/
\ No newline at end of file