init
Dependencies: aconno_I2C Lis2dh12 WatchdogTimer
Diff: MM2WayRanging.cpp
- Revision:
- 58:8d4a354816b1
--- /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