Matthias Grob & Manuel Stalder / Mbed 2 deprecated DecaWave

This is the DW1000 driver and our self developed distance measurement application based on it. We do this as a semester thesis at ETH Zürich under the Automatic Control Laboratory in the Department of electrical engineering.

Dependencies:   mbed

MM2WayRanging/MM2WayRanging.cpp@45:01a33363bc21, 2015-03-08 (annotated)

Committer:
manumaet
Date:
Sun Mar 08 15:59:14 2015 +0000
Revision:
45:01a33363bc21
Parent:
44:2e0045042a59
Child:
46:6398237672a0
Added initialization values & calibration data.

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 45:01a33363bc21 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 45:01a33363bc21 87 while(!acknowledgement[destination] && (LocalTimer.read() < time_before + 0.5f)); // 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 45:01a33363bc21 99
manumaet 45:01a33363bc21 100 float rawDistance = (tofs[destination] * 300 * TIMEUNITS_TO_US / 4);
manumaet 45:01a33363bc21 101
manumaet 45:01a33363bc21 102 if (this->address == 1) rawDistance+= 10;
manumaet 45:01a33363bc21 103
manumaet 45:01a33363bc21 104 switch(destination){
manumaet 45:01a33363bc21 105 case 2:
manumaet 45:01a33363bc21 106 return rawDistance * 0.9754 - 0.5004;
manumaet 45:01a33363bc21 107 case 3:
manumaet 45:01a33363bc21 108 return rawDistance * 0.9759 - 0.4103;
manumaet 45:01a33363bc21 109 case 4:
manumaet 45:01a33363bc21 110 return rawDistance * 0.9798 - 0.5499;
manumaet 45:01a33363bc21 111 case 5:
manumaet 45:01a33363bc21 112 return rawDistance * 0.9765 - 0.5169;
manumaet 45:01a33363bc21 113 }
manumaet 45:01a33363bc21 114
manumaet 45:01a33363bc21 115
manumaet 44:2e0045042a59 116 }
manumaet 44:2e0045042a59 117
manumaet 44:2e0045042a59 118 void MM2WayRanging::requestRangingAll() {
manumaet 45:01a33363bc21 119 for (int i = 1; i <= 5; i++) { // Request ranging to all anchors
manumaet 44:2e0045042a59 120 requestRanging(i);
manumaet 44:2e0045042a59 121 }
manumaet 44:2e0045042a59 122 }
manumaet 44:2e0045042a59 123
manumaet 44:2e0045042a59 124 void MM2WayRanging::sendPingFrame(uint8_t destination) {
manumaet 44:2e0045042a59 125 rangingFrame.source = address;
manumaet 44:2e0045042a59 126 rangingFrame.destination = destination;
manumaet 44:2e0045042a59 127 rangingFrame.type = PING;
manumaet 44:2e0045042a59 128 dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame));
manumaet 44:2e0045042a59 129 }
manumaet 44:2e0045042a59 130
manumaet 44:2e0045042a59 131 void MM2WayRanging::sendTransferFrame(uint8_t destination, int timeDiffsReceiver) {
manumaet 44:2e0045042a59 132 transferFrame.source = address;
manumaet 44:2e0045042a59 133 transferFrame.destination = destination;
manumaet 44:2e0045042a59 134 transferFrame.type = TRANSFER_FRAME;
manumaet 44:2e0045042a59 135 transferFrame.signedTime = timeDiffsReceiver; //cast the time difference
manumaet 44:2e0045042a59 136 dw.sendFrame((uint8_t*)&transferFrame, sizeof(transferFrame));
manumaet 44:2e0045042a59 137 }
manumaet 44:2e0045042a59 138
manumaet 44:2e0045042a59 139 void MM2WayRanging::sendDelayedAnswer(uint8_t destination, uint8_t type, uint64_t rxTimestamp) {
manumaet 45:01a33363bc21 140
manumaet 44:2e0045042a59 141 rangingFrame.source = address;
manumaet 44:2e0045042a59 142 rangingFrame.destination = destination;
manumaet 44:2e0045042a59 143 rangingFrame.type = type;
manumaet 45:01a33363bc21 144
manumaet 45:01a33363bc21 145 if(rxTimestamp + ANSWER_DELAY_TIMEUNITS > MMRANGING_2POWER40)
manumaet 45:01a33363bc21 146 dw.sendDelayedFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), rxTimestamp + ANSWER_DELAY_TIMEUNITS - MMRANGING_2POWER40);
manumaet 45:01a33363bc21 147 else
manumaet 45:01a33363bc21 148 dw.sendDelayedFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), rxTimestamp + ANSWER_DELAY_TIMEUNITS);
manumaet 45:01a33363bc21 149 }
manumaet 45:01a33363bc21 150
manumaet 45:01a33363bc21 151 void MM2WayRanging::correctReceiverTimestamps(uint8_t source){
manumaet 45:01a33363bc21 152
manumaet 45:01a33363bc21 153 if(receiverTimestamps[source][0] > receiverTimestamps[source][1]){
manumaet 45:01a33363bc21 154 receiverTimestamps[source][1] += MMRANGING_2POWER40;
manumaet 45:01a33363bc21 155 receiverTimestamps[source][2] += MMRANGING_2POWER40;
manumaet 45:01a33363bc21 156 }
manumaet 45:01a33363bc21 157
manumaet 45:01a33363bc21 158 if(receiverTimestamps[source][1] > receiverTimestamps[source][2]){
manumaet 45:01a33363bc21 159 receiverTimestamps[source][2] += MMRANGING_2POWER40;
manumaet 45:01a33363bc21 160 }
manumaet 45:01a33363bc21 161
manumaet 45:01a33363bc21 162 }
manumaet 45:01a33363bc21 163
manumaet 45:01a33363bc21 164 void MM2WayRanging::correctSenderTimestamps(uint8_t source){
manumaet 45:01a33363bc21 165
manumaet 45:01a33363bc21 166 if (senderTimestamps[source][0] > senderTimestamps[source][1]) {
manumaet 45:01a33363bc21 167 senderTimestamps[source][1] += MMRANGING_2POWER40;
manumaet 45:01a33363bc21 168 senderTimestamps[source][2] += MMRANGING_2POWER40;
manumaet 45:01a33363bc21 169 overflow = true;
manumaet 45:01a33363bc21 170 } else if (senderTimestamps[source][1] > senderTimestamps[source][2]) {
manumaet 45:01a33363bc21 171 senderTimestamps[source][2] += MMRANGING_2POWER40;
manumaet 45:01a33363bc21 172 overflow = true;
manumaet 45:01a33363bc21 173 }else overflow = false;
manumaet 45:01a33363bc21 174
manumaet 45:01a33363bc21 175 }