Tobias Naegeli / Mbed 2 deprecated AIT_UWB_Range

Tobi's ubw test branch

Dependencies:   mavlink_bridge mbed

Fork of AIT_UWB_Range by Benjamin Hepp

MM2WayRanging/MM2WayRanging.cpp@47:b6120c152ad1, 2015-03-19 (annotated)

Committer:
manumaet
Date:
Thu Mar 19 12:54:28 2015 +0000
Revision:
47:b6120c152ad1
Parent:
46:6398237672a0
Child:
48:5999e510f154
final release with the code used in the verification test case

Who changed what in which revision?

UserRevisionLine numberNew contents of line
manumaet 44:2e0045042a59 1 #include "MM2WayRanging.h"
manumaet 44:2e0045042a59 2
manumaet 44:2e0045042a59 3 MM2WayRanging::MM2WayRanging(DW1000& DW) : dw(DW) {
manumaet 44:2e0045042a59 4 isAnchor = true;
manumaet 45:01a33363bc21 5 overflow = false;
manumaet 44:2e0045042a59 6 address = 0;
manumaet 44:2e0045042a59 7 rxTimestamp = 0;
manumaet 44:2e0045042a59 8 timediffRec = 0;
manumaet 44:2e0045042a59 9 timediffSend = 0;
manumaet 44:2e0045042a59 10 for (int i = 0; i < 10; i++)
manumaet 44:2e0045042a59 11 acknowledgement[i] = true;
manumaet 44:2e0045042a59 12
manumaet 44:2e0045042a59 13 dw.setCallbacks(this, &MM2WayRanging::callbackRX, &MM2WayRanging::callbackTX);
manumaet 44:2e0045042a59 14
manumaet 45:01a33363bc21 15 LocalTimer.start();
manumaet 44:2e0045042a59 16
manumaet 44:2e0045042a59 17 dw.startRX();
manumaet 44:2e0045042a59 18 }
manumaet 44:2e0045042a59 19
manumaet 44:2e0045042a59 20 void MM2WayRanging::callbackRX() {
manumaet 44:2e0045042a59 21 dw.readRegister(DW1000_RX_BUFFER, 0, (uint8_t*)&receivedFrame, dw.getFramelength());
manumaet 44:2e0045042a59 22
manumaet 44:2e0045042a59 23 if (receivedFrame.destination == address)
manumaet 44:2e0045042a59 24 switch (receivedFrame.type) {
manumaet 44:2e0045042a59 25 case PING:
manumaet 44:2e0045042a59 26 rxTimestamp = dw.getRXTimestamp();
manumaet 44:2e0045042a59 27 receiverTimestamps[receivedFrame.source][0] = rxTimestamp; //Save the first timestamp on the receiving node/anchor (T_rp)
manumaet 44:2e0045042a59 28 sendDelayedAnswer(receivedFrame.source, ANCHOR_RESPONSE, rxTimestamp);
manumaet 44:2e0045042a59 29 break;
manumaet 44:2e0045042a59 30 case ANCHOR_RESPONSE:
manumaet 44:2e0045042a59 31 rxTimestamp = dw.getRXTimestamp();
manumaet 44:2e0045042a59 32 senderTimestamps[receivedFrame.source][1] = rxTimestamp; //Save the second timestamp on the sending node/beacon (T_rr)
manumaet 44:2e0045042a59 33 sendDelayedAnswer(receivedFrame.source, 3, rxTimestamp);
manumaet 44:2e0045042a59 34 break;
manumaet 44:2e0045042a59 35 case BEACON_RESPONSE:
manumaet 44:2e0045042a59 36 rxTimestamp = dw.getRXTimestamp();
manumaet 44:2e0045042a59 37 receiverTimestamps[receivedFrame.source][2] = rxTimestamp; //Save the third timestamp on the receiving node/anchor (T_rf)
manumaet 45:01a33363bc21 38
manumaet 45:01a33363bc21 39 correctReceiverTimestamps(receivedFrame.source); //Correct the timestamps for the case of a counter overflow
manumaet 44:2e0045042a59 40 //calculation of the summand on the receiving node/anchor
manumaet 45:01a33363bc21 41 timediffRec = - 2*receiverTimestamps[receivedFrame.source][1] + receiverTimestamps[receivedFrame.source][0] + receiverTimestamps[receivedFrame.source][2];
manumaet 44:2e0045042a59 42 sendTransferFrame(receivedFrame.source, timediffRec );
manumaet 44:2e0045042a59 43 break;
manumaet 44:2e0045042a59 44 case TRANSFER_FRAME:
manumaet 44:2e0045042a59 45 //calculation of the summand on the sending node/beacon
manumaet 44:2e0045042a59 46 timediffSend = 2 * senderTimestamps[receivedFrame.source][1] - senderTimestamps[receivedFrame.source][0] - senderTimestamps[receivedFrame.source][2];
manumaet 44:2e0045042a59 47 //calculation of the resulting sum of all four ToFs.
manumaet 44:2e0045042a59 48 tofs[receivedFrame.source] = receivedFrame.signedTime + timediffSend;
manumaet 44:2e0045042a59 49 acknowledgement[receivedFrame.source] = true;
manumaet 44:2e0045042a59 50 break;
manumaet 44:2e0045042a59 51 default : break;
manumaet 44:2e0045042a59 52 }
manumaet 46:6398237672a0 53
manumaet 44:2e0045042a59 54 dw.startRX();
manumaet 44:2e0045042a59 55 }
manumaet 44:2e0045042a59 56
manumaet 44:2e0045042a59 57 void MM2WayRanging::callbackTX() {
manumaet 44:2e0045042a59 58 switch (rangingFrame.type) {
manumaet 44:2e0045042a59 59 case PING:
manumaet 44:2e0045042a59 60 senderTimestamps[rangingFrame.destination][0] = dw.getTXTimestamp(); //Save the first timestamp on the sending node/beacon (T_sp)
manumaet 44:2e0045042a59 61 break;
manumaet 44:2e0045042a59 62 case ANCHOR_RESPONSE:
manumaet 44:2e0045042a59 63 receiverTimestamps[rangingFrame.destination][1] = dw.getTXTimestamp(); //Save the second timestamp on the receiving node/anchor (T_sr)
manumaet 44:2e0045042a59 64 break;
manumaet 44:2e0045042a59 65 case BEACON_RESPONSE:
manumaet 44:2e0045042a59 66 senderTimestamps[rangingFrame.destination][2] = dw.getTXTimestamp(); //Save the third timestamp on the sending node/beacon (T_sr)
manumaet 45:01a33363bc21 67 correctSenderTimestamps(rangingFrame.destination); //Correct the timestamps for the case of a counter overflow
manumaet 44:2e0045042a59 68 break;
manumaet 44:2e0045042a59 69 default:
manumaet 44:2e0045042a59 70 break;
manumaet 44:2e0045042a59 71 }
manumaet 45:01a33363bc21 72
manumaet 44:2e0045042a59 73 }
manumaet 44:2e0045042a59 74
manumaet 44:2e0045042a59 75 /**
manumaet 44:2e0045042a59 76 * Get the distance to the Anchor with address @param destination.
manumaet 44:2e0045042a59 77 *
manumaet 44:2e0045042a59 78 * @param destination The address of the anchor
manumaet 44:2e0045042a59 79 */
manumaet 44:2e0045042a59 80 void MM2WayRanging::requestRanging(uint8_t destination) {
manumaet 44:2e0045042a59 81 acknowledgement[destination] = false;
manumaet 44:2e0045042a59 82 float time_before = LocalTimer.read();
manumaet 44:2e0045042a59 83
manumaet 44:2e0045042a59 84 sendPingFrame(destination);
manumaet 44:2e0045042a59 85
manumaet 46:6398237672a0 86 while(!acknowledgement[destination] && (LocalTimer.read() < time_before + 0.02f)); // wait for succeeding ranging or timeout
manumaet 44:2e0045042a59 87
manumaet 44:2e0045042a59 88 roundtriptimes[destination] = LocalTimer.read() - time_before;
manumaet 45:01a33363bc21 89
manumaet 45:01a33363bc21 90 if(acknowledgement[destination]){
manumaet 45:01a33363bc21 91 distances[destination] = calibratedDistance(destination);
manumaet 45:01a33363bc21 92 } else {
manumaet 45:01a33363bc21 93 distances[destination] = -1;
manumaet 45:01a33363bc21 94 }
manumaet 45:01a33363bc21 95 }
manumaet 45:01a33363bc21 96
manumaet 45:01a33363bc21 97 inline float MM2WayRanging::calibratedDistance(uint8_t destination) {
manumaet 46:6398237672a0 98
manumaet 45:01a33363bc21 99 float rawDistance = (tofs[destination] * 300 * TIMEUNITS_TO_US / 4);
manumaet 46:6398237672a0 100
manumaet 46:6398237672a0 101
manumaet 46:6398237672a0 102
manumaet 46:6398237672a0 103 // Calibration for Nucleo 0 (and 1)
manumaet 46:6398237672a0 104
manumaet 46:6398237672a0 105 // if (this->address == 1) rawDistance+= 10;
manumaet 46:6398237672a0 106 // switch(destination){
manumaet 46:6398237672a0 107 // case 2:
manumaet 46:6398237672a0 108 // return rawDistance * 0.9754 - 0.5004;
manumaet 46:6398237672a0 109 // case 3:
manumaet 46:6398237672a0 110 // return rawDistance * 0.9759 - 0.4103;
manumaet 46:6398237672a0 111 // case 4:
manumaet 46:6398237672a0 112 // return rawDistance * 0.9798 - 0.5499;
manumaet 46:6398237672a0 113 // case 5:
manumaet 46:6398237672a0 114 // return rawDistance * 0.9765 - 0.5169;
manumaet 46:6398237672a0 115 // }
manumaet 46:6398237672a0 116
manumaet 46:6398237672a0 117 return rawDistance;
manumaet 46:6398237672a0 118
manumaet 44:2e0045042a59 119 }
manumaet 44:2e0045042a59 120
manumaet 44:2e0045042a59 121 void MM2WayRanging::requestRangingAll() {
manumaet 46:6398237672a0 122 for (int i = 1; i <= 4; i++) { // Request ranging to all anchors
manumaet 44:2e0045042a59 123 requestRanging(i);
manumaet 44:2e0045042a59 124 }
manumaet 44:2e0045042a59 125 }
manumaet 44:2e0045042a59 126
manumaet 44:2e0045042a59 127 void MM2WayRanging::sendPingFrame(uint8_t destination) {
manumaet 44:2e0045042a59 128 rangingFrame.source = address;
manumaet 44:2e0045042a59 129 rangingFrame.destination = destination;
manumaet 44:2e0045042a59 130 rangingFrame.type = PING;
manumaet 44:2e0045042a59 131 dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame));
manumaet 44:2e0045042a59 132 }
manumaet 44:2e0045042a59 133
manumaet 44:2e0045042a59 134 void MM2WayRanging::sendTransferFrame(uint8_t destination, int timeDiffsReceiver) {
manumaet 44:2e0045042a59 135 transferFrame.source = address;
manumaet 44:2e0045042a59 136 transferFrame.destination = destination;
manumaet 44:2e0045042a59 137 transferFrame.type = TRANSFER_FRAME;
manumaet 44:2e0045042a59 138 transferFrame.signedTime = timeDiffsReceiver; //cast the time difference
manumaet 44:2e0045042a59 139 dw.sendFrame((uint8_t*)&transferFrame, sizeof(transferFrame));
manumaet 44:2e0045042a59 140 }
manumaet 44:2e0045042a59 141
manumaet 44:2e0045042a59 142 void MM2WayRanging::sendDelayedAnswer(uint8_t destination, uint8_t type, uint64_t rxTimestamp) {
manumaet 45:01a33363bc21 143
manumaet 44:2e0045042a59 144 rangingFrame.source = address;
manumaet 44:2e0045042a59 145 rangingFrame.destination = destination;
manumaet 44:2e0045042a59 146 rangingFrame.type = type;
manumaet 45:01a33363bc21 147
manumaet 45:01a33363bc21 148 if(rxTimestamp + ANSWER_DELAY_TIMEUNITS > MMRANGING_2POWER40)
manumaet 45:01a33363bc21 149 dw.sendDelayedFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), rxTimestamp + ANSWER_DELAY_TIMEUNITS - MMRANGING_2POWER40);
manumaet 45:01a33363bc21 150 else
manumaet 45:01a33363bc21 151 dw.sendDelayedFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), rxTimestamp + ANSWER_DELAY_TIMEUNITS);
manumaet 45:01a33363bc21 152 }
manumaet 45:01a33363bc21 153
manumaet 45:01a33363bc21 154 void MM2WayRanging::correctReceiverTimestamps(uint8_t source){
manumaet 45:01a33363bc21 155
manumaet 45:01a33363bc21 156 if(receiverTimestamps[source][0] > receiverTimestamps[source][1]){
manumaet 45:01a33363bc21 157 receiverTimestamps[source][1] += MMRANGING_2POWER40;
manumaet 45:01a33363bc21 158 receiverTimestamps[source][2] += MMRANGING_2POWER40;
manumaet 45:01a33363bc21 159 }
manumaet 45:01a33363bc21 160
manumaet 45:01a33363bc21 161 if(receiverTimestamps[source][1] > receiverTimestamps[source][2]){
manumaet 45:01a33363bc21 162 receiverTimestamps[source][2] += MMRANGING_2POWER40;
manumaet 45:01a33363bc21 163 }
manumaet 45:01a33363bc21 164
manumaet 45:01a33363bc21 165 }
manumaet 45:01a33363bc21 166
manumaet 45:01a33363bc21 167 void MM2WayRanging::correctSenderTimestamps(uint8_t source){
manumaet 45:01a33363bc21 168
manumaet 45:01a33363bc21 169 if (senderTimestamps[source][0] > senderTimestamps[source][1]) {
manumaet 45:01a33363bc21 170 senderTimestamps[source][1] += MMRANGING_2POWER40;
manumaet 45:01a33363bc21 171 senderTimestamps[source][2] += MMRANGING_2POWER40;
manumaet 45:01a33363bc21 172 overflow = true;
manumaet 45:01a33363bc21 173 } else if (senderTimestamps[source][1] > senderTimestamps[source][2]) {
manumaet 45:01a33363bc21 174 senderTimestamps[source][2] += MMRANGING_2POWER40;
manumaet 45:01a33363bc21 175 overflow = true;
manumaet 45:01a33363bc21 176 }else overflow = false;
manumaet 45:01a33363bc21 177
manumaet 45:01a33363bc21 178 }