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@58:b824bdac7cd8, 2016-01-04 (annotated)

Committer:
bhepp
Date:
Mon Jan 04 20:55:33 2016 +0000
Revision:
58:b824bdac7cd8
Parent:
53:79a72d752ec4
Child:
67:bd0f0580af5a
Manually setting interrupt flags of DW1000 unit before and after ranging

Who changed what in which revision?

UserRevisionLine numberNew contents of line
manumaet 44:2e0045042a59 1 #include "MM2WayRanging.h"
manumaet 44:2e0045042a59 2
bhepp 51:e9391d04af00 3 #include "PC.h"
bhepp 51:e9391d04af00 4 static PC pc(USBTX, USBRX, 115200); // USB UART Terminal
bhepp 48:5999e510f154 5
manumaet 44:2e0045042a59 6 MM2WayRanging::MM2WayRanging(DW1000& DW) : dw(DW) {
bhepp 48:5999e510f154 7 for (int i = 0; i < 3; ++i) {
bhepp 48:5999e510f154 8 memset(reception_stats[i], 0, sizeof(ReceptionStats));
bhepp 48:5999e510f154 9 }
bhepp 48:5999e510f154 10 memset(roundtriptimes, 0, sizeof(roundtriptimes));
bhepp 48:5999e510f154 11 memset(distances, 0, sizeof(distances));
bhepp 48:5999e510f154 12
manumaet 44:2e0045042a59 13 isAnchor = true;
manumaet 45:01a33363bc21 14 overflow = false;
manumaet 44:2e0045042a59 15 address = 0;
manumaet 44:2e0045042a59 16 rxTimestamp = 0;
manumaet 44:2e0045042a59 17 timediffRec = 0;
manumaet 44:2e0045042a59 18 timediffSend = 0;
manumaet 44:2e0045042a59 19 for (int i = 0; i < 10; i++)
manumaet 44:2e0045042a59 20 acknowledgement[i] = true;
manumaet 44:2e0045042a59 21
manumaet 44:2e0045042a59 22 dw.setCallbacks(this, &MM2WayRanging::callbackRX, &MM2WayRanging::callbackTX);
manumaet 44:2e0045042a59 23
manumaet 45:01a33363bc21 24 LocalTimer.start();
manumaet 44:2e0045042a59 25
bhepp 51:e9391d04af00 26 if (isAnchor) {
bhepp 51:e9391d04af00 27 dw.enable_irq();
bhepp 51:e9391d04af00 28 dw.startRX();
bhepp 58:b824bdac7cd8 29 } else {
bhepp 58:b824bdac7cd8 30 dw.disable_irq();
bhepp 58:b824bdac7cd8 31 dw.setInterrupt(false, false);
bhepp 51:e9391d04af00 32 }
manumaet 44:2e0045042a59 33 }
manumaet 44:2e0045042a59 34
manumaet 44:2e0045042a59 35 void MM2WayRanging::callbackRX() {
bhepp 48:5999e510f154 36 //pc.printf("Received frame\r\n");
manumaet 44:2e0045042a59 37 dw.readRegister(DW1000_RX_BUFFER, 0, (uint8_t*)&receivedFrame, dw.getFramelength());
manumaet 44:2e0045042a59 38
bhepp 48:5999e510f154 39 if (receivedFrame.remote_address == address) {
bhepp 48:5999e510f154 40 uint16_t std_noise = dw.getStdNoise();
bhepp 48:5999e510f154 41 uint16_t preamble_acc_count = dw.getPACC();
bhepp 48:5999e510f154 42 uint16_t first_path_index = dw.getFPINDEX();
bhepp 48:5999e510f154 43 uint16_t first_path_amp_1 = dw.getFPAMPL1();
bhepp 48:5999e510f154 44 uint16_t first_path_amp_2 = dw.getFPAMPL2();
bhepp 48:5999e510f154 45 uint16_t first_path_amp_3 = dw.getFPAMPL3();
bhepp 48:5999e510f154 46 uint16_t channel_impulse_response_power = dw.getCIRPWR();
bhepp 48:5999e510f154 47 uint8_t prf = dw.getPRF();
manumaet 44:2e0045042a59 48 switch (receivedFrame.type) {
manumaet 44:2e0045042a59 49 case PING:
manumaet 44:2e0045042a59 50 rxTimestamp = dw.getRXTimestamp();
bhepp 48:5999e510f154 51 receiverTimestamps[receivedFrame.address][0] = rxTimestamp; //Save the first timestamp on the receiving node/anchor (T_rp)
bhepp 48:5999e510f154 52 sendDelayedAnswer(receivedFrame.address, ANCHOR_RESPONSE, rxTimestamp);
bhepp 48:5999e510f154 53 reception_stats[receivedFrame.address][0].std_noise = std_noise;
bhepp 48:5999e510f154 54 reception_stats[receivedFrame.address][0].preamble_acc_count = preamble_acc_count;
bhepp 48:5999e510f154 55 reception_stats[receivedFrame.address][0].first_path_index = first_path_index;
bhepp 48:5999e510f154 56 reception_stats[receivedFrame.address][0].first_path_amp_1 = first_path_amp_1;
bhepp 48:5999e510f154 57 reception_stats[receivedFrame.address][0].first_path_amp_2 = first_path_amp_2;
bhepp 48:5999e510f154 58 reception_stats[receivedFrame.address][0].first_path_amp_3 = first_path_amp_3;
bhepp 48:5999e510f154 59 reception_stats[receivedFrame.address][0].channel_impulse_response_power = channel_impulse_response_power;
bhepp 48:5999e510f154 60 reception_stats[receivedFrame.address][0].prf = prf;
bhepp 48:5999e510f154 61 //pc.printf("Received ping. Noise 0x%0x %d\r\n", std_noise, prf);
manumaet 44:2e0045042a59 62 break;
manumaet 44:2e0045042a59 63 case ANCHOR_RESPONSE:
manumaet 44:2e0045042a59 64 rxTimestamp = dw.getRXTimestamp();
bhepp 48:5999e510f154 65 senderTimestamps[receivedFrame.address][1] = rxTimestamp; //Save the second timestamp on the sending node/beacon (T_rr)
bhepp 48:5999e510f154 66 sendDelayedAnswer(receivedFrame.address, 3, rxTimestamp);
bhepp 48:5999e510f154 67 //pc.printf("Received anchor response\r\n");
bhepp 48:5999e510f154 68 reception_stats[receivedFrame.address][1].std_noise = std_noise;
bhepp 48:5999e510f154 69 reception_stats[receivedFrame.address][1].preamble_acc_count = preamble_acc_count;
bhepp 48:5999e510f154 70 reception_stats[receivedFrame.address][1].first_path_index = first_path_index;
bhepp 48:5999e510f154 71 reception_stats[receivedFrame.address][1].first_path_amp_1 = first_path_amp_1;
bhepp 48:5999e510f154 72 reception_stats[receivedFrame.address][1].first_path_amp_2 = first_path_amp_2;
bhepp 48:5999e510f154 73 reception_stats[receivedFrame.address][1].first_path_amp_3 = first_path_amp_3;
bhepp 48:5999e510f154 74 reception_stats[receivedFrame.address][1].channel_impulse_response_power = channel_impulse_response_power;
bhepp 48:5999e510f154 75 reception_stats[receivedFrame.address][1].prf = prf;
manumaet 44:2e0045042a59 76 break;
manumaet 44:2e0045042a59 77 case BEACON_RESPONSE:
manumaet 44:2e0045042a59 78 rxTimestamp = dw.getRXTimestamp();
bhepp 48:5999e510f154 79 receiverTimestamps[receivedFrame.address][2] = rxTimestamp; //Save the third timestamp on the receiving node/anchor (T_rf)
manumaet 45:01a33363bc21 80
bhepp 48:5999e510f154 81 correctReceiverTimestamps(receivedFrame.address); //Correct the timestamps for the case of a counter overflow
manumaet 44:2e0045042a59 82 //calculation of the summand on the receiving node/anchor
bhepp 48:5999e510f154 83 timediffRec = - 2*receiverTimestamps[receivedFrame.address][1] + receiverTimestamps[receivedFrame.address][0] + receiverTimestamps[receivedFrame.address][2];
bhepp 48:5999e510f154 84 reception_stats[receivedFrame.address][2].std_noise = std_noise;
bhepp 48:5999e510f154 85 reception_stats[receivedFrame.address][2].preamble_acc_count = preamble_acc_count;
bhepp 48:5999e510f154 86 reception_stats[receivedFrame.address][2].first_path_index = first_path_index;
bhepp 48:5999e510f154 87 reception_stats[receivedFrame.address][2].first_path_amp_1 = first_path_amp_1;
bhepp 48:5999e510f154 88 reception_stats[receivedFrame.address][2].first_path_amp_2 = first_path_amp_2;
bhepp 48:5999e510f154 89 reception_stats[receivedFrame.address][2].first_path_amp_3 = first_path_amp_3;
bhepp 48:5999e510f154 90 reception_stats[receivedFrame.address][2].channel_impulse_response_power = channel_impulse_response_power;
bhepp 48:5999e510f154 91 reception_stats[receivedFrame.address][2].prf = prf;
bhepp 48:5999e510f154 92 sendTransferFrame(receivedFrame.address, timediffRec);
bhepp 48:5999e510f154 93 //pc.printf("%x %d\r\n", noise_levels[receivedFrame.address][0]);
bhepp 48:5999e510f154 94 //pc.printf("%x %d\r\n", noise_levels[receivedFrame.address][1]);
bhepp 48:5999e510f154 95 //pc.printf("%x %d\r\n", noise_levels[receivedFrame.address][2]);
bhepp 48:5999e510f154 96 //pc.printf("Received beacon response. Noise %d\r\n", noise_level);
manumaet 44:2e0045042a59 97 break;
manumaet 44:2e0045042a59 98 case TRANSFER_FRAME:
manumaet 44:2e0045042a59 99 //calculation of the summand on the sending node/beacon
bhepp 48:5999e510f154 100 timediffSend = 2 * senderTimestamps[receivedFrame.address][1] - senderTimestamps[receivedFrame.address][0] - senderTimestamps[receivedFrame.address][2];
manumaet 44:2e0045042a59 101 //calculation of the resulting sum of all four ToFs.
bhepp 48:5999e510f154 102 tofs[receivedFrame.address] = receivedFrame.signedTime + timediffSend;
bhepp 48:5999e510f154 103 acknowledgement[receivedFrame.address] = true;
bhepp 48:5999e510f154 104 memcpy(&reception_stats[receivedFrame.address][0], &receivedFrame.stats1, sizeof(ReceptionStats));
bhepp 48:5999e510f154 105 memcpy(&reception_stats[receivedFrame.address][2], &receivedFrame.stats2, sizeof(ReceptionStats));
bhepp 48:5999e510f154 106 //reception_stats[receivedFrame.address][0].std_noise = receivedFrame.stats1.std_noise;
bhepp 48:5999e510f154 107 //reception_stats[receivedFrame.address][2].std_noise = receivedFrame.stats2.std_noise;
bhepp 48:5999e510f154 108 //noise_levels[receivedFrame.address][0] = receivedFrame.std_noise1;
bhepp 48:5999e510f154 109 //noise_levels[receivedFrame.address][2] = receivedFrame.std_noise2;
bhepp 48:5999e510f154 110 //pc.printf("Received transfer frame\r\n");
manumaet 44:2e0045042a59 111 break;
manumaet 44:2e0045042a59 112 default : break;
manumaet 44:2e0045042a59 113 }
bhepp 48:5999e510f154 114 }
manumaet 46:6398237672a0 115
manumaet 44:2e0045042a59 116 dw.startRX();
manumaet 44:2e0045042a59 117 }
manumaet 44:2e0045042a59 118
manumaet 44:2e0045042a59 119 void MM2WayRanging::callbackTX() {
bhepp 48:5999e510f154 120 //pc.printf("Sent frame\r\n");
manumaet 44:2e0045042a59 121 switch (rangingFrame.type) {
manumaet 44:2e0045042a59 122 case PING:
bhepp 48:5999e510f154 123 senderTimestamps[rangingFrame.remote_address][0] = dw.getTXTimestamp(); //Save the first timestamp on the sending node/beacon (T_sp)
bhepp 48:5999e510f154 124 //pc.printf("Sent ping\r\n");
manumaet 44:2e0045042a59 125 break;
manumaet 44:2e0045042a59 126 case ANCHOR_RESPONSE:
bhepp 48:5999e510f154 127 receiverTimestamps[rangingFrame.remote_address][1] = dw.getTXTimestamp(); //Save the second timestamp on the receiving node/anchor (T_sr)
bhepp 48:5999e510f154 128 //pc.printf("Sent anchor response\r\n");
manumaet 44:2e0045042a59 129 break;
manumaet 44:2e0045042a59 130 case BEACON_RESPONSE:
bhepp 48:5999e510f154 131 senderTimestamps[rangingFrame.remote_address][2] = dw.getTXTimestamp(); //Save the third timestamp on the sending node/beacon (T_sr)
bhepp 48:5999e510f154 132 correctSenderTimestamps(rangingFrame.remote_address); //Correct the timestamps for the case of a counter overflow
bhepp 48:5999e510f154 133 //pc.printf("Sent beacon response\r\n");
manumaet 44:2e0045042a59 134 break;
manumaet 44:2e0045042a59 135 default:
manumaet 44:2e0045042a59 136 break;
manumaet 44:2e0045042a59 137 }
manumaet 45:01a33363bc21 138
manumaet 44:2e0045042a59 139 }
manumaet 44:2e0045042a59 140
manumaet 44:2e0045042a59 141 /**
bhepp 48:5999e510f154 142 * Get the distance to the Anchor with address @param remote_address.
manumaet 44:2e0045042a59 143 *
bhepp 48:5999e510f154 144 * @param remote_address The address of the anchor
manumaet 44:2e0045042a59 145 */
bhepp 48:5999e510f154 146 void MM2WayRanging::requestRanging(uint8_t remote_address) {
bhepp 52:94688f414dcd 147 if (remote_address == address) {
bhepp 52:94688f414dcd 148 distances[remote_address] = -2;
bhepp 52:94688f414dcd 149 return;
bhepp 52:94688f414dcd 150 }
bhepp 52:94688f414dcd 151
bhepp 58:b824bdac7cd8 152 dw.setInterrupt(true, true);
bhepp 48:5999e510f154 153 // pc.printf("Request range %d\r\n", remote_address);
bhepp 48:5999e510f154 154 dw.enable_irq();
naegelit 53:79a72d752ec4 155
naegelit 53:79a72d752ec4 156 dw.startRX();
naegelit 53:79a72d752ec4 157 //// pc.printf("Enabled IRQ %d\r\n", remote_address);
bhepp 48:5999e510f154 158 acknowledgement[remote_address] = false;
manumaet 44:2e0045042a59 159 float time_before = LocalTimer.read();
naegelit 53:79a72d752ec4 160 //
naegelit 53:79a72d752ec4 161 //// pc.printf("Sending ping\r\n");
bhepp 48:5999e510f154 162 sendPingFrame(remote_address);
naegelit 53:79a72d752ec4 163 //
naegelit 53:79a72d752ec4 164 // pc.printf("Waiting for ack\r\n");
naegelit 53:79a72d752ec4 165
bhepp 50:50b8aea54a51 166 while(!acknowledgement[remote_address] && (LocalTimer.read() < time_before + 0.3f)); // wait for succeeding ranging or timeout
manumaet 45:01a33363bc21 167
bhepp 48:5999e510f154 168 roundtriptimes[remote_address] = LocalTimer.read() - time_before;
bhepp 48:5999e510f154 169
bhepp 48:5999e510f154 170 if(acknowledgement[remote_address]){
bhepp 48:5999e510f154 171 distances[remote_address] = calibratedDistance(remote_address);
manumaet 45:01a33363bc21 172 } else {
bhepp 48:5999e510f154 173 distances[remote_address] = -1;
manumaet 45:01a33363bc21 174 }
bhepp 50:50b8aea54a51 175 dw.stopTRX();
bhepp 48:5999e510f154 176 dw.disable_irq();
bhepp 58:b824bdac7cd8 177 dw.setInterrupt(false, false);
manumaet 45:01a33363bc21 178 }
manumaet 45:01a33363bc21 179
bhepp 48:5999e510f154 180 inline float MM2WayRanging::calibratedDistance(uint8_t remote_address) {
manumaet 46:6398237672a0 181
bhepp 48:5999e510f154 182 float rawDistance = (tofs[remote_address] * 300 * TIMEUNITS_TO_US / 4);
manumaet 46:6398237672a0 183
manumaet 46:6398237672a0 184
manumaet 46:6398237672a0 185
manumaet 46:6398237672a0 186 // Calibration for Nucleo 0 (and 1)
manumaet 46:6398237672a0 187
manumaet 46:6398237672a0 188 // if (this->address == 1) rawDistance+= 10;
bhepp 48:5999e510f154 189 // switch(remote_address){
manumaet 46:6398237672a0 190 // case 2:
manumaet 46:6398237672a0 191 // return rawDistance * 0.9754 - 0.5004;
manumaet 46:6398237672a0 192 // case 3:
manumaet 46:6398237672a0 193 // return rawDistance * 0.9759 - 0.4103;
manumaet 46:6398237672a0 194 // case 4:
manumaet 46:6398237672a0 195 // return rawDistance * 0.9798 - 0.5499;
manumaet 46:6398237672a0 196 // case 5:
manumaet 46:6398237672a0 197 // return rawDistance * 0.9765 - 0.5169;
manumaet 46:6398237672a0 198 // }
manumaet 46:6398237672a0 199
manumaet 46:6398237672a0 200 return rawDistance;
manumaet 46:6398237672a0 201
manumaet 44:2e0045042a59 202 }
manumaet 44:2e0045042a59 203
bhepp 48:5999e510f154 204 void MM2WayRanging::sendPingFrame(uint8_t remote_address) {
bhepp 48:5999e510f154 205 rangingFrame.address = address;
bhepp 48:5999e510f154 206 rangingFrame.remote_address = remote_address;
manumaet 44:2e0045042a59 207 rangingFrame.type = PING;
manumaet 44:2e0045042a59 208 dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame));
manumaet 44:2e0045042a59 209 }
manumaet 44:2e0045042a59 210
bhepp 48:5999e510f154 211 void MM2WayRanging::sendTransferFrame(uint8_t remote_address, int timeDiffsReceiver) {
bhepp 48:5999e510f154 212 transferFrame.address = address;
bhepp 48:5999e510f154 213 transferFrame.remote_address = remote_address;
manumaet 44:2e0045042a59 214 transferFrame.type = TRANSFER_FRAME;
manumaet 44:2e0045042a59 215 transferFrame.signedTime = timeDiffsReceiver; //cast the time difference
bhepp 48:5999e510f154 216 memcpy(&transferFrame.stats1, &reception_stats[receivedFrame.address][0], sizeof(ReceptionStats));
bhepp 48:5999e510f154 217 memcpy(&transferFrame.stats2, &reception_stats[receivedFrame.address][2], sizeof(ReceptionStats));
manumaet 44:2e0045042a59 218 dw.sendFrame((uint8_t*)&transferFrame, sizeof(transferFrame));
manumaet 44:2e0045042a59 219 }
manumaet 44:2e0045042a59 220
bhepp 48:5999e510f154 221 void MM2WayRanging::sendDelayedAnswer(uint8_t remote_address, uint8_t type, uint64_t rxTimestamp) {
manumaet 45:01a33363bc21 222
bhepp 48:5999e510f154 223 rangingFrame.address = address;
bhepp 48:5999e510f154 224 rangingFrame.remote_address = remote_address;
manumaet 44:2e0045042a59 225 rangingFrame.type = type;
manumaet 45:01a33363bc21 226
manumaet 45:01a33363bc21 227 if(rxTimestamp + ANSWER_DELAY_TIMEUNITS > MMRANGING_2POWER40)
manumaet 45:01a33363bc21 228 dw.sendDelayedFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), rxTimestamp + ANSWER_DELAY_TIMEUNITS - MMRANGING_2POWER40);
manumaet 45:01a33363bc21 229 else
manumaet 45:01a33363bc21 230 dw.sendDelayedFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), rxTimestamp + ANSWER_DELAY_TIMEUNITS);
manumaet 45:01a33363bc21 231 }
manumaet 45:01a33363bc21 232
manumaet 45:01a33363bc21 233 void MM2WayRanging::correctReceiverTimestamps(uint8_t source){
manumaet 45:01a33363bc21 234
manumaet 45:01a33363bc21 235 if(receiverTimestamps[source][0] > receiverTimestamps[source][1]){
manumaet 45:01a33363bc21 236 receiverTimestamps[source][1] += MMRANGING_2POWER40;
manumaet 45:01a33363bc21 237 receiverTimestamps[source][2] += MMRANGING_2POWER40;
manumaet 45:01a33363bc21 238 }
manumaet 45:01a33363bc21 239
manumaet 45:01a33363bc21 240 if(receiverTimestamps[source][1] > receiverTimestamps[source][2]){
manumaet 45:01a33363bc21 241 receiverTimestamps[source][2] += MMRANGING_2POWER40;
manumaet 45:01a33363bc21 242 }
manumaet 45:01a33363bc21 243
manumaet 45:01a33363bc21 244 }
manumaet 45:01a33363bc21 245
manumaet 45:01a33363bc21 246 void MM2WayRanging::correctSenderTimestamps(uint8_t source){
manumaet 45:01a33363bc21 247
manumaet 45:01a33363bc21 248 if (senderTimestamps[source][0] > senderTimestamps[source][1]) {
manumaet 45:01a33363bc21 249 senderTimestamps[source][1] += MMRANGING_2POWER40;
manumaet 45:01a33363bc21 250 senderTimestamps[source][2] += MMRANGING_2POWER40;
manumaet 45:01a33363bc21 251 overflow = true;
manumaet 45:01a33363bc21 252 } else if (senderTimestamps[source][1] > senderTimestamps[source][2]) {
manumaet 45:01a33363bc21 253 senderTimestamps[source][2] += MMRANGING_2POWER40;
manumaet 45:01a33363bc21 254 overflow = true;
manumaet 45:01a33363bc21 255 }else overflow = false;
manumaet 45:01a33363bc21 256
manumaet 45:01a33363bc21 257 }