Pathfindr / Mbed OS mbed-os-PF-UWBBEACON_v1_dev

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?

UserRevisionLine numberNew 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 */