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