MM2WayRanging.cpp

00001 #include "MM2WayRanging.h"
00002 #include "NRFuart.h"
00003 
00004 
00005 MM2WayRanging::MM2WayRanging(DW1000& DW) : dw(DW) {
00006     isBeacon = true;
00007     overflow = false;
00008     address = 0;
00009     
00010     LocalTimer.start();
00011     dw.startRX();   
00012 }
00013 
00014 
00015 bool MM2WayRanging::waitForFrameRX(float time_before) {
00016     bool frameReceived = false;
00017     while(!frameReceived && (LocalTimer.read() < time_before + 0.02f)) {
00018         frameReceived = dw.hasReceivedFrame();
00019     }; // wait for succeeding or timeout
00020     if (frameReceived) {
00021         //debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "Frame Received\n");debug_exe();
00022         callbackRX();
00023         dw.clearReceivedFlag();
00024     } else {
00025         //debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "Receive Timeout\n");debug_exe();
00026     }
00027     return frameReceived;
00028 }
00029 
00030 
00031 bool MM2WayRanging::waitForFrameTX(float time_before) {
00032     bool frameSent = false;
00033     while(!frameSent && (LocalTimer.read() < time_before + 0.02f)) {
00034         frameSent = dw.hasSentFrame();
00035     }; // wait for succeeding or timeout
00036     if (frameSent) {
00037         //debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "Frame Sent\n");debug_exe();
00038         callbackTX();
00039         dw.clearSentFlag();
00040     } else {
00041         //debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "Send Timeout\n");debug_exe();
00042     }
00043     return frameSent;
00044 }
00045 
00046  
00047 
00048 void MM2WayRanging::callbackRX() {
00049     dw.readRegister(DW1000_RX_BUFFER, 0, (uint8_t*)&receivedFrame, dw.getFramelength());
00050     
00051     //MESSAGES ADDRESSED FOR ALL DETECTORS
00052     if (receivedFrame.destination == 0) {  
00053         switch (receivedFrame.type) {
00054             case BEACON_READY:
00055                 anchor_to_beacon_Send(receivedFrame.source);
00056                 //TODO - WE COULD ALSO TIME THIS FRAME AND THEN DIVIDE BY THREE TO GET MORE ACCURACY?
00057                 break;
00058             default : break;
00059         }
00060     }
00061     
00062     //MESSAGES ADDRESSED FOR ME
00063     if (receivedFrame.destination == address) 
00064         switch (receivedFrame.type) {
00065             case ANCHOR_TO_BEACON_PING:
00066                 RxTimestamp = dw.getRXTimestamp();
00067                 beacon_to_anchor_response_Send(receivedFrame.source, RxTimestamp);
00068                 break;
00069             case BEACON_TO_ANCHOR_RESPONSE:
00070                 rangingRxTimestamp[receivedFrame.destination] = dw.getRXTimestamp();
00071                 //Calulate time/distance
00072                 rangingTOF[receivedFrame.source] = (rangingRxTimestamp[receivedFrame.source] - rangingTxTimestamp[receivedFrame.source]); //TODO need to remove ANSWER_DELAY_TIMEUNITS from this
00073                 rangingDistance[receivedFrame.source] = (rangingTOF[receivedFrame.source] * 300 * TIMEUNITS_TO_US / 4); //TODO should this be divide by 2?
00074                 debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "Distance(%d): %d\n",receivedFrame.source, rangingDistance[receivedFrame.source]);debug_exe();
00075                 break;
00076             default : break;
00077         }
00078  
00079     //debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "Received From: %d\n",receivedFrame.destination);debug_exe();
00080     dw.startRX();
00081 }
00082 
00083  
00084 
00085 void MM2WayRanging::callbackTX() {
00086     //dw.readRegister(DW1000_TX_BUFFER, 0, (uint8_t*)&sentFrame, dw.getFramelength());
00087     switch (rangingFrame.type) {
00088         case BEACON_READY:
00089             //No Need to do anything
00090             break;
00091         case ANCHOR_TO_BEACON_PING:
00092             rangingTxTimestamp[rangingFrame.destination] = dw.getTXTimestamp();
00093             break;
00094         case BEACON_TO_ANCHOR_RESPONSE:
00095             //No Need to do anything
00096             break;
00097         default: break;
00098     }
00099 }
00100 
00101 
00102 
00103 bool MM2WayRanging::beacon_requestRanging() {
00104     float time_before = LocalTimer.read();
00105     beacon_ready_Send();
00106     bool sendSuccess = waitForFrameTX(time_before);
00107     if (sendSuccess) {
00108         //WAIT FOR FIRST RANGING FROM AN ANCHOR - TIMEOUT AFTER NO RANGING FOR CERTAIN TIME
00109         float lastRangingTime = LocalTimer.read();
00110         while((LocalTimer.read() - lastRangingTime) < 0.02f) {
00111             if (waitForFrameRX(lastRangingTime)) {
00112                 lastRangingTime = LocalTimer.read();
00113             }
00114         }
00115     } else {
00116         //Send Fail   
00117     }
00118     //debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "Range End - %d\n",destination);debug_exe();
00119 }
00120 
00121 
00122 
00123 void MM2WayRanging::anchor_standbyToRange() {
00124     float time_before = LocalTimer.read();
00125     waitForFrameRX(time_before);
00126 }
00127 
00128 
00129 void MM2WayRanging::beacon_ready_Send() {
00130     rangingFrame.source = address;
00131     rangingFrame.destination = 0;
00132     rangingFrame.type = BEACON_READY;
00133     dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame));
00134 }
00135  
00136 void MM2WayRanging::anchor_to_beacon_Send(uint8_t destination) {
00137     rangingFrame.source = address;
00138     rangingFrame.destination = destination;
00139     rangingFrame.type = ANCHOR_TO_BEACON_PING;
00140     dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame));
00141 }
00142 
00143 void MM2WayRanging::beacon_to_anchor_response_Send(uint8_t destination, uint64_t rxTimestamp) {
00144     rangingFrame.source = address;
00145     rangingFrame.destination = destination;
00146     rangingFrame.type = BEACON_TO_ANCHOR_RESPONSE;
00147     if(rxTimestamp + ANSWER_DELAY_TIMEUNITS > MMRANGING_2POWER40) {
00148         dw.sendDelayedFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), rxTimestamp + ANSWER_DELAY_TIMEUNITS - MMRANGING_2POWER40);
00149     } else {
00150         dw.sendDelayedFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), rxTimestamp + ANSWER_DELAY_TIMEUNITS);
00151     }
00152 }
00153 
00154 
00155 
00156 
00157 
00158 
00159 
00160 
00161 
00162 
00163 
00164 
00165 /*
00166 void MM2WayRanging::correctReceiverTimestamps(uint8_t source){
00167     if(receiverTimestamps[source][0] > receiverTimestamps[source][1]){
00168         receiverTimestamps[source][1] += MMRANGING_2POWER40;
00169         receiverTimestamps[source][2] += MMRANGING_2POWER40;
00170     }
00171     if(receiverTimestamps[source][1] > receiverTimestamps[source][2]){
00172         receiverTimestamps[source][2] += MMRANGING_2POWER40;
00173     }
00174 }
00175  
00176 void MM2WayRanging::correctSenderTimestamps(uint8_t source){
00177     if (senderTimestamps[source][0] > senderTimestamps[source][1]) {
00178         senderTimestamps[source][1] += MMRANGING_2POWER40;
00179         senderTimestamps[source][2] += MMRANGING_2POWER40;
00180         overflow = true;
00181     } else if (senderTimestamps[source][1] > senderTimestamps[source][2]) {
00182         senderTimestamps[source][2] += MMRANGING_2POWER40;
00183         overflow = true;
00184     } else { 
00185         overflow = false;
00186     }
00187 }
00188 */