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@51:e9391d04af00, 2015-11-26 (annotated)

Committer:
bhepp
Date:
Thu Nov 26 22:24:17 2015 +0000
Revision:
51:e9391d04af00
Parent:
50:50b8aea54a51
Child:
52:94688f414dcd
Fixed problems with receiver start and stop.

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