init
Dependencies: aconno_I2C Lis2dh12 WatchdogTimer
MM2WayRanging.cpp@58:8d4a354816b1, 2020-02-17 (annotated)
- Committer:
- pathfindr
- Date:
- Mon Feb 17 23:24:52 2020 +0000
- Revision:
- 58:8d4a354816b1
usb
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
pathfindr | 58:8d4a354816b1 | 1 | #include "MM2WayRanging.h" |
pathfindr | 58:8d4a354816b1 | 2 | #include "NRFuart.h" |
pathfindr | 58:8d4a354816b1 | 3 | |
pathfindr | 58:8d4a354816b1 | 4 | |
pathfindr | 58:8d4a354816b1 | 5 | MM2WayRanging::MM2WayRanging(DW1000& DW) : dw(DW) { |
pathfindr | 58:8d4a354816b1 | 6 | isBeacon = true; |
pathfindr | 58:8d4a354816b1 | 7 | overflow = false; |
pathfindr | 58:8d4a354816b1 | 8 | address = 0; |
pathfindr | 58:8d4a354816b1 | 9 | |
pathfindr | 58:8d4a354816b1 | 10 | LocalTimer.start(); |
pathfindr | 58:8d4a354816b1 | 11 | dw.startRX(); |
pathfindr | 58:8d4a354816b1 | 12 | } |
pathfindr | 58:8d4a354816b1 | 13 | |
pathfindr | 58:8d4a354816b1 | 14 | |
pathfindr | 58:8d4a354816b1 | 15 | bool MM2WayRanging::waitForFrameRX(float time_before) { |
pathfindr | 58:8d4a354816b1 | 16 | bool frameReceived = false; |
pathfindr | 58:8d4a354816b1 | 17 | while(!frameReceived && (LocalTimer.read() < time_before + 0.02f)) { |
pathfindr | 58:8d4a354816b1 | 18 | frameReceived = dw.hasReceivedFrame(); |
pathfindr | 58:8d4a354816b1 | 19 | }; // wait for succeeding or timeout |
pathfindr | 58:8d4a354816b1 | 20 | if (frameReceived) { |
pathfindr | 58:8d4a354816b1 | 21 | //debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "Frame Received\n");debug_exe(); |
pathfindr | 58:8d4a354816b1 | 22 | callbackRX(); |
pathfindr | 58:8d4a354816b1 | 23 | dw.clearReceivedFlag(); |
pathfindr | 58:8d4a354816b1 | 24 | } else { |
pathfindr | 58:8d4a354816b1 | 25 | //debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "Receive Timeout\n");debug_exe(); |
pathfindr | 58:8d4a354816b1 | 26 | } |
pathfindr | 58:8d4a354816b1 | 27 | return frameReceived; |
pathfindr | 58:8d4a354816b1 | 28 | } |
pathfindr | 58:8d4a354816b1 | 29 | |
pathfindr | 58:8d4a354816b1 | 30 | |
pathfindr | 58:8d4a354816b1 | 31 | bool MM2WayRanging::waitForFrameTX(float time_before) { |
pathfindr | 58:8d4a354816b1 | 32 | bool frameSent = false; |
pathfindr | 58:8d4a354816b1 | 33 | while(!frameSent && (LocalTimer.read() < time_before + 0.02f)) { |
pathfindr | 58:8d4a354816b1 | 34 | frameSent = dw.hasSentFrame(); |
pathfindr | 58:8d4a354816b1 | 35 | }; // wait for succeeding or timeout |
pathfindr | 58:8d4a354816b1 | 36 | if (frameSent) { |
pathfindr | 58:8d4a354816b1 | 37 | //debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "Frame Sent\n");debug_exe(); |
pathfindr | 58:8d4a354816b1 | 38 | callbackTX(); |
pathfindr | 58:8d4a354816b1 | 39 | dw.clearSentFlag(); |
pathfindr | 58:8d4a354816b1 | 40 | } else { |
pathfindr | 58:8d4a354816b1 | 41 | //debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "Send Timeout\n");debug_exe(); |
pathfindr | 58:8d4a354816b1 | 42 | } |
pathfindr | 58:8d4a354816b1 | 43 | return frameSent; |
pathfindr | 58:8d4a354816b1 | 44 | } |
pathfindr | 58:8d4a354816b1 | 45 | |
pathfindr | 58:8d4a354816b1 | 46 | |
pathfindr | 58:8d4a354816b1 | 47 | |
pathfindr | 58:8d4a354816b1 | 48 | void MM2WayRanging::callbackRX() { |
pathfindr | 58:8d4a354816b1 | 49 | dw.readRegister(DW1000_RX_BUFFER, 0, (uint8_t*)&receivedFrame, dw.getFramelength()); |
pathfindr | 58:8d4a354816b1 | 50 | |
pathfindr | 58:8d4a354816b1 | 51 | //MESSAGES ADDRESSED FOR ALL DETECTORS |
pathfindr | 58:8d4a354816b1 | 52 | if (receivedFrame.destination == 0) { |
pathfindr | 58:8d4a354816b1 | 53 | switch (receivedFrame.type) { |
pathfindr | 58:8d4a354816b1 | 54 | case BEACON_READY: |
pathfindr | 58:8d4a354816b1 | 55 | anchor_to_beacon_Send(receivedFrame.source); |
pathfindr | 58:8d4a354816b1 | 56 | //TODO - WE COULD ALSO TIME THIS FRAME AND THEN DIVIDE BY THREE TO GET MORE ACCURACY? |
pathfindr | 58:8d4a354816b1 | 57 | break; |
pathfindr | 58:8d4a354816b1 | 58 | default : break; |
pathfindr | 58:8d4a354816b1 | 59 | } |
pathfindr | 58:8d4a354816b1 | 60 | } |
pathfindr | 58:8d4a354816b1 | 61 | |
pathfindr | 58:8d4a354816b1 | 62 | //MESSAGES ADDRESSED FOR ME |
pathfindr | 58:8d4a354816b1 | 63 | if (receivedFrame.destination == address) |
pathfindr | 58:8d4a354816b1 | 64 | switch (receivedFrame.type) { |
pathfindr | 58:8d4a354816b1 | 65 | case ANCHOR_TO_BEACON_PING: |
pathfindr | 58:8d4a354816b1 | 66 | RxTimestamp = dw.getRXTimestamp(); |
pathfindr | 58:8d4a354816b1 | 67 | beacon_to_anchor_response_Send(receivedFrame.source, RxTimestamp); |
pathfindr | 58:8d4a354816b1 | 68 | break; |
pathfindr | 58:8d4a354816b1 | 69 | case BEACON_TO_ANCHOR_RESPONSE: |
pathfindr | 58:8d4a354816b1 | 70 | rangingRxTimestamp[receivedFrame.destination] = dw.getRXTimestamp(); |
pathfindr | 58:8d4a354816b1 | 71 | //Calulate time/distance |
pathfindr | 58:8d4a354816b1 | 72 | rangingTOF[receivedFrame.source] = (rangingRxTimestamp[receivedFrame.source] - rangingTxTimestamp[receivedFrame.source]); //TODO need to remove ANSWER_DELAY_TIMEUNITS from this |
pathfindr | 58:8d4a354816b1 | 73 | rangingDistance[receivedFrame.source] = (rangingTOF[receivedFrame.source] * 300 * TIMEUNITS_TO_US / 4); //TODO should this be divide by 2? |
pathfindr | 58:8d4a354816b1 | 74 | debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "Distance(%d): %d\n",receivedFrame.source, rangingDistance[receivedFrame.source]);debug_exe(); |
pathfindr | 58:8d4a354816b1 | 75 | break; |
pathfindr | 58:8d4a354816b1 | 76 | default : break; |
pathfindr | 58:8d4a354816b1 | 77 | } |
pathfindr | 58:8d4a354816b1 | 78 | |
pathfindr | 58:8d4a354816b1 | 79 | //debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "Received From: %d\n",receivedFrame.destination);debug_exe(); |
pathfindr | 58:8d4a354816b1 | 80 | dw.startRX(); |
pathfindr | 58:8d4a354816b1 | 81 | } |
pathfindr | 58:8d4a354816b1 | 82 | |
pathfindr | 58:8d4a354816b1 | 83 | |
pathfindr | 58:8d4a354816b1 | 84 | |
pathfindr | 58:8d4a354816b1 | 85 | void MM2WayRanging::callbackTX() { |
pathfindr | 58:8d4a354816b1 | 86 | //dw.readRegister(DW1000_TX_BUFFER, 0, (uint8_t*)&sentFrame, dw.getFramelength()); |
pathfindr | 58:8d4a354816b1 | 87 | switch (rangingFrame.type) { |
pathfindr | 58:8d4a354816b1 | 88 | case BEACON_READY: |
pathfindr | 58:8d4a354816b1 | 89 | //No Need to do anything |
pathfindr | 58:8d4a354816b1 | 90 | break; |
pathfindr | 58:8d4a354816b1 | 91 | case ANCHOR_TO_BEACON_PING: |
pathfindr | 58:8d4a354816b1 | 92 | rangingTxTimestamp[rangingFrame.destination] = dw.getTXTimestamp(); |
pathfindr | 58:8d4a354816b1 | 93 | break; |
pathfindr | 58:8d4a354816b1 | 94 | case BEACON_TO_ANCHOR_RESPONSE: |
pathfindr | 58:8d4a354816b1 | 95 | //No Need to do anything |
pathfindr | 58:8d4a354816b1 | 96 | break; |
pathfindr | 58:8d4a354816b1 | 97 | default: break; |
pathfindr | 58:8d4a354816b1 | 98 | } |
pathfindr | 58:8d4a354816b1 | 99 | } |
pathfindr | 58:8d4a354816b1 | 100 | |
pathfindr | 58:8d4a354816b1 | 101 | |
pathfindr | 58:8d4a354816b1 | 102 | |
pathfindr | 58:8d4a354816b1 | 103 | bool MM2WayRanging::beacon_requestRanging() { |
pathfindr | 58:8d4a354816b1 | 104 | float time_before = LocalTimer.read(); |
pathfindr | 58:8d4a354816b1 | 105 | beacon_ready_Send(); |
pathfindr | 58:8d4a354816b1 | 106 | bool sendSuccess = waitForFrameTX(time_before); |
pathfindr | 58:8d4a354816b1 | 107 | if (sendSuccess) { |
pathfindr | 58:8d4a354816b1 | 108 | //WAIT FOR FIRST RANGING FROM AN ANCHOR - TIMEOUT AFTER NO RANGING FOR CERTAIN TIME |
pathfindr | 58:8d4a354816b1 | 109 | float lastRangingTime = LocalTimer.read(); |
pathfindr | 58:8d4a354816b1 | 110 | while((LocalTimer.read() - lastRangingTime) < 0.02f) { |
pathfindr | 58:8d4a354816b1 | 111 | if (waitForFrameRX(lastRangingTime)) { |
pathfindr | 58:8d4a354816b1 | 112 | lastRangingTime = LocalTimer.read(); |
pathfindr | 58:8d4a354816b1 | 113 | } |
pathfindr | 58:8d4a354816b1 | 114 | } |
pathfindr | 58:8d4a354816b1 | 115 | } else { |
pathfindr | 58:8d4a354816b1 | 116 | //Send Fail |
pathfindr | 58:8d4a354816b1 | 117 | } |
pathfindr | 58:8d4a354816b1 | 118 | //debug_prep();snprintf(GLOBAL_debug_buffer, sizeof(GLOBAL_debug_buffer), "Range End - %d\n",destination);debug_exe(); |
pathfindr | 58:8d4a354816b1 | 119 | } |
pathfindr | 58:8d4a354816b1 | 120 | |
pathfindr | 58:8d4a354816b1 | 121 | |
pathfindr | 58:8d4a354816b1 | 122 | |
pathfindr | 58:8d4a354816b1 | 123 | void MM2WayRanging::anchor_standbyToRange() { |
pathfindr | 58:8d4a354816b1 | 124 | float time_before = LocalTimer.read(); |
pathfindr | 58:8d4a354816b1 | 125 | waitForFrameRX(time_before); |
pathfindr | 58:8d4a354816b1 | 126 | } |
pathfindr | 58:8d4a354816b1 | 127 | |
pathfindr | 58:8d4a354816b1 | 128 | |
pathfindr | 58:8d4a354816b1 | 129 | void MM2WayRanging::beacon_ready_Send() { |
pathfindr | 58:8d4a354816b1 | 130 | rangingFrame.source = address; |
pathfindr | 58:8d4a354816b1 | 131 | rangingFrame.destination = 0; |
pathfindr | 58:8d4a354816b1 | 132 | rangingFrame.type = BEACON_READY; |
pathfindr | 58:8d4a354816b1 | 133 | dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame)); |
pathfindr | 58:8d4a354816b1 | 134 | } |
pathfindr | 58:8d4a354816b1 | 135 | |
pathfindr | 58:8d4a354816b1 | 136 | void MM2WayRanging::anchor_to_beacon_Send(uint8_t destination) { |
pathfindr | 58:8d4a354816b1 | 137 | rangingFrame.source = address; |
pathfindr | 58:8d4a354816b1 | 138 | rangingFrame.destination = destination; |
pathfindr | 58:8d4a354816b1 | 139 | rangingFrame.type = ANCHOR_TO_BEACON_PING; |
pathfindr | 58:8d4a354816b1 | 140 | dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame)); |
pathfindr | 58:8d4a354816b1 | 141 | } |
pathfindr | 58:8d4a354816b1 | 142 | |
pathfindr | 58:8d4a354816b1 | 143 | void MM2WayRanging::beacon_to_anchor_response_Send(uint8_t destination, uint64_t rxTimestamp) { |
pathfindr | 58:8d4a354816b1 | 144 | rangingFrame.source = address; |
pathfindr | 58:8d4a354816b1 | 145 | rangingFrame.destination = destination; |
pathfindr | 58:8d4a354816b1 | 146 | rangingFrame.type = BEACON_TO_ANCHOR_RESPONSE; |
pathfindr | 58:8d4a354816b1 | 147 | if(rxTimestamp + ANSWER_DELAY_TIMEUNITS > MMRANGING_2POWER40) { |
pathfindr | 58:8d4a354816b1 | 148 | dw.sendDelayedFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), rxTimestamp + ANSWER_DELAY_TIMEUNITS - MMRANGING_2POWER40); |
pathfindr | 58:8d4a354816b1 | 149 | } else { |
pathfindr | 58:8d4a354816b1 | 150 | dw.sendDelayedFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), rxTimestamp + ANSWER_DELAY_TIMEUNITS); |
pathfindr | 58:8d4a354816b1 | 151 | } |
pathfindr | 58:8d4a354816b1 | 152 | } |
pathfindr | 58:8d4a354816b1 | 153 | |
pathfindr | 58:8d4a354816b1 | 154 | |
pathfindr | 58:8d4a354816b1 | 155 | |
pathfindr | 58:8d4a354816b1 | 156 | |
pathfindr | 58:8d4a354816b1 | 157 | |
pathfindr | 58:8d4a354816b1 | 158 | |
pathfindr | 58:8d4a354816b1 | 159 | |
pathfindr | 58:8d4a354816b1 | 160 | |
pathfindr | 58:8d4a354816b1 | 161 | |
pathfindr | 58:8d4a354816b1 | 162 | |
pathfindr | 58:8d4a354816b1 | 163 | |
pathfindr | 58:8d4a354816b1 | 164 | |
pathfindr | 58:8d4a354816b1 | 165 | /* |
pathfindr | 58:8d4a354816b1 | 166 | void MM2WayRanging::correctReceiverTimestamps(uint8_t source){ |
pathfindr | 58:8d4a354816b1 | 167 | if(receiverTimestamps[source][0] > receiverTimestamps[source][1]){ |
pathfindr | 58:8d4a354816b1 | 168 | receiverTimestamps[source][1] += MMRANGING_2POWER40; |
pathfindr | 58:8d4a354816b1 | 169 | receiverTimestamps[source][2] += MMRANGING_2POWER40; |
pathfindr | 58:8d4a354816b1 | 170 | } |
pathfindr | 58:8d4a354816b1 | 171 | if(receiverTimestamps[source][1] > receiverTimestamps[source][2]){ |
pathfindr | 58:8d4a354816b1 | 172 | receiverTimestamps[source][2] += MMRANGING_2POWER40; |
pathfindr | 58:8d4a354816b1 | 173 | } |
pathfindr | 58:8d4a354816b1 | 174 | } |
pathfindr | 58:8d4a354816b1 | 175 | |
pathfindr | 58:8d4a354816b1 | 176 | void MM2WayRanging::correctSenderTimestamps(uint8_t source){ |
pathfindr | 58:8d4a354816b1 | 177 | if (senderTimestamps[source][0] > senderTimestamps[source][1]) { |
pathfindr | 58:8d4a354816b1 | 178 | senderTimestamps[source][1] += MMRANGING_2POWER40; |
pathfindr | 58:8d4a354816b1 | 179 | senderTimestamps[source][2] += MMRANGING_2POWER40; |
pathfindr | 58:8d4a354816b1 | 180 | overflow = true; |
pathfindr | 58:8d4a354816b1 | 181 | } else if (senderTimestamps[source][1] > senderTimestamps[source][2]) { |
pathfindr | 58:8d4a354816b1 | 182 | senderTimestamps[source][2] += MMRANGING_2POWER40; |
pathfindr | 58:8d4a354816b1 | 183 | overflow = true; |
pathfindr | 58:8d4a354816b1 | 184 | } else { |
pathfindr | 58:8d4a354816b1 | 185 | overflow = false; |
pathfindr | 58:8d4a354816b1 | 186 | } |
pathfindr | 58:8d4a354816b1 | 187 | } |
pathfindr | 58:8d4a354816b1 | 188 | */ |