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@48:5999e510f154, 2015-11-24 (annotated)

Committer:
bhepp
Date:
Tue Nov 24 16:41:23 2015 +0000
Revision:
48:5999e510f154
Parent:
47:b6120c152ad1
Child:
50:50b8aea54a51
Multiple Receivers implemented

Who changed what in which revision?

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