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@46:6398237672a0, 2015-03-10 (annotated)

Committer:
manumaet
Date:
Tue Mar 10 12:11:13 2015 +0000
Revision:
46:6398237672a0
Parent:
45:01a33363bc21
Child:
47:b6120c152ad1
Final commit with altCallbackRX for the testcase and improved power.

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