Alejandro Ungria Hirte
/
GA-Test_copy
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
nodes/nodes.cpp@0:a3b83d366423, 2017-12-06 (annotated)
- Committer:
- aungriah
- Date:
- Wed Dec 06 21:35:45 2017 +0000
- Revision:
- 0:a3b83d366423
test
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| aungriah | 0:a3b83d366423 | 1 | #include "nodes.h" |
| aungriah | 0:a3b83d366423 | 2 | |
| aungriah | 0:a3b83d366423 | 3 | extern Node *node; |
| aungriah | 0:a3b83d366423 | 4 | extern BeaconNode beaconNode; |
| aungriah | 0:a3b83d366423 | 5 | extern AnchorNode anchorNode; |
| aungriah | 0:a3b83d366423 | 6 | extern BaseStationNode baseStationNode; |
| aungriah | 0:a3b83d366423 | 7 | |
| aungriah | 0:a3b83d366423 | 8 | Node::Node(DecaWave& DW) : dw(DW) { |
| aungriah | 0:a3b83d366423 | 9 | address = 0; |
| aungriah | 0:a3b83d366423 | 10 | } |
| aungriah | 0:a3b83d366423 | 11 | void Node::setAnchor(bool anc){ |
| aungriah | 0:a3b83d366423 | 12 | Anchor = anc; |
| aungriah | 0:a3b83d366423 | 13 | } |
| aungriah | 0:a3b83d366423 | 14 | |
| aungriah | 0:a3b83d366423 | 15 | bool Node::isAnchor(){ |
| aungriah | 0:a3b83d366423 | 16 | return Anchor; |
| aungriah | 0:a3b83d366423 | 17 | } |
| aungriah | 0:a3b83d366423 | 18 | bool Node::isBaseStation(){ |
| aungriah | 0:a3b83d366423 | 19 | return address == BASE_STATION_ADDR; |
| aungriah | 0:a3b83d366423 | 20 | } |
| aungriah | 0:a3b83d366423 | 21 | void Node::setAddress(uint8_t adr){ |
| aungriah | 0:a3b83d366423 | 22 | address = adr; |
| aungriah | 0:a3b83d366423 | 23 | } |
| aungriah | 0:a3b83d366423 | 24 | |
| aungriah | 0:a3b83d366423 | 25 | uint8_t Node::getAddress(){ |
| aungriah | 0:a3b83d366423 | 26 | return address; |
| aungriah | 0:a3b83d366423 | 27 | } |
| aungriah | 0:a3b83d366423 | 28 | |
| aungriah | 0:a3b83d366423 | 29 | |
| aungriah | 0:a3b83d366423 | 30 | |
| aungriah | 0:a3b83d366423 | 31 | //------- BeaconNode Class ------------------- |
| aungriah | 0:a3b83d366423 | 32 | BeaconNode::BeaconNode(DecaWave& DW) : Node(DW) { |
| aungriah | 0:a3b83d366423 | 33 | Anchor = false; |
| aungriah | 0:a3b83d366423 | 34 | for (int i = 0; i < 3; i++) |
| aungriah | 0:a3b83d366423 | 35 | acknowledgement[i] = true; |
| aungriah | 0:a3b83d366423 | 36 | LocalTimer.start(); |
| aungriah | 0:a3b83d366423 | 37 | mode=0; |
| aungriah | 0:a3b83d366423 | 38 | } |
| aungriah | 0:a3b83d366423 | 39 | |
| aungriah | 0:a3b83d366423 | 40 | #pragma Otime // Compiler optimize Runtime at the cost of image size |
| aungriah | 0:a3b83d366423 | 41 | // Two Way Ranging Actions when Frame was received |
| aungriah | 0:a3b83d366423 | 42 | void BeaconNode::callbackRX(uint64_t RxTime) { |
| aungriah | 0:a3b83d366423 | 43 | //pc.printf("Got a frame, adress: %d source: %d \r\n", receivedFrame.destination, receivedFrame.source); |
| aungriah | 0:a3b83d366423 | 44 | if (receivedFrame.destination == address){ |
| aungriah | 0:a3b83d366423 | 45 | switch (receivedFrame.type) { |
| aungriah | 0:a3b83d366423 | 46 | case SWITCH_TYPE: |
| aungriah | 0:a3b83d366423 | 47 | sendBaseAck(); |
| aungriah | 0:a3b83d366423 | 48 | if((receivedFrame.signedTime >> 24) == BECOME_BEACON){ |
| aungriah | 0:a3b83d366423 | 49 | pc.printf("\r\n \r\n ---This Node is already a beacon ---\r\n \r\n"); |
| aungriah | 0:a3b83d366423 | 50 | } |
| aungriah | 0:a3b83d366423 | 51 | else if((receivedFrame.signedTime >> 24) == BECOME_ANCHOR){ |
| aungriah | 0:a3b83d366423 | 52 | node = &anchorNode; |
| aungriah | 0:a3b83d366423 | 53 | pc.printf("\r\n \r\n ---This Node is an anchor now ---\r\n \r\n"); |
| aungriah | 0:a3b83d366423 | 54 | } |
| aungriah | 0:a3b83d366423 | 55 | |
| aungriah | 0:a3b83d366423 | 56 | break; |
| aungriah | 0:a3b83d366423 | 57 | case BASE_ORDER: |
| aungriah | 0:a3b83d366423 | 58 | repetitions = receivedFrame.signedTime&0xffff; |
| aungriah | 0:a3b83d366423 | 59 | mode = receivedFrame.signedTime >> 24; |
| aungriah | 0:a3b83d366423 | 60 | destination = (receivedFrame.signedTime&0xff0000) >> 16; |
| aungriah | 0:a3b83d366423 | 61 | sendBaseAck(); |
| aungriah | 0:a3b83d366423 | 62 | break; |
| aungriah | 0:a3b83d366423 | 63 | case ANCHOR_RESPONSE: |
| aungriah | 0:a3b83d366423 | 64 | { |
| aungriah | 0:a3b83d366423 | 65 | sendAnswer(receivedFrame.source, BEACON_RESPONSE); |
| aungriah | 0:a3b83d366423 | 66 | senderTimestamps[receivedFrame.source][1] = RxTime; //Save the second timestamp on the sending node/beacon (T_rr) |
| aungriah | 0:a3b83d366423 | 67 | acknowledgement[0] = true; |
| aungriah | 0:a3b83d366423 | 68 | |
| aungriah | 0:a3b83d366423 | 69 | // Get Signal Level |
| aungriah | 0:a3b83d366423 | 70 | dwt_rxdiag_t diagnostic; |
| aungriah | 0:a3b83d366423 | 71 | dwt_readdiagnostics(&diagnostic); |
| aungriah | 0:a3b83d366423 | 72 | signalStrength[receivedFrame.source] = dw.getRXLevel(&diagnostic); |
| aungriah | 0:a3b83d366423 | 73 | |
| aungriah | 0:a3b83d366423 | 74 | break; |
| aungriah | 0:a3b83d366423 | 75 | } |
| aungriah | 0:a3b83d366423 | 76 | case TRANSFER_FRAME: |
| aungriah | 0:a3b83d366423 | 77 | { |
| aungriah | 0:a3b83d366423 | 78 | //timediffSend = 2 * senderTimestamps[receivedFrame.source][1] - senderTimestamps[receivedFrame.source][0] - senderTimestamps[receivedFrame.source][2]; |
| aungriah | 0:a3b83d366423 | 79 | tofs[receivedFrame.source] = receivedFrame.signedTime + 2 * senderTimestamps[receivedFrame.source][1] - senderTimestamps[receivedFrame.source][0] - senderTimestamps[receivedFrame.source][2]; |
| aungriah | 0:a3b83d366423 | 80 | |
| aungriah | 0:a3b83d366423 | 81 | // Get Signal Level |
| aungriah | 0:a3b83d366423 | 82 | dwt_rxdiag_t diagnostic2; |
| aungriah | 0:a3b83d366423 | 83 | dwt_readdiagnostics(&diagnostic2); |
| aungriah | 0:a3b83d366423 | 84 | float level = dw.getRXLevel(&diagnostic2); |
| aungriah | 0:a3b83d366423 | 85 | if(level < signalStrength[receivedFrame.source]) |
| aungriah | 0:a3b83d366423 | 86 | signalStrength[receivedFrame.source] = level; |
| aungriah | 0:a3b83d366423 | 87 | |
| aungriah | 0:a3b83d366423 | 88 | acknowledgement[1] = true; |
| aungriah | 0:a3b83d366423 | 89 | |
| aungriah | 0:a3b83d366423 | 90 | //dw.turnonrx(); // start listening again |
| aungriah | 0:a3b83d366423 | 91 | break; |
| aungriah | 0:a3b83d366423 | 92 | } |
| aungriah | 0:a3b83d366423 | 93 | default : break; |
| aungriah | 0:a3b83d366423 | 94 | } |
| aungriah | 0:a3b83d366423 | 95 | } |
| aungriah | 0:a3b83d366423 | 96 | else{ |
| aungriah | 0:a3b83d366423 | 97 | dw.turnonrx(); // start listening again |
| aungriah | 0:a3b83d366423 | 98 | } |
| aungriah | 0:a3b83d366423 | 99 | } |
| aungriah | 0:a3b83d366423 | 100 | |
| aungriah | 0:a3b83d366423 | 101 | |
| aungriah | 0:a3b83d366423 | 102 | #pragma Otime // Compiler optimize Runtime at the cost of image size |
| aungriah | 0:a3b83d366423 | 103 | // Two Way Ranging Actions when Frame was transmitted |
| aungriah | 0:a3b83d366423 | 104 | void BeaconNode::callbackTX(uint64_t TxTime) { |
| aungriah | 0:a3b83d366423 | 105 | //pc.printf("TXCallback: %d %d %d \r\n", acknowledgement[0], acknowledgement[1], acknowledgement[2]); |
| aungriah | 0:a3b83d366423 | 106 | dw.turnonrx(); // start listening again |
| aungriah | 0:a3b83d366423 | 107 | |
| aungriah | 0:a3b83d366423 | 108 | switch (rangingFrame.type) { |
| aungriah | 0:a3b83d366423 | 109 | case PING: |
| aungriah | 0:a3b83d366423 | 110 | senderTimestamps[rangingFrame.destination][0] = TxTime; //Save the first timestamp on the sending node/beacon (T_sp) |
| aungriah | 0:a3b83d366423 | 111 | break; |
| aungriah | 0:a3b83d366423 | 112 | case BEACON_RESPONSE: |
| aungriah | 0:a3b83d366423 | 113 | senderTimestamps[rangingFrame.destination][2] = TxTime; //Save the third timestamp on the sending node/beacon (T_sr) |
| aungriah | 0:a3b83d366423 | 114 | correctSenderTimestamps(rangingFrame.destination); //Correct the timestamps for the case of a counter overflow |
| aungriah | 0:a3b83d366423 | 115 | break; |
| aungriah | 0:a3b83d366423 | 116 | default: |
| aungriah | 0:a3b83d366423 | 117 | break; |
| aungriah | 0:a3b83d366423 | 118 | } |
| aungriah | 0:a3b83d366423 | 119 | if(acknowledgement[1] == true){ |
| aungriah | 0:a3b83d366423 | 120 | acknowledgement[2] = true; |
| aungriah | 0:a3b83d366423 | 121 | // pc.printf("Ack edited %d \r\n", acknowledgement[2]); |
| aungriah | 0:a3b83d366423 | 122 | } |
| aungriah | 0:a3b83d366423 | 123 | } |
| aungriah | 0:a3b83d366423 | 124 | |
| aungriah | 0:a3b83d366423 | 125 | |
| aungriah | 0:a3b83d366423 | 126 | #pragma Otime // Compiler optimize Runtime at the cost of image size |
| aungriah | 0:a3b83d366423 | 127 | /** |
| aungriah | 0:a3b83d366423 | 128 | * Get the distance to the Anchor with address @param destination. |
| aungriah | 0:a3b83d366423 | 129 | * |
| aungriah | 0:a3b83d366423 | 130 | * @param destination The address of the anchor |
| aungriah | 0:a3b83d366423 | 131 | */ |
| aungriah | 0:a3b83d366423 | 132 | void BeaconNode::requestRanging(uint8_t destination) { |
| aungriah | 0:a3b83d366423 | 133 | if(noRec[destination] <= MAX_TRIES){ |
| aungriah | 0:a3b83d366423 | 134 | |
| aungriah | 0:a3b83d366423 | 135 | |
| aungriah | 0:a3b83d366423 | 136 | float time_before = LocalTimer.read(); |
| aungriah | 0:a3b83d366423 | 137 | |
| aungriah | 0:a3b83d366423 | 138 | while(!acknowledgement[2] && LocalTimer.read() < time_before + 0.001f); // Wait until previous StreamFrame is sent |
| aungriah | 0:a3b83d366423 | 139 | |
| aungriah | 0:a3b83d366423 | 140 | acknowledgement[0] = false; |
| aungriah | 0:a3b83d366423 | 141 | acknowledgement[1] = false; |
| aungriah | 0:a3b83d366423 | 142 | acknowledgement[2] = false; |
| aungriah | 0:a3b83d366423 | 143 | time_before = LocalTimer.read(); |
| aungriah | 0:a3b83d366423 | 144 | |
| aungriah | 0:a3b83d366423 | 145 | sendPingFrame(destination); |
| aungriah | 0:a3b83d366423 | 146 | |
| aungriah | 0:a3b83d366423 | 147 | while(!acknowledgement[1] && (LocalTimer.read() < time_before + 0.001f + 0.003f*acknowledgement[0])); // One Ranging normaly takes less than 1.5 miliseconds |
| aungriah | 0:a3b83d366423 | 148 | |
| aungriah | 0:a3b83d366423 | 149 | if(acknowledgement[1]){ |
| aungriah | 0:a3b83d366423 | 150 | distances[destination] = calibratedDistance(destination); |
| aungriah | 0:a3b83d366423 | 151 | noRec[destination] = 0; |
| aungriah | 0:a3b83d366423 | 152 | // Stream Data to Basestation |
| aungriah | 0:a3b83d366423 | 153 | sendStreamFrame(destination); |
| aungriah | 0:a3b83d366423 | 154 | } else { |
| aungriah | 0:a3b83d366423 | 155 | distances[destination] = -10; |
| aungriah | 0:a3b83d366423 | 156 | noRec[destination]++; |
| aungriah | 0:a3b83d366423 | 157 | } |
| aungriah | 0:a3b83d366423 | 158 | } |
| aungriah | 0:a3b83d366423 | 159 | } |
| aungriah | 0:a3b83d366423 | 160 | |
| aungriah | 0:a3b83d366423 | 161 | #pragma Otime // Compiler optimize Runtime at the cost of image size |
| aungriah | 0:a3b83d366423 | 162 | inline float BeaconNode::calibratedDistance(uint8_t destination) { |
| aungriah | 0:a3b83d366423 | 163 | |
| aungriah | 0:a3b83d366423 | 164 | float rawDistance = (tofs[destination] * 300 * TIMEUNITS_TO_US / 4); |
| aungriah | 0:a3b83d366423 | 165 | //float correctDistance = rawDistance + dwt_getrangebias(7, rawDistance, DWT_PRF_64M); |
| aungriah | 0:a3b83d366423 | 166 | |
| aungriah | 0:a3b83d366423 | 167 | //if(rawDistance <= 8.458) |
| aungriah | 0:a3b83d366423 | 168 | // rawDistance -= 0.0541*rawDistance; // Correction Term 22-03-2017 |
| aungriah | 0:a3b83d366423 | 169 | //else |
| aungriah | 0:a3b83d366423 | 170 | if(rawDistance >= 22.7) |
| aungriah | 0:a3b83d366423 | 171 | rawDistance += -0.0004*rawDistance - 0.3971; |
| aungriah | 0:a3b83d366423 | 172 | else if (rawDistance >= 14.3) |
| aungriah | 0:a3b83d366423 | 173 | rawDistance += -0.0015*rawDistance - 0.372; |
| aungriah | 0:a3b83d366423 | 174 | else if (rawDistance >= 8) |
| aungriah | 0:a3b83d366423 | 175 | rawDistance += -0.0029*rawDistance - 0.352; |
| aungriah | 0:a3b83d366423 | 176 | else if (rawDistance >= 3.93) |
| aungriah | 0:a3b83d366423 | 177 | rawDistance += 0.001*rawDistance - 0.370; |
| aungriah | 0:a3b83d366423 | 178 | else |
| aungriah | 0:a3b83d366423 | 179 | rawDistance += -0.0235*rawDistance - 0.273; |
| aungriah | 0:a3b83d366423 | 180 | |
| aungriah | 0:a3b83d366423 | 181 | //else if (rawDistance >= 3) |
| aungriah | 0:a3b83d366423 | 182 | // rawDistance += 0.0004*rawDistance - 0.5556 |
| aungriah | 0:a3b83d366423 | 183 | /* else |
| aungriah | 0:a3b83d366423 | 184 | rawDistance += -0.01799*rawDistance - 0.2724; |
| aungriah | 0:a3b83d366423 | 185 | |
| aungriah | 0:a3b83d366423 | 186 | else if ()*/ |
| aungriah | 0:a3b83d366423 | 187 | |
| aungriah | 0:a3b83d366423 | 188 | |
| aungriah | 0:a3b83d366423 | 189 | //Non-Correction-Term: rawDistance -= 0.458; |
| aungriah | 0:a3b83d366423 | 190 | |
| aungriah | 0:a3b83d366423 | 191 | |
| aungriah | 0:a3b83d366423 | 192 | // Calibration for Nucleo 0 (and 1) |
| aungriah | 0:a3b83d366423 | 193 | |
| aungriah | 0:a3b83d366423 | 194 | // if (this->address == 1) rawDistance+= 10; |
| aungriah | 0:a3b83d366423 | 195 | // switch(destination){ |
| aungriah | 0:a3b83d366423 | 196 | // case 2: |
| aungriah | 0:a3b83d366423 | 197 | // return rawDistance * 0.9754 - 0.5004; |
| aungriah | 0:a3b83d366423 | 198 | // case 3: |
| aungriah | 0:a3b83d366423 | 199 | // return rawDistance * 0.9759 - 0.4103; |
| aungriah | 0:a3b83d366423 | 200 | // case 4: |
| aungriah | 0:a3b83d366423 | 201 | // return rawDistance * 0.9798 - 0.5499; |
| aungriah | 0:a3b83d366423 | 202 | // case 5: |
| aungriah | 0:a3b83d366423 | 203 | // return rawDistance * 0.9765 - 0.5169; |
| aungriah | 0:a3b83d366423 | 204 | // } |
| aungriah | 0:a3b83d366423 | 205 | |
| aungriah | 0:a3b83d366423 | 206 | return rawDistance; |
| aungriah | 0:a3b83d366423 | 207 | |
| aungriah | 0:a3b83d366423 | 208 | } |
| aungriah | 0:a3b83d366423 | 209 | |
| aungriah | 0:a3b83d366423 | 210 | #pragma Otime // Compiler optimize Runtime at the cost of image size |
| aungriah | 0:a3b83d366423 | 211 | void BeaconNode::requestRangingAll() { |
| aungriah | 0:a3b83d366423 | 212 | for (int i = 0; i < ADRESSES_COUNT; i++) { // Request ranging to all anchors |
| aungriah | 0:a3b83d366423 | 213 | if(i != address){ |
| aungriah | 0:a3b83d366423 | 214 | requestRanging(i); |
| aungriah | 0:a3b83d366423 | 215 | } |
| aungriah | 0:a3b83d366423 | 216 | else |
| aungriah | 0:a3b83d366423 | 217 | distances[i] = -10; |
| aungriah | 0:a3b83d366423 | 218 | } |
| aungriah | 0:a3b83d366423 | 219 | } |
| aungriah | 0:a3b83d366423 | 220 | |
| aungriah | 0:a3b83d366423 | 221 | #pragma Otime // Compiler optimize Runtime at the cost of image size |
| aungriah | 0:a3b83d366423 | 222 | void BeaconNode::sendPingFrame(uint8_t destination) { |
| aungriah | 0:a3b83d366423 | 223 | rangingFrame.source = address; |
| aungriah | 0:a3b83d366423 | 224 | rangingFrame.destination = destination; |
| aungriah | 0:a3b83d366423 | 225 | rangingFrame.type = PING; |
| aungriah | 0:a3b83d366423 | 226 | //dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame)); |
| aungriah | 0:a3b83d366423 | 227 | dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), 0, 0); |
| aungriah | 0:a3b83d366423 | 228 | } |
| aungriah | 0:a3b83d366423 | 229 | |
| aungriah | 0:a3b83d366423 | 230 | #pragma Otime // Compiler optimize Runtime at the cost of image size |
| aungriah | 0:a3b83d366423 | 231 | void BeaconNode::sendBaseAck() { |
| aungriah | 0:a3b83d366423 | 232 | rangingFrame.source = address; |
| aungriah | 0:a3b83d366423 | 233 | rangingFrame.destination = BASE_STATION_ADDR; |
| aungriah | 0:a3b83d366423 | 234 | rangingFrame.type = BASE_ORDER_ACK; |
| aungriah | 0:a3b83d366423 | 235 | //dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame)); |
| aungriah | 0:a3b83d366423 | 236 | dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), 0, 0); |
| aungriah | 0:a3b83d366423 | 237 | } |
| aungriah | 0:a3b83d366423 | 238 | |
| aungriah | 0:a3b83d366423 | 239 | #pragma Otime // Compiler optimize Runtime at the cost of image size |
| aungriah | 0:a3b83d366423 | 240 | void BeaconNode::sendStreamFrame(uint8_t anchor_addr) { |
| aungriah | 0:a3b83d366423 | 241 | StreamFrame streamFrame; |
| aungriah | 0:a3b83d366423 | 242 | |
| aungriah | 0:a3b83d366423 | 243 | streamFrame.source = address; |
| aungriah | 0:a3b83d366423 | 244 | streamFrame.destination = BASE_STATION_ADDR; |
| aungriah | 0:a3b83d366423 | 245 | streamFrame.type = STREAM_TO_BASE; |
| aungriah | 0:a3b83d366423 | 246 | streamFrame.anchor_adress = anchor_addr; |
| aungriah | 0:a3b83d366423 | 247 | streamFrame.distance = getDistance(anchor_addr); |
| aungriah | 0:a3b83d366423 | 248 | streamFrame.signalStrength = getSignalStrength(anchor_addr); |
| aungriah | 0:a3b83d366423 | 249 | streamFrame.FPLevel = dw.getFPLevel(); |
| aungriah | 0:a3b83d366423 | 250 | |
| aungriah | 0:a3b83d366423 | 251 | dw.sendFrame((uint8_t*)&streamFrame, sizeof(streamFrame), 0, 0); |
| aungriah | 0:a3b83d366423 | 252 | } |
| aungriah | 0:a3b83d366423 | 253 | |
| aungriah | 0:a3b83d366423 | 254 | #pragma Otime // Compiler optimize Runtime at the cost of image size |
| aungriah | 0:a3b83d366423 | 255 | void BeaconNode::sendAnswer(uint8_t destination, uint8_t type) { |
| aungriah | 0:a3b83d366423 | 256 | |
| aungriah | 0:a3b83d366423 | 257 | rangingFrame.source = address; |
| aungriah | 0:a3b83d366423 | 258 | rangingFrame.destination = destination; |
| aungriah | 0:a3b83d366423 | 259 | rangingFrame.type = type; |
| aungriah | 0:a3b83d366423 | 260 | |
| aungriah | 0:a3b83d366423 | 261 | dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), 0, 0); |
| aungriah | 0:a3b83d366423 | 262 | } |
| aungriah | 0:a3b83d366423 | 263 | |
| aungriah | 0:a3b83d366423 | 264 | #pragma Otime // Compiler optimize Runtime at the cost of image size |
| aungriah | 0:a3b83d366423 | 265 | void BeaconNode::correctSenderTimestamps(uint8_t source){ |
| aungriah | 0:a3b83d366423 | 266 | |
| aungriah | 0:a3b83d366423 | 267 | if (senderTimestamps[source][0] - dw.getAntennaDelay() > senderTimestamps[source][1]) { |
| aungriah | 0:a3b83d366423 | 268 | senderTimestamps[source][1] += MMRANGING_2POWER40; |
| aungriah | 0:a3b83d366423 | 269 | senderTimestamps[source][2] += MMRANGING_2POWER40; |
| aungriah | 0:a3b83d366423 | 270 | } |
| aungriah | 0:a3b83d366423 | 271 | if (senderTimestamps[source][1] > senderTimestamps[source][2]) { |
| aungriah | 0:a3b83d366423 | 272 | senderTimestamps[source][2] += MMRANGING_2POWER40; |
| aungriah | 0:a3b83d366423 | 273 | } |
| aungriah | 0:a3b83d366423 | 274 | |
| aungriah | 0:a3b83d366423 | 275 | } |
| aungriah | 0:a3b83d366423 | 276 | |
| aungriah | 0:a3b83d366423 | 277 | float BeaconNode::getSignalStrength(uint8_t index){ |
| aungriah | 0:a3b83d366423 | 278 | return signalStrength[index]; |
| aungriah | 0:a3b83d366423 | 279 | } |
| aungriah | 0:a3b83d366423 | 280 | |
| aungriah | 0:a3b83d366423 | 281 | float BeaconNode::getDistance(uint8_t index){ |
| aungriah | 0:a3b83d366423 | 282 | return distances[index]; |
| aungriah | 0:a3b83d366423 | 283 | } |
| aungriah | 0:a3b83d366423 | 284 | |
| aungriah | 0:a3b83d366423 | 285 | uint8* BeaconNode::getRecFrameRef(){ |
| aungriah | 0:a3b83d366423 | 286 | return (uint8 *) &receivedFrame; |
| aungriah | 0:a3b83d366423 | 287 | } |
| aungriah | 0:a3b83d366423 | 288 | uint16 BeaconNode::getRecFrameLength(){ |
| aungriah | 0:a3b83d366423 | 289 | return sizeof(receivedFrame); |
| aungriah | 0:a3b83d366423 | 290 | } |
| aungriah | 0:a3b83d366423 | 291 | |
| aungriah | 0:a3b83d366423 | 292 | int BeaconNode::getMode(){ |
| aungriah | 0:a3b83d366423 | 293 | return mode; |
| aungriah | 0:a3b83d366423 | 294 | } |
| aungriah | 0:a3b83d366423 | 295 | |
| aungriah | 0:a3b83d366423 | 296 | uint16_t BeaconNode::getRepetitions(){ |
| aungriah | 0:a3b83d366423 | 297 | return repetitions; |
| aungriah | 0:a3b83d366423 | 298 | } |
| aungriah | 0:a3b83d366423 | 299 | void BeaconNode::decreaseRepetitions(){ |
| aungriah | 0:a3b83d366423 | 300 | repetitions--; |
| aungriah | 0:a3b83d366423 | 301 | } |
| aungriah | 0:a3b83d366423 | 302 | uint8_t BeaconNode::getDestination(){ |
| aungriah | 0:a3b83d366423 | 303 | return destination; |
| aungriah | 0:a3b83d366423 | 304 | } |
| aungriah | 0:a3b83d366423 | 305 | |
| aungriah | 0:a3b83d366423 | 306 | void BeaconNode::clearRec(){ |
| aungriah | 0:a3b83d366423 | 307 | for(int j = 0; j < ADRESSES_COUNT; j++) |
| aungriah | 0:a3b83d366423 | 308 | noRec[j] = 0; |
| aungriah | 0:a3b83d366423 | 309 | } |
| aungriah | 0:a3b83d366423 | 310 | |
| aungriah | 0:a3b83d366423 | 311 | |
| aungriah | 0:a3b83d366423 | 312 | //------- AnchorNode Class ------------------- |
| aungriah | 0:a3b83d366423 | 313 | AnchorNode::AnchorNode(DecaWave& DW) : Node(DW) { |
| aungriah | 0:a3b83d366423 | 314 | Anchor = true; |
| aungriah | 0:a3b83d366423 | 315 | } |
| aungriah | 0:a3b83d366423 | 316 | |
| aungriah | 0:a3b83d366423 | 317 | #pragma Otime // Compiler optimize Runtime at the cost of image size |
| aungriah | 0:a3b83d366423 | 318 | // Two Way Ranging Actions when Frame was received |
| aungriah | 0:a3b83d366423 | 319 | void AnchorNode::callbackRX(uint64_t RxTime) { |
| aungriah | 0:a3b83d366423 | 320 | // pc.printf("Got a frame, adress: %d source: %d \r\n", receivedFrame.destination, receivedFrame.source); |
| aungriah | 0:a3b83d366423 | 321 | // if(!isBaseStation()){ |
| aungriah | 0:a3b83d366423 | 322 | if (receivedFrame.destination == address){ |
| aungriah | 0:a3b83d366423 | 323 | switch (receivedFrame.type) { |
| aungriah | 0:a3b83d366423 | 324 | case SWITCH_TYPE: |
| aungriah | 0:a3b83d366423 | 325 | sendBaseAck(); |
| aungriah | 0:a3b83d366423 | 326 | if((receivedFrame.signedTime >> 24) == BECOME_BEACON){ |
| aungriah | 0:a3b83d366423 | 327 | node = &beaconNode; |
| aungriah | 0:a3b83d366423 | 328 | pc.printf("\r\n \r\n ---This Node is a beacon now ---\r\n \r\n"); |
| aungriah | 0:a3b83d366423 | 329 | } |
| aungriah | 0:a3b83d366423 | 330 | else if((receivedFrame.signedTime >> 24) == BECOME_ANCHOR){ |
| aungriah | 0:a3b83d366423 | 331 | pc.printf("\r\n \r\n ---This Node is already an anchor ---\r\n \r\n"); |
| aungriah | 0:a3b83d366423 | 332 | } |
| aungriah | 0:a3b83d366423 | 333 | break; |
| aungriah | 0:a3b83d366423 | 334 | case PING: |
| aungriah | 0:a3b83d366423 | 335 | sendAnswer(receivedFrame.source, ANCHOR_RESPONSE); |
| aungriah | 0:a3b83d366423 | 336 | receiverTimestamps[receivedFrame.source][0] = RxTime; //Save the first timestamp on the receiving node/anchor (T_rp) |
| aungriah | 0:a3b83d366423 | 337 | break; |
| aungriah | 0:a3b83d366423 | 338 | case BEACON_RESPONSE: |
| aungriah | 0:a3b83d366423 | 339 | { |
| aungriah | 0:a3b83d366423 | 340 | receiverTimestamps[receivedFrame.source][2] = RxTime; //Save the third timestamp on the receiving node/anchor (T_rf) |
| aungriah | 0:a3b83d366423 | 341 | correctReceiverTimestamps(receivedFrame.source); //Correct the timestamps for the case of a counter overflow |
| aungriah | 0:a3b83d366423 | 342 | //timediffRec = receiverTimestamps[receivedFrame.source][0] + receiverTimestamps[receivedFrame.source][2] - 2*receiverTimestamps[receivedFrame.source][1]; |
| aungriah | 0:a3b83d366423 | 343 | //if(timediffRec < 0) |
| aungriah | 0:a3b83d366423 | 344 | // timediffRec = 0; |
| aungriah | 0:a3b83d366423 | 345 | sendTransferFrame(receivedFrame.source, receiverTimestamps[receivedFrame.source][0] + receiverTimestamps[receivedFrame.source][2] - 2*receiverTimestamps[receivedFrame.source][1]); |
| aungriah | 0:a3b83d366423 | 346 | break; |
| aungriah | 0:a3b83d366423 | 347 | } |
| aungriah | 0:a3b83d366423 | 348 | default : break; |
| aungriah | 0:a3b83d366423 | 349 | } |
| aungriah | 0:a3b83d366423 | 350 | } |
| aungriah | 0:a3b83d366423 | 351 | else{ |
| aungriah | 0:a3b83d366423 | 352 | dw.turnonrx(); // start listening again |
| aungriah | 0:a3b83d366423 | 353 | } |
| aungriah | 0:a3b83d366423 | 354 | } |
| aungriah | 0:a3b83d366423 | 355 | |
| aungriah | 0:a3b83d366423 | 356 | |
| aungriah | 0:a3b83d366423 | 357 | #pragma Otime // Compiler optimize Runtime at the cost of image size |
| aungriah | 0:a3b83d366423 | 358 | // Two Way Ranging Actions when Frame was transmitted |
| aungriah | 0:a3b83d366423 | 359 | void AnchorNode::callbackTX(uint64_t TxTime) { |
| aungriah | 0:a3b83d366423 | 360 | dw.turnonrx(); // start listening again |
| aungriah | 0:a3b83d366423 | 361 | |
| aungriah | 0:a3b83d366423 | 362 | switch (rangingFrame.type) { |
| aungriah | 0:a3b83d366423 | 363 | case ANCHOR_RESPONSE: |
| aungriah | 0:a3b83d366423 | 364 | receiverTimestamps[rangingFrame.destination][1] = TxTime; //Save the second timestamp on the receiving node/anchor (T_sr) |
| aungriah | 0:a3b83d366423 | 365 | break; |
| aungriah | 0:a3b83d366423 | 366 | default: |
| aungriah | 0:a3b83d366423 | 367 | break; |
| aungriah | 0:a3b83d366423 | 368 | } |
| aungriah | 0:a3b83d366423 | 369 | } |
| aungriah | 0:a3b83d366423 | 370 | |
| aungriah | 0:a3b83d366423 | 371 | |
| aungriah | 0:a3b83d366423 | 372 | |
| aungriah | 0:a3b83d366423 | 373 | |
| aungriah | 0:a3b83d366423 | 374 | #pragma Otime // Compiler optimize Runtime at the cost of image size |
| aungriah | 0:a3b83d366423 | 375 | void AnchorNode::sendBaseAck() { |
| aungriah | 0:a3b83d366423 | 376 | rangingFrame.source = address; |
| aungriah | 0:a3b83d366423 | 377 | rangingFrame.destination = BASE_STATION_ADDR; |
| aungriah | 0:a3b83d366423 | 378 | rangingFrame.type = BASE_ORDER_ACK; |
| aungriah | 0:a3b83d366423 | 379 | //dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame)); |
| aungriah | 0:a3b83d366423 | 380 | dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), 0, 0); |
| aungriah | 0:a3b83d366423 | 381 | } |
| aungriah | 0:a3b83d366423 | 382 | |
| aungriah | 0:a3b83d366423 | 383 | |
| aungriah | 0:a3b83d366423 | 384 | |
| aungriah | 0:a3b83d366423 | 385 | |
| aungriah | 0:a3b83d366423 | 386 | |
| aungriah | 0:a3b83d366423 | 387 | #pragma Otime // Compiler optimize Runtime at the cost of image size |
| aungriah | 0:a3b83d366423 | 388 | void AnchorNode::sendTransferFrame(uint8_t destination, int timeDiffsReceiver) { |
| aungriah | 0:a3b83d366423 | 389 | ExtendedRangingFrame transferFrame; |
| aungriah | 0:a3b83d366423 | 390 | |
| aungriah | 0:a3b83d366423 | 391 | transferFrame.source = address; |
| aungriah | 0:a3b83d366423 | 392 | transferFrame.destination = destination; |
| aungriah | 0:a3b83d366423 | 393 | transferFrame.type = TRANSFER_FRAME; |
| aungriah | 0:a3b83d366423 | 394 | transferFrame.signedTime = timeDiffsReceiver; //cast the time difference |
| aungriah | 0:a3b83d366423 | 395 | dw.sendFrame((uint8_t*)&transferFrame, sizeof(transferFrame), 0, 0); |
| aungriah | 0:a3b83d366423 | 396 | } |
| aungriah | 0:a3b83d366423 | 397 | |
| aungriah | 0:a3b83d366423 | 398 | #pragma Otime // Compiler optimize Runtime at the cost of image size |
| aungriah | 0:a3b83d366423 | 399 | void AnchorNode::sendAnswer(uint8_t destination, uint8_t type) { |
| aungriah | 0:a3b83d366423 | 400 | |
| aungriah | 0:a3b83d366423 | 401 | rangingFrame.source = address; |
| aungriah | 0:a3b83d366423 | 402 | rangingFrame.destination = destination; |
| aungriah | 0:a3b83d366423 | 403 | rangingFrame.type = type; |
| aungriah | 0:a3b83d366423 | 404 | |
| aungriah | 0:a3b83d366423 | 405 | dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), 0, 0); |
| aungriah | 0:a3b83d366423 | 406 | } |
| aungriah | 0:a3b83d366423 | 407 | |
| aungriah | 0:a3b83d366423 | 408 | #pragma Otime // Compiler optimize Runtime at the cost of image size |
| aungriah | 0:a3b83d366423 | 409 | void AnchorNode::correctReceiverTimestamps(uint8_t source){ |
| aungriah | 0:a3b83d366423 | 410 | |
| aungriah | 0:a3b83d366423 | 411 | if(receiverTimestamps[source][0] > receiverTimestamps[source][1]){ |
| aungriah | 0:a3b83d366423 | 412 | receiverTimestamps[source][1] += MMRANGING_2POWER40; |
| aungriah | 0:a3b83d366423 | 413 | receiverTimestamps[source][2] += MMRANGING_2POWER40; |
| aungriah | 0:a3b83d366423 | 414 | } |
| aungriah | 0:a3b83d366423 | 415 | |
| aungriah | 0:a3b83d366423 | 416 | if(receiverTimestamps[source][1] - dw.getAntennaDelay() > receiverTimestamps[source][2]){ |
| aungriah | 0:a3b83d366423 | 417 | receiverTimestamps[source][2] += MMRANGING_2POWER40; |
| aungriah | 0:a3b83d366423 | 418 | } |
| aungriah | 0:a3b83d366423 | 419 | |
| aungriah | 0:a3b83d366423 | 420 | } |
| aungriah | 0:a3b83d366423 | 421 | |
| aungriah | 0:a3b83d366423 | 422 | |
| aungriah | 0:a3b83d366423 | 423 | |
| aungriah | 0:a3b83d366423 | 424 | uint8* AnchorNode::getRecFrameRef(){ |
| aungriah | 0:a3b83d366423 | 425 | return (uint8 *) &receivedFrame; |
| aungriah | 0:a3b83d366423 | 426 | } |
| aungriah | 0:a3b83d366423 | 427 | uint16 AnchorNode::getRecFrameLength(){ |
| aungriah | 0:a3b83d366423 | 428 | return sizeof(receivedFrame); |
| aungriah | 0:a3b83d366423 | 429 | } |
| aungriah | 0:a3b83d366423 | 430 | |
| aungriah | 0:a3b83d366423 | 431 | |
| aungriah | 0:a3b83d366423 | 432 | //------- BaseStationNode Class ------------------- |
| aungriah | 0:a3b83d366423 | 433 | BaseStationNode::BaseStationNode(DecaWave& DW) : Node(DW) { |
| aungriah | 0:a3b83d366423 | 434 | Anchor = false; |
| aungriah | 0:a3b83d366423 | 435 | LocalTimer.start(); |
| aungriah | 0:a3b83d366423 | 436 | } |
| aungriah | 0:a3b83d366423 | 437 | |
| aungriah | 0:a3b83d366423 | 438 | uint8* BaseStationNode::getRecFrameRef(){ |
| aungriah | 0:a3b83d366423 | 439 | return (uint8 *) &receivedStreamFrame; |
| aungriah | 0:a3b83d366423 | 440 | } |
| aungriah | 0:a3b83d366423 | 441 | uint16 BaseStationNode::getRecFrameLength(){ |
| aungriah | 0:a3b83d366423 | 442 | return sizeof(receivedStreamFrame); |
| aungriah | 0:a3b83d366423 | 443 | } |
| aungriah | 0:a3b83d366423 | 444 | |
| aungriah | 0:a3b83d366423 | 445 | |
| aungriah | 0:a3b83d366423 | 446 | #pragma Otime // Compiler optimize Runtime at the cost of image size |
| aungriah | 0:a3b83d366423 | 447 | // Two Way Ranging Actions when Frame was received |
| aungriah | 0:a3b83d366423 | 448 | void BaseStationNode::callbackRX(uint64_t RxTime) { |
| aungriah | 0:a3b83d366423 | 449 | //pc.printf("Got a frame, adress: %d source: %d \r\n", receivedStreamFrame.destination, receivedStreamFrame.source); |
| aungriah | 0:a3b83d366423 | 450 | if (receivedStreamFrame.destination == address){ |
| aungriah | 0:a3b83d366423 | 451 | switch(receivedStreamFrame.type){ |
| aungriah | 0:a3b83d366423 | 452 | case STREAM_TO_BASE: |
| aungriah | 0:a3b83d366423 | 453 | |
| aungriah | 0:a3b83d366423 | 454 | //pc.printf("#%d to #%d %f(%f) \r\n", receivedStreamFrame.source, receivedStreamFrame.anchor_adress, receivedStreamFrame.distance, receivedStreamFrame.signalStrength); |
| aungriah | 0:a3b83d366423 | 455 | pc.printf("#%03d/#%03d/%+011.6f/%+011.6f/%+011.6f/ \r\n", receivedStreamFrame.source, receivedStreamFrame.anchor_adress, receivedStreamFrame.distance, receivedStreamFrame.signalStrength, receivedStreamFrame.FPLevel/*dw.getFPLevel()*/); |
| aungriah | 0:a3b83d366423 | 456 | break; |
| aungriah | 0:a3b83d366423 | 457 | case BASE_ORDER_ACK: |
| aungriah | 0:a3b83d366423 | 458 | |
| aungriah | 0:a3b83d366423 | 459 | ack = true; |
| aungriah | 0:a3b83d366423 | 460 | break; |
| aungriah | 0:a3b83d366423 | 461 | |
| aungriah | 0:a3b83d366423 | 462 | |
| aungriah | 0:a3b83d366423 | 463 | |
| aungriah | 0:a3b83d366423 | 464 | } |
| aungriah | 0:a3b83d366423 | 465 | } |
| aungriah | 0:a3b83d366423 | 466 | dw.turnonrx(); // start listening again |
| aungriah | 0:a3b83d366423 | 467 | |
| aungriah | 0:a3b83d366423 | 468 | |
| aungriah | 0:a3b83d366423 | 469 | } |
| aungriah | 0:a3b83d366423 | 470 | |
| aungriah | 0:a3b83d366423 | 471 | #pragma Otime // Compiler optimize Runtime at the cost of image size |
| aungriah | 0:a3b83d366423 | 472 | // Two Way Ranging Actions when Frame was transmitted |
| aungriah | 0:a3b83d366423 | 473 | void BaseStationNode::callbackTX(uint64_t TxTime) { |
| aungriah | 0:a3b83d366423 | 474 | dw.turnonrx(); // start listening again |
| aungriah | 0:a3b83d366423 | 475 | } |
| aungriah | 0:a3b83d366423 | 476 | |
| aungriah | 0:a3b83d366423 | 477 | #pragma Otime // Compiler optimize Runtime at the cost of image size |
| aungriah | 0:a3b83d366423 | 478 | void BaseStationNode::sendOrder(uint8_t beacon_destination, uint8_t anchor_destination, uint8_t action, uint16_t repetitions, uint8_t type) { |
| aungriah | 0:a3b83d366423 | 479 | ExtendedRangingFrame orderFrame; |
| aungriah | 0:a3b83d366423 | 480 | ack = false; |
| aungriah | 0:a3b83d366423 | 481 | |
| aungriah | 0:a3b83d366423 | 482 | orderFrame.source = address; |
| aungriah | 0:a3b83d366423 | 483 | orderFrame.destination = beacon_destination; |
| aungriah | 0:a3b83d366423 | 484 | orderFrame.type = type; |
| aungriah | 0:a3b83d366423 | 485 | orderFrame.signedTime = action << 24 | anchor_destination << 16 | repetitions; |
| aungriah | 0:a3b83d366423 | 486 | |
| aungriah | 0:a3b83d366423 | 487 | int i = 0; |
| aungriah | 0:a3b83d366423 | 488 | for(i = 0; i < 10 && !ack; i++){ |
| aungriah | 0:a3b83d366423 | 489 | float time_before = LocalTimer.read(); |
| aungriah | 0:a3b83d366423 | 490 | dw.sendFrame((uint8_t*)&orderFrame, sizeof(orderFrame), 0, 0); |
| aungriah | 0:a3b83d366423 | 491 | while(!ack && (LocalTimer.read() < time_before + 0.010 + 0.01*i)); // One Ranging normaly takes less than 1.5 miliseconds |
| aungriah | 0:a3b83d366423 | 492 | } |
| aungriah | 0:a3b83d366423 | 493 | if(!ack) |
| aungriah | 0:a3b83d366423 | 494 | { |
| aungriah | 0:a3b83d366423 | 495 | pc.printf("ERROR: Tag #%d did not respond \r\n", beacon_destination); |
| aungriah | 0:a3b83d366423 | 496 | } |
| aungriah | 0:a3b83d366423 | 497 | else |
| aungriah | 0:a3b83d366423 | 498 | { |
| aungriah | 0:a3b83d366423 | 499 | //pc.printf("Order sent, %d tries \r\n", i); |
| aungriah | 0:a3b83d366423 | 500 | } |
| aungriah | 0:a3b83d366423 | 501 | |
| aungriah | 0:a3b83d366423 | 502 | |
| aungriah | 0:a3b83d366423 | 503 | |
| aungriah | 0:a3b83d366423 | 504 | } |