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@53:79a72d752ec4, 2016-01-04 (annotated)

Committer:
naegelit
Date:
Mon Jan 04 12:45:37 2016 +0000
Revision:
53:79a72d752ec4
Parent:
52:94688f414dcd
Child:
58:b824bdac7cd8
i hope not commiting into bennis branch ;-)

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 52:94688f414dcd 144 if (remote_address == address) {
bhepp 52:94688f414dcd 145 distances[remote_address] = -2;
bhepp 52:94688f414dcd 146 return;
bhepp 52:94688f414dcd 147 }
bhepp 52:94688f414dcd 148
bhepp 48:5999e510f154 149 // pc.printf("Request range %d\r\n", remote_address);
bhepp 48:5999e510f154 150 dw.enable_irq();
naegelit 53:79a72d752ec4 151
naegelit 53:79a72d752ec4 152 dw.startRX();
naegelit 53:79a72d752ec4 153 //// pc.printf("Enabled IRQ %d\r\n", remote_address);
bhepp 48:5999e510f154 154 acknowledgement[remote_address] = false;
manumaet 44:2e0045042a59 155 float time_before = LocalTimer.read();
naegelit 53:79a72d752ec4 156 //
naegelit 53:79a72d752ec4 157 //// pc.printf("Sending ping\r\n");
bhepp 48:5999e510f154 158 sendPingFrame(remote_address);
naegelit 53:79a72d752ec4 159 //
naegelit 53:79a72d752ec4 160 // pc.printf("Waiting for ack\r\n");
naegelit 53:79a72d752ec4 161
bhepp 50:50b8aea54a51 162 while(!acknowledgement[remote_address] && (LocalTimer.read() < time_before + 0.3f)); // wait for succeeding ranging or timeout
manumaet 45:01a33363bc21 163
bhepp 48:5999e510f154 164 roundtriptimes[remote_address] = LocalTimer.read() - time_before;
bhepp 48:5999e510f154 165
bhepp 48:5999e510f154 166 if(acknowledgement[remote_address]){
bhepp 48:5999e510f154 167 distances[remote_address] = calibratedDistance(remote_address);
manumaet 45:01a33363bc21 168 } else {
bhepp 48:5999e510f154 169 distances[remote_address] = -1;
manumaet 45:01a33363bc21 170 }
bhepp 50:50b8aea54a51 171 dw.stopTRX();
bhepp 48:5999e510f154 172 dw.disable_irq();
manumaet 45:01a33363bc21 173 }
manumaet 45:01a33363bc21 174
bhepp 48:5999e510f154 175 inline float MM2WayRanging::calibratedDistance(uint8_t remote_address) {
manumaet 46:6398237672a0 176
bhepp 48:5999e510f154 177 float rawDistance = (tofs[remote_address] * 300 * TIMEUNITS_TO_US / 4);
manumaet 46:6398237672a0 178
manumaet 46:6398237672a0 179
manumaet 46:6398237672a0 180
manumaet 46:6398237672a0 181 // Calibration for Nucleo 0 (and 1)
manumaet 46:6398237672a0 182
manumaet 46:6398237672a0 183 // if (this->address == 1) rawDistance+= 10;
bhepp 48:5999e510f154 184 // switch(remote_address){
manumaet 46:6398237672a0 185 // case 2:
manumaet 46:6398237672a0 186 // return rawDistance * 0.9754 - 0.5004;
manumaet 46:6398237672a0 187 // case 3:
manumaet 46:6398237672a0 188 // return rawDistance * 0.9759 - 0.4103;
manumaet 46:6398237672a0 189 // case 4:
manumaet 46:6398237672a0 190 // return rawDistance * 0.9798 - 0.5499;
manumaet 46:6398237672a0 191 // case 5:
manumaet 46:6398237672a0 192 // return rawDistance * 0.9765 - 0.5169;
manumaet 46:6398237672a0 193 // }
manumaet 46:6398237672a0 194
manumaet 46:6398237672a0 195 return rawDistance;
manumaet 46:6398237672a0 196
manumaet 44:2e0045042a59 197 }
manumaet 44:2e0045042a59 198
bhepp 48:5999e510f154 199 void MM2WayRanging::sendPingFrame(uint8_t remote_address) {
bhepp 48:5999e510f154 200 rangingFrame.address = address;
bhepp 48:5999e510f154 201 rangingFrame.remote_address = remote_address;
manumaet 44:2e0045042a59 202 rangingFrame.type = PING;
manumaet 44:2e0045042a59 203 dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame));
manumaet 44:2e0045042a59 204 }
manumaet 44:2e0045042a59 205
bhepp 48:5999e510f154 206 void MM2WayRanging::sendTransferFrame(uint8_t remote_address, int timeDiffsReceiver) {
bhepp 48:5999e510f154 207 transferFrame.address = address;
bhepp 48:5999e510f154 208 transferFrame.remote_address = remote_address;
manumaet 44:2e0045042a59 209 transferFrame.type = TRANSFER_FRAME;
manumaet 44:2e0045042a59 210 transferFrame.signedTime = timeDiffsReceiver; //cast the time difference
bhepp 48:5999e510f154 211 memcpy(&transferFrame.stats1, &reception_stats[receivedFrame.address][0], sizeof(ReceptionStats));
bhepp 48:5999e510f154 212 memcpy(&transferFrame.stats2, &reception_stats[receivedFrame.address][2], sizeof(ReceptionStats));
manumaet 44:2e0045042a59 213 dw.sendFrame((uint8_t*)&transferFrame, sizeof(transferFrame));
manumaet 44:2e0045042a59 214 }
manumaet 44:2e0045042a59 215
bhepp 48:5999e510f154 216 void MM2WayRanging::sendDelayedAnswer(uint8_t remote_address, uint8_t type, uint64_t rxTimestamp) {
manumaet 45:01a33363bc21 217
bhepp 48:5999e510f154 218 rangingFrame.address = address;
bhepp 48:5999e510f154 219 rangingFrame.remote_address = remote_address;
manumaet 44:2e0045042a59 220 rangingFrame.type = type;
manumaet 45:01a33363bc21 221
manumaet 45:01a33363bc21 222 if(rxTimestamp + ANSWER_DELAY_TIMEUNITS > MMRANGING_2POWER40)
manumaet 45:01a33363bc21 223 dw.sendDelayedFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), rxTimestamp + ANSWER_DELAY_TIMEUNITS - MMRANGING_2POWER40);
manumaet 45:01a33363bc21 224 else
manumaet 45:01a33363bc21 225 dw.sendDelayedFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), rxTimestamp + ANSWER_DELAY_TIMEUNITS);
manumaet 45:01a33363bc21 226 }
manumaet 45:01a33363bc21 227
manumaet 45:01a33363bc21 228 void MM2WayRanging::correctReceiverTimestamps(uint8_t source){
manumaet 45:01a33363bc21 229
manumaet 45:01a33363bc21 230 if(receiverTimestamps[source][0] > receiverTimestamps[source][1]){
manumaet 45:01a33363bc21 231 receiverTimestamps[source][1] += MMRANGING_2POWER40;
manumaet 45:01a33363bc21 232 receiverTimestamps[source][2] += MMRANGING_2POWER40;
manumaet 45:01a33363bc21 233 }
manumaet 45:01a33363bc21 234
manumaet 45:01a33363bc21 235 if(receiverTimestamps[source][1] > receiverTimestamps[source][2]){
manumaet 45:01a33363bc21 236 receiverTimestamps[source][2] += MMRANGING_2POWER40;
manumaet 45:01a33363bc21 237 }
manumaet 45:01a33363bc21 238
manumaet 45:01a33363bc21 239 }
manumaet 45:01a33363bc21 240
manumaet 45:01a33363bc21 241 void MM2WayRanging::correctSenderTimestamps(uint8_t source){
manumaet 45:01a33363bc21 242
manumaet 45:01a33363bc21 243 if (senderTimestamps[source][0] > senderTimestamps[source][1]) {
manumaet 45:01a33363bc21 244 senderTimestamps[source][1] += MMRANGING_2POWER40;
manumaet 45:01a33363bc21 245 senderTimestamps[source][2] += MMRANGING_2POWER40;
manumaet 45:01a33363bc21 246 overflow = true;
manumaet 45:01a33363bc21 247 } else if (senderTimestamps[source][1] > senderTimestamps[source][2]) {
manumaet 45:01a33363bc21 248 senderTimestamps[source][2] += MMRANGING_2POWER40;
manumaet 45:01a33363bc21 249 overflow = true;
manumaet 45:01a33363bc21 250 }else overflow = false;
manumaet 45:01a33363bc21 251
manumaet 45:01a33363bc21 252 }