Huseyin Berkay Berabi
/
GA-Final
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.
Fork of
GA-Berkay_Alex
by 
Alejandro Ungria Hirte
nodes/nodes.cpp@4:120ff05a7c27, 2018-02-28 (annotated)
- Committer:
- bberabi
- Date:
- Wed Feb 28 17:06:22 2018 +0000
- Revision:
- 4:120ff05a7c27
- Parent:
- 3:8bee1711d186
GA Final code with comments
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 ------------------- |
| bberabi | 4:120ff05a7c27 | 32 | |
| bberabi | 4:120ff05a7c27 | 33 | //This is the class for the Tag ! |
| bberabi | 4:120ff05a7c27 | 34 | |
| aungriah | 0:a3b83d366423 | 35 | BeaconNode::BeaconNode(DecaWave& DW) : Node(DW) { |
| bberabi | 4:120ff05a7c27 | 36 | Anchor = false; // since it is a tag |
| aungriah | 0:a3b83d366423 | 37 | for (int i = 0; i < 3; i++) |
| aungriah | 0:a3b83d366423 | 38 | acknowledgement[i] = true; |
| bberabi | 4:120ff05a7c27 | 39 | LocalTimer.start(); //starts the timer of the tag. |
| aungriah | 0:a3b83d366423 | 40 | mode=0; |
| aungriah | 0:a3b83d366423 | 41 | } |
| aungriah | 0:a3b83d366423 | 42 | |
| aungriah | 0:a3b83d366423 | 43 | #pragma Otime // Compiler optimize Runtime at the cost of image size |
| aungriah | 0:a3b83d366423 | 44 | // Two Way Ranging Actions when Frame was received |
| bberabi | 4:120ff05a7c27 | 45 | //a Frame is basically a message. |
| aungriah | 0:a3b83d366423 | 46 | void BeaconNode::callbackRX(uint64_t RxTime) { |
| aungriah | 0:a3b83d366423 | 47 | //pc.printf("Got a frame, adress: %d source: %d \r\n", receivedFrame.destination, receivedFrame.source); |
| bberabi | 4:120ff05a7c27 | 48 | if (receivedFrame.destination == address){ //if id of the package is the same as the id of the anchor to which the tag was communicating |
| aungriah | 3:8bee1711d186 | 49 | |
| aungriah | 0:a3b83d366423 | 50 | switch (receivedFrame.type) { |
| bberabi | 4:120ff05a7c27 | 51 | case SWITCH_TYPE: //base station can change the type of an antenna, making an anchor tag or making a tag anchor |
| aungriah | 0:a3b83d366423 | 52 | sendBaseAck(); |
| aungriah | 0:a3b83d366423 | 53 | if((receivedFrame.signedTime >> 24) == BECOME_BEACON){ |
| aungriah | 0:a3b83d366423 | 54 | pc.printf("\r\n \r\n ---This Node is already a beacon ---\r\n \r\n"); |
| aungriah | 0:a3b83d366423 | 55 | } |
| aungriah | 0:a3b83d366423 | 56 | else if((receivedFrame.signedTime >> 24) == BECOME_ANCHOR){ |
| aungriah | 0:a3b83d366423 | 57 | node = &anchorNode; |
| aungriah | 0:a3b83d366423 | 58 | pc.printf("\r\n \r\n ---This Node is an anchor now ---\r\n \r\n"); |
| aungriah | 0:a3b83d366423 | 59 | } |
| aungriah | 0:a3b83d366423 | 60 | |
| aungriah | 0:a3b83d366423 | 61 | break; |
| bberabi | 4:120ff05a7c27 | 62 | case BASE_ORDER: // tag received an order from base station |
| bberabi | 4:120ff05a7c27 | 63 | repetitions = receivedFrame.signedTime&0xffff; |
| aungriah | 0:a3b83d366423 | 64 | mode = receivedFrame.signedTime >> 24; |
| aungriah | 0:a3b83d366423 | 65 | destination = (receivedFrame.signedTime&0xff0000) >> 16; |
| bberabi | 4:120ff05a7c27 | 66 | sendBaseAck(); //send an acknowledgement to base that message is received |
| aungriah | 0:a3b83d366423 | 67 | break; |
| bberabi | 4:120ff05a7c27 | 68 | case ANCHOR_RESPONSE: //tag was measuring distance to an anchor, and respond from anchor came |
| aungriah | 0:a3b83d366423 | 69 | { |
| aungriah | 0:a3b83d366423 | 70 | sendAnswer(receivedFrame.source, BEACON_RESPONSE); |
| aungriah | 0:a3b83d366423 | 71 | senderTimestamps[receivedFrame.source][1] = RxTime; //Save the second timestamp on the sending node/beacon (T_rr) |
| aungriah | 0:a3b83d366423 | 72 | acknowledgement[0] = true; |
| aungriah | 0:a3b83d366423 | 73 | |
| aungriah | 0:a3b83d366423 | 74 | // Get Signal Level |
| aungriah | 0:a3b83d366423 | 75 | dwt_rxdiag_t diagnostic; |
| aungriah | 0:a3b83d366423 | 76 | dwt_readdiagnostics(&diagnostic); |
| aungriah | 0:a3b83d366423 | 77 | signalStrength[receivedFrame.source] = dw.getRXLevel(&diagnostic); |
| aungriah | 0:a3b83d366423 | 78 | |
| aungriah | 0:a3b83d366423 | 79 | break; |
| aungriah | 0:a3b83d366423 | 80 | } |
| aungriah | 0:a3b83d366423 | 81 | case TRANSFER_FRAME: |
| aungriah | 0:a3b83d366423 | 82 | { |
| aungriah | 0:a3b83d366423 | 83 | //timediffSend = 2 * senderTimestamps[receivedFrame.source][1] - senderTimestamps[receivedFrame.source][0] - senderTimestamps[receivedFrame.source][2]; |
| aungriah | 0:a3b83d366423 | 84 | tofs[receivedFrame.source] = receivedFrame.signedTime + 2 * senderTimestamps[receivedFrame.source][1] - senderTimestamps[receivedFrame.source][0] - senderTimestamps[receivedFrame.source][2]; |
| aungriah | 0:a3b83d366423 | 85 | |
| aungriah | 0:a3b83d366423 | 86 | // Get Signal Level |
| aungriah | 0:a3b83d366423 | 87 | dwt_rxdiag_t diagnostic2; |
| aungriah | 0:a3b83d366423 | 88 | dwt_readdiagnostics(&diagnostic2); |
| aungriah | 0:a3b83d366423 | 89 | float level = dw.getRXLevel(&diagnostic2); |
| aungriah | 0:a3b83d366423 | 90 | if(level < signalStrength[receivedFrame.source]) |
| aungriah | 0:a3b83d366423 | 91 | signalStrength[receivedFrame.source] = level; |
| aungriah | 3:8bee1711d186 | 92 | |
| aungriah | 0:a3b83d366423 | 93 | |
| aungriah | 0:a3b83d366423 | 94 | acknowledgement[1] = true; |
| aungriah | 3:8bee1711d186 | 95 | |
| bberabi | 2:5adf0b785944 | 96 | // dw.turnonrx(); // start listening again |
| aungriah | 0:a3b83d366423 | 97 | break; |
| aungriah | 0:a3b83d366423 | 98 | } |
| bberabi | 2:5adf0b785944 | 99 | default : dw.turnonrx(); |
| bberabi | 2:5adf0b785944 | 100 | break; |
| aungriah | 0:a3b83d366423 | 101 | } |
| aungriah | 0:a3b83d366423 | 102 | } |
| aungriah | 0:a3b83d366423 | 103 | else{ |
| aungriah | 0:a3b83d366423 | 104 | dw.turnonrx(); // start listening again |
| aungriah | 0:a3b83d366423 | 105 | } |
| aungriah | 0:a3b83d366423 | 106 | } |
| aungriah | 0:a3b83d366423 | 107 | |
| aungriah | 0:a3b83d366423 | 108 | |
| aungriah | 0:a3b83d366423 | 109 | #pragma Otime // Compiler optimize Runtime at the cost of image size |
| aungriah | 0:a3b83d366423 | 110 | // Two Way Ranging Actions when Frame was transmitted |
| aungriah | 0:a3b83d366423 | 111 | void BeaconNode::callbackTX(uint64_t TxTime) { |
| aungriah | 0:a3b83d366423 | 112 | //pc.printf("TXCallback: %d %d %d \r\n", acknowledgement[0], acknowledgement[1], acknowledgement[2]); |
| aungriah | 0:a3b83d366423 | 113 | dw.turnonrx(); // start listening again |
| aungriah | 0:a3b83d366423 | 114 | |
| aungriah | 0:a3b83d366423 | 115 | switch (rangingFrame.type) { |
| aungriah | 0:a3b83d366423 | 116 | case PING: |
| aungriah | 0:a3b83d366423 | 117 | senderTimestamps[rangingFrame.destination][0] = TxTime; //Save the first timestamp on the sending node/beacon (T_sp) |
| aungriah | 0:a3b83d366423 | 118 | break; |
| aungriah | 0:a3b83d366423 | 119 | case BEACON_RESPONSE: |
| aungriah | 0:a3b83d366423 | 120 | senderTimestamps[rangingFrame.destination][2] = TxTime; //Save the third timestamp on the sending node/beacon (T_sr) |
| aungriah | 0:a3b83d366423 | 121 | correctSenderTimestamps(rangingFrame.destination); //Correct the timestamps for the case of a counter overflow |
| aungriah | 0:a3b83d366423 | 122 | break; |
| aungriah | 0:a3b83d366423 | 123 | default: |
| aungriah | 0:a3b83d366423 | 124 | break; |
| aungriah | 0:a3b83d366423 | 125 | } |
| aungriah | 0:a3b83d366423 | 126 | if(acknowledgement[1] == true){ |
| aungriah | 0:a3b83d366423 | 127 | acknowledgement[2] = true; |
| aungriah | 0:a3b83d366423 | 128 | // pc.printf("Ack edited %d \r\n", acknowledgement[2]); |
| aungriah | 3:8bee1711d186 | 129 | } |
| aungriah | 3:8bee1711d186 | 130 | |
| aungriah | 0:a3b83d366423 | 131 | } |
| aungriah | 0:a3b83d366423 | 132 | |
| aungriah | 0:a3b83d366423 | 133 | |
| aungriah | 0:a3b83d366423 | 134 | #pragma Otime // Compiler optimize Runtime at the cost of image size |
| aungriah | 0:a3b83d366423 | 135 | /** |
| aungriah | 0:a3b83d366423 | 136 | * Get the distance to the Anchor with address @param destination. |
| aungriah | 0:a3b83d366423 | 137 | * |
| aungriah | 0:a3b83d366423 | 138 | * @param destination The address of the anchor |
| aungriah | 0:a3b83d366423 | 139 | */ |
| aungriah | 0:a3b83d366423 | 140 | void BeaconNode::requestRanging(uint8_t destination) { |
| aungriah | 0:a3b83d366423 | 141 | if(noRec[destination] <= MAX_TRIES){ |
| bberabi | 4:120ff05a7c27 | 142 | |
| bberabi | 4:120ff05a7c27 | 143 | //norec array containg for each id(index) how many times it was tried to contact that anchor. |
| bberabi | 4:120ff05a7c27 | 144 | //for example if tag has tried to contact anchor 5 3 times, noRec[5] = 3 ! |
| bberabi | 4:120ff05a7c27 | 145 | //MAX_TRIES can be changed at nodex.h |
| bberabi | 4:120ff05a7c27 | 146 | |
| bberabi | 4:120ff05a7c27 | 147 | // pc.printf(" max try %d\n\r",MAX_TRIES); |
| aungriah | 0:a3b83d366423 | 148 | float time_before = LocalTimer.read(); |
| aungriah | 0:a3b83d366423 | 149 | |
| aungriah | 3:8bee1711d186 | 150 | while(!acknowledgement[2] && LocalTimer.read() < time_before + 0.0025f); // Wait until previous StreamFrame is sent |
| aungriah | 0:a3b83d366423 | 151 | |
| aungriah | 0:a3b83d366423 | 152 | acknowledgement[0] = false; |
| aungriah | 0:a3b83d366423 | 153 | acknowledgement[1] = false; |
| aungriah | 0:a3b83d366423 | 154 | acknowledgement[2] = false; |
| bberabi | 4:120ff05a7c27 | 155 | time_before = LocalTimer.read(); //start time t0 |
| aungriah | 0:a3b83d366423 | 156 | |
| aungriah | 0:a3b83d366423 | 157 | sendPingFrame(destination); |
| aungriah | 0:a3b83d366423 | 158 | |
| aungriah | 3:8bee1711d186 | 159 | while(!acknowledgement[1] && (LocalTimer.read() < time_before + 0.0025f + 0.003f*acknowledgement[0])); // One Ranging normaly takes less than 1.5 miliseconds |
| aungriah | 0:a3b83d366423 | 160 | |
| bberabi | 4:120ff05a7c27 | 161 | if(acknowledgement[1]){ //measurement is done |
| aungriah | 0:a3b83d366423 | 162 | distances[destination] = calibratedDistance(destination); |
| aungriah | 0:a3b83d366423 | 163 | noRec[destination] = 0; |
| aungriah | 0:a3b83d366423 | 164 | // Stream Data to Basestation |
| aungriah | 0:a3b83d366423 | 165 | sendStreamFrame(destination); |
| bberabi | 4:120ff05a7c27 | 166 | } else { //maesurement was not succesfull |
| aungriah | 3:8bee1711d186 | 167 | //acknowledgement[1]=1; |
| aungriah | 0:a3b83d366423 | 168 | distances[destination] = -10; |
| bberabi | 2:5adf0b785944 | 169 | noRec[destination] = 0; |
| bberabi | 2:5adf0b785944 | 170 | sendStreamFrame(destination); |
| bberabi | 2:5adf0b785944 | 171 | |
| bberabi | 2:5adf0b785944 | 172 | |
| aungriah | 0:a3b83d366423 | 173 | } |
| aungriah | 0:a3b83d366423 | 174 | } |
| bberabi | 2:5adf0b785944 | 175 | pc.printf("no rec dest %d\n\r",noRec[destination]); |
| aungriah | 0:a3b83d366423 | 176 | } |
| aungriah | 0:a3b83d366423 | 177 | |
| aungriah | 0:a3b83d366423 | 178 | #pragma Otime // Compiler optimize Runtime at the cost of image size |
| aungriah | 0:a3b83d366423 | 179 | inline float BeaconNode::calibratedDistance(uint8_t destination) { |
| aungriah | 0:a3b83d366423 | 180 | |
| bberabi | 4:120ff05a7c27 | 181 | //the error of two way ranging also depends on how long the distance is. We have tried to calibrate these errors. |
| bberabi | 4:120ff05a7c27 | 182 | //the numbers are found according to our measurements and test. |
| bberabi | 4:120ff05a7c27 | 183 | //PLEASE NOTE THAT THESE CALIBRATIONS MIGHT BE DONE WISELY IN ORDER TO INCREASE ACCURANCY ! |
| bberabi | 4:120ff05a7c27 | 184 | |
| aungriah | 0:a3b83d366423 | 185 | float rawDistance = (tofs[destination] * 300 * TIMEUNITS_TO_US / 4); |
| aungriah | 0:a3b83d366423 | 186 | //float correctDistance = rawDistance + dwt_getrangebias(7, rawDistance, DWT_PRF_64M); |
| aungriah | 0:a3b83d366423 | 187 | |
| aungriah | 0:a3b83d366423 | 188 | //if(rawDistance <= 8.458) |
| aungriah | 0:a3b83d366423 | 189 | // rawDistance -= 0.0541*rawDistance; // Correction Term 22-03-2017 |
| aungriah | 0:a3b83d366423 | 190 | //else |
| bberabi | 4:120ff05a7c27 | 191 | |
| bberabi | 4:120ff05a7c27 | 192 | if(rawDistance >= 22.7) |
| bberabi | 4:120ff05a7c27 | 193 | rawDistance += -0.0004*rawDistance - 0.3971; |
| bberabi | 4:120ff05a7c27 | 194 | else if (rawDistance >= 14.3) |
| bberabi | 4:120ff05a7c27 | 195 | rawDistance += -0.0015*rawDistance - 0.372; |
| bberabi | 4:120ff05a7c27 | 196 | else if (rawDistance >= 8) |
| bberabi | 4:120ff05a7c27 | 197 | rawDistance += -0.0029*rawDistance - 0.352; |
| bberabi | 4:120ff05a7c27 | 198 | else if (rawDistance >= 3.93) |
| bberabi | 4:120ff05a7c27 | 199 | rawDistance += 0.001*rawDistance - 0.370; |
| bberabi | 4:120ff05a7c27 | 200 | else |
| bberabi | 4:120ff05a7c27 | 201 | rawDistance += -0.0235*rawDistance - 0.273; |
| aungriah | 0:a3b83d366423 | 202 | |
| aungriah | 0:a3b83d366423 | 203 | //else if (rawDistance >= 3) |
| aungriah | 0:a3b83d366423 | 204 | // rawDistance += 0.0004*rawDistance - 0.5556 |
| aungriah | 0:a3b83d366423 | 205 | /* else |
| aungriah | 0:a3b83d366423 | 206 | rawDistance += -0.01799*rawDistance - 0.2724; |
| aungriah | 0:a3b83d366423 | 207 | |
| aungriah | 0:a3b83d366423 | 208 | else if ()*/ |
| aungriah | 0:a3b83d366423 | 209 | |
| aungriah | 0:a3b83d366423 | 210 | |
| aungriah | 0:a3b83d366423 | 211 | //Non-Correction-Term: rawDistance -= 0.458; |
| aungriah | 0:a3b83d366423 | 212 | |
| aungriah | 0:a3b83d366423 | 213 | |
| aungriah | 0:a3b83d366423 | 214 | // Calibration for Nucleo 0 (and 1) |
| aungriah | 0:a3b83d366423 | 215 | |
| aungriah | 0:a3b83d366423 | 216 | // if (this->address == 1) rawDistance+= 10; |
| aungriah | 0:a3b83d366423 | 217 | // switch(destination){ |
| aungriah | 0:a3b83d366423 | 218 | // case 2: |
| aungriah | 0:a3b83d366423 | 219 | // return rawDistance * 0.9754 - 0.5004; |
| aungriah | 0:a3b83d366423 | 220 | // case 3: |
| aungriah | 0:a3b83d366423 | 221 | // return rawDistance * 0.9759 - 0.4103; |
| aungriah | 0:a3b83d366423 | 222 | // case 4: |
| aungriah | 0:a3b83d366423 | 223 | // return rawDistance * 0.9798 - 0.5499; |
| aungriah | 0:a3b83d366423 | 224 | // case 5: |
| aungriah | 0:a3b83d366423 | 225 | // return rawDistance * 0.9765 - 0.5169; |
| aungriah | 0:a3b83d366423 | 226 | // } |
| aungriah | 0:a3b83d366423 | 227 | |
| aungriah | 0:a3b83d366423 | 228 | return rawDistance; |
| aungriah | 3:8bee1711d186 | 229 | //return tofs[destination]; |
| aungriah | 0:a3b83d366423 | 230 | } |
| aungriah | 0:a3b83d366423 | 231 | |
| aungriah | 0:a3b83d366423 | 232 | #pragma Otime // Compiler optimize Runtime at the cost of image size |
| aungriah | 0:a3b83d366423 | 233 | void BeaconNode::requestRangingAll() { |
| bberabi | 4:120ff05a7c27 | 234 | //ADRESSES_COUNT can be changed in nodes.h ! |
| aungriah | 0:a3b83d366423 | 235 | for (int i = 0; i < ADRESSES_COUNT; i++) { // Request ranging to all anchors |
| aungriah | 0:a3b83d366423 | 236 | if(i != address){ |
| aungriah | 0:a3b83d366423 | 237 | requestRanging(i); |
| aungriah | 0:a3b83d366423 | 238 | } |
| aungriah | 0:a3b83d366423 | 239 | else |
| aungriah | 0:a3b83d366423 | 240 | distances[i] = -10; |
| aungriah | 0:a3b83d366423 | 241 | } |
| aungriah | 0:a3b83d366423 | 242 | } |
| aungriah | 3:8bee1711d186 | 243 | void BeaconNode::requestRangingInt(uint8_t from,uint8_t to) { |
| bberabi | 4:120ff05a7c27 | 244 | //measures distances to anhors with id in interval [from,to] |
| bberabi | 4:120ff05a7c27 | 245 | //------------- Unfourtunately, it does not work properly, this case needs to be debugged !!! --------- |
| aungriah | 3:8bee1711d186 | 246 | for (int i=from; i <= to; i++){ |
| aungriah | 3:8bee1711d186 | 247 | if (i!= address) { |
| aungriah | 3:8bee1711d186 | 248 | requestRanging(i); |
| aungriah | 3:8bee1711d186 | 249 | } |
| aungriah | 3:8bee1711d186 | 250 | else |
| aungriah | 3:8bee1711d186 | 251 | distances[i] = -10; |
| aungriah | 3:8bee1711d186 | 252 | } |
| aungriah | 3:8bee1711d186 | 253 | } |
| aungriah | 0:a3b83d366423 | 254 | #pragma Otime // Compiler optimize Runtime at the cost of image size |
| aungriah | 0:a3b83d366423 | 255 | void BeaconNode::sendPingFrame(uint8_t destination) { |
| aungriah | 0:a3b83d366423 | 256 | rangingFrame.source = address; |
| aungriah | 0:a3b83d366423 | 257 | rangingFrame.destination = destination; |
| aungriah | 0:a3b83d366423 | 258 | rangingFrame.type = PING; |
| aungriah | 0:a3b83d366423 | 259 | //dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame)); |
| aungriah | 0:a3b83d366423 | 260 | dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), 0, 0); |
| aungriah | 0:a3b83d366423 | 261 | } |
| aungriah | 0:a3b83d366423 | 262 | |
| aungriah | 0:a3b83d366423 | 263 | #pragma Otime // Compiler optimize Runtime at the cost of image size |
| aungriah | 0:a3b83d366423 | 264 | void BeaconNode::sendBaseAck() { |
| aungriah | 0:a3b83d366423 | 265 | rangingFrame.source = address; |
| aungriah | 0:a3b83d366423 | 266 | rangingFrame.destination = BASE_STATION_ADDR; |
| aungriah | 0:a3b83d366423 | 267 | rangingFrame.type = BASE_ORDER_ACK; |
| aungriah | 0:a3b83d366423 | 268 | //dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame)); |
| aungriah | 0:a3b83d366423 | 269 | dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), 0, 0); |
| aungriah | 0:a3b83d366423 | 270 | } |
| aungriah | 0:a3b83d366423 | 271 | |
| aungriah | 0:a3b83d366423 | 272 | #pragma Otime // Compiler optimize Runtime at the cost of image size |
| aungriah | 0:a3b83d366423 | 273 | void BeaconNode::sendStreamFrame(uint8_t anchor_addr) { |
| aungriah | 0:a3b83d366423 | 274 | StreamFrame streamFrame; |
| aungriah | 0:a3b83d366423 | 275 | |
| aungriah | 0:a3b83d366423 | 276 | streamFrame.source = address; |
| aungriah | 0:a3b83d366423 | 277 | streamFrame.destination = BASE_STATION_ADDR; |
| aungriah | 0:a3b83d366423 | 278 | streamFrame.type = STREAM_TO_BASE; |
| aungriah | 0:a3b83d366423 | 279 | streamFrame.anchor_adress = anchor_addr; |
| aungriah | 0:a3b83d366423 | 280 | streamFrame.distance = getDistance(anchor_addr); |
| aungriah | 0:a3b83d366423 | 281 | streamFrame.signalStrength = getSignalStrength(anchor_addr); |
| aungriah | 0:a3b83d366423 | 282 | streamFrame.FPLevel = dw.getFPLevel(); |
| aungriah | 0:a3b83d366423 | 283 | |
| aungriah | 0:a3b83d366423 | 284 | dw.sendFrame((uint8_t*)&streamFrame, sizeof(streamFrame), 0, 0); |
| aungriah | 0:a3b83d366423 | 285 | } |
| aungriah | 0:a3b83d366423 | 286 | |
| aungriah | 0:a3b83d366423 | 287 | #pragma Otime // Compiler optimize Runtime at the cost of image size |
| aungriah | 0:a3b83d366423 | 288 | void BeaconNode::sendAnswer(uint8_t destination, uint8_t type) { |
| aungriah | 0:a3b83d366423 | 289 | |
| aungriah | 0:a3b83d366423 | 290 | rangingFrame.source = address; |
| aungriah | 0:a3b83d366423 | 291 | rangingFrame.destination = destination; |
| aungriah | 0:a3b83d366423 | 292 | rangingFrame.type = type; |
| aungriah | 0:a3b83d366423 | 293 | |
| aungriah | 0:a3b83d366423 | 294 | dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), 0, 0); |
| aungriah | 0:a3b83d366423 | 295 | } |
| aungriah | 0:a3b83d366423 | 296 | |
| aungriah | 0:a3b83d366423 | 297 | #pragma Otime // Compiler optimize Runtime at the cost of image size |
| aungriah | 0:a3b83d366423 | 298 | void BeaconNode::correctSenderTimestamps(uint8_t source){ |
| aungriah | 0:a3b83d366423 | 299 | |
| aungriah | 0:a3b83d366423 | 300 | if (senderTimestamps[source][0] - dw.getAntennaDelay() > senderTimestamps[source][1]) { |
| aungriah | 0:a3b83d366423 | 301 | senderTimestamps[source][1] += MMRANGING_2POWER40; |
| aungriah | 0:a3b83d366423 | 302 | senderTimestamps[source][2] += MMRANGING_2POWER40; |
| aungriah | 0:a3b83d366423 | 303 | } |
| aungriah | 0:a3b83d366423 | 304 | if (senderTimestamps[source][1] > senderTimestamps[source][2]) { |
| aungriah | 0:a3b83d366423 | 305 | senderTimestamps[source][2] += MMRANGING_2POWER40; |
| aungriah | 0:a3b83d366423 | 306 | } |
| aungriah | 0:a3b83d366423 | 307 | |
| aungriah | 0:a3b83d366423 | 308 | } |
| aungriah | 0:a3b83d366423 | 309 | |
| aungriah | 0:a3b83d366423 | 310 | float BeaconNode::getSignalStrength(uint8_t index){ |
| aungriah | 0:a3b83d366423 | 311 | return signalStrength[index]; |
| aungriah | 0:a3b83d366423 | 312 | } |
| aungriah | 0:a3b83d366423 | 313 | |
| aungriah | 0:a3b83d366423 | 314 | float BeaconNode::getDistance(uint8_t index){ |
| aungriah | 0:a3b83d366423 | 315 | return distances[index]; |
| aungriah | 0:a3b83d366423 | 316 | } |
| aungriah | 0:a3b83d366423 | 317 | |
| aungriah | 0:a3b83d366423 | 318 | uint8* BeaconNode::getRecFrameRef(){ |
| aungriah | 0:a3b83d366423 | 319 | return (uint8 *) &receivedFrame; |
| aungriah | 0:a3b83d366423 | 320 | } |
| aungriah | 0:a3b83d366423 | 321 | uint16 BeaconNode::getRecFrameLength(){ |
| aungriah | 0:a3b83d366423 | 322 | return sizeof(receivedFrame); |
| aungriah | 0:a3b83d366423 | 323 | } |
| aungriah | 0:a3b83d366423 | 324 | |
| aungriah | 0:a3b83d366423 | 325 | int BeaconNode::getMode(){ |
| aungriah | 0:a3b83d366423 | 326 | return mode; |
| aungriah | 0:a3b83d366423 | 327 | } |
| aungriah | 0:a3b83d366423 | 328 | |
| aungriah | 0:a3b83d366423 | 329 | uint16_t BeaconNode::getRepetitions(){ |
| aungriah | 0:a3b83d366423 | 330 | return repetitions; |
| aungriah | 0:a3b83d366423 | 331 | } |
| aungriah | 0:a3b83d366423 | 332 | void BeaconNode::decreaseRepetitions(){ |
| aungriah | 0:a3b83d366423 | 333 | repetitions--; |
| aungriah | 0:a3b83d366423 | 334 | } |
| aungriah | 0:a3b83d366423 | 335 | uint8_t BeaconNode::getDestination(){ |
| aungriah | 0:a3b83d366423 | 336 | return destination; |
| aungriah | 0:a3b83d366423 | 337 | } |
| aungriah | 0:a3b83d366423 | 338 | |
| bberabi | 4:120ff05a7c27 | 339 | void BeaconNode::clearRec(){ //the fufnction clears the noRec array ! |
| aungriah | 0:a3b83d366423 | 340 | for(int j = 0; j < ADRESSES_COUNT; j++) |
| aungriah | 0:a3b83d366423 | 341 | noRec[j] = 0; |
| aungriah | 0:a3b83d366423 | 342 | } |
| aungriah | 0:a3b83d366423 | 343 | |
| aungriah | 0:a3b83d366423 | 344 | |
| aungriah | 0:a3b83d366423 | 345 | //------- AnchorNode Class ------------------- |
| aungriah | 0:a3b83d366423 | 346 | AnchorNode::AnchorNode(DecaWave& DW) : Node(DW) { |
| bberabi | 4:120ff05a7c27 | 347 | Anchor = true; //Since it is an anchor |
| aungriah | 0:a3b83d366423 | 348 | } |
| aungriah | 0:a3b83d366423 | 349 | |
| aungriah | 0:a3b83d366423 | 350 | #pragma Otime // Compiler optimize Runtime at the cost of image size |
| aungriah | 0:a3b83d366423 | 351 | // Two Way Ranging Actions when Frame was received |
| aungriah | 0:a3b83d366423 | 352 | void AnchorNode::callbackRX(uint64_t RxTime) { |
| aungriah | 0:a3b83d366423 | 353 | // pc.printf("Got a frame, adress: %d source: %d \r\n", receivedFrame.destination, receivedFrame.source); |
| aungriah | 0:a3b83d366423 | 354 | // if(!isBaseStation()){ |
| aungriah | 0:a3b83d366423 | 355 | if (receivedFrame.destination == address){ |
| aungriah | 0:a3b83d366423 | 356 | switch (receivedFrame.type) { |
| bberabi | 4:120ff05a7c27 | 357 | case SWITCH_TYPE: //as explained in beaconnode class |
| aungriah | 0:a3b83d366423 | 358 | sendBaseAck(); |
| aungriah | 0:a3b83d366423 | 359 | if((receivedFrame.signedTime >> 24) == BECOME_BEACON){ |
| aungriah | 0:a3b83d366423 | 360 | node = &beaconNode; |
| aungriah | 0:a3b83d366423 | 361 | pc.printf("\r\n \r\n ---This Node is a beacon now ---\r\n \r\n"); |
| aungriah | 0:a3b83d366423 | 362 | } |
| aungriah | 0:a3b83d366423 | 363 | else if((receivedFrame.signedTime >> 24) == BECOME_ANCHOR){ |
| aungriah | 0:a3b83d366423 | 364 | pc.printf("\r\n \r\n ---This Node is already an anchor ---\r\n \r\n"); |
| aungriah | 0:a3b83d366423 | 365 | } |
| aungriah | 0:a3b83d366423 | 366 | break; |
| bberabi | 4:120ff05a7c27 | 367 | case PING: |
| aungriah | 0:a3b83d366423 | 368 | sendAnswer(receivedFrame.source, ANCHOR_RESPONSE); |
| aungriah | 0:a3b83d366423 | 369 | receiverTimestamps[receivedFrame.source][0] = RxTime; //Save the first timestamp on the receiving node/anchor (T_rp) |
| aungriah | 3:8bee1711d186 | 370 | //dw.turnonrx(); |
| aungriah | 0:a3b83d366423 | 371 | break; |
| bberabi | 4:120ff05a7c27 | 372 | case BEACON_RESPONSE: |
| aungriah | 0:a3b83d366423 | 373 | { |
| aungriah | 0:a3b83d366423 | 374 | receiverTimestamps[receivedFrame.source][2] = RxTime; //Save the third timestamp on the receiving node/anchor (T_rf) |
| aungriah | 0:a3b83d366423 | 375 | correctReceiverTimestamps(receivedFrame.source); //Correct the timestamps for the case of a counter overflow |
| aungriah | 0:a3b83d366423 | 376 | //timediffRec = receiverTimestamps[receivedFrame.source][0] + receiverTimestamps[receivedFrame.source][2] - 2*receiverTimestamps[receivedFrame.source][1]; |
| aungriah | 0:a3b83d366423 | 377 | //if(timediffRec < 0) |
| aungriah | 0:a3b83d366423 | 378 | // timediffRec = 0; |
| aungriah | 0:a3b83d366423 | 379 | sendTransferFrame(receivedFrame.source, receiverTimestamps[receivedFrame.source][0] + receiverTimestamps[receivedFrame.source][2] - 2*receiverTimestamps[receivedFrame.source][1]); |
| aungriah | 3:8bee1711d186 | 380 | //dw.turnonrx(); |
| aungriah | 0:a3b83d366423 | 381 | break; |
| aungriah | 0:a3b83d366423 | 382 | } |
| aungriah | 3:8bee1711d186 | 383 | default : |
| bberabi | 4:120ff05a7c27 | 384 | dw.turnonrx(); //turn on listening mode |
| aungriah | 3:8bee1711d186 | 385 | break; |
| aungriah | 0:a3b83d366423 | 386 | } |
| aungriah | 0:a3b83d366423 | 387 | } |
| aungriah | 0:a3b83d366423 | 388 | else{ |
| aungriah | 0:a3b83d366423 | 389 | dw.turnonrx(); // start listening again |
| aungriah | 0:a3b83d366423 | 390 | } |
| aungriah | 0:a3b83d366423 | 391 | } |
| aungriah | 0:a3b83d366423 | 392 | |
| aungriah | 0:a3b83d366423 | 393 | |
| aungriah | 0:a3b83d366423 | 394 | #pragma Otime // Compiler optimize Runtime at the cost of image size |
| aungriah | 0:a3b83d366423 | 395 | // Two Way Ranging Actions when Frame was transmitted |
| aungriah | 0:a3b83d366423 | 396 | void AnchorNode::callbackTX(uint64_t TxTime) { |
| aungriah | 0:a3b83d366423 | 397 | dw.turnonrx(); // start listening again |
| aungriah | 0:a3b83d366423 | 398 | |
| aungriah | 0:a3b83d366423 | 399 | switch (rangingFrame.type) { |
| aungriah | 0:a3b83d366423 | 400 | case ANCHOR_RESPONSE: |
| aungriah | 0:a3b83d366423 | 401 | receiverTimestamps[rangingFrame.destination][1] = TxTime; //Save the second timestamp on the receiving node/anchor (T_sr) |
| aungriah | 0:a3b83d366423 | 402 | break; |
| aungriah | 0:a3b83d366423 | 403 | default: |
| aungriah | 0:a3b83d366423 | 404 | break; |
| aungriah | 0:a3b83d366423 | 405 | } |
| aungriah | 0:a3b83d366423 | 406 | } |
| aungriah | 0:a3b83d366423 | 407 | |
| aungriah | 0:a3b83d366423 | 408 | |
| aungriah | 0:a3b83d366423 | 409 | |
| aungriah | 0:a3b83d366423 | 410 | |
| aungriah | 0:a3b83d366423 | 411 | #pragma Otime // Compiler optimize Runtime at the cost of image size |
| bberabi | 4:120ff05a7c27 | 412 | void AnchorNode::sendBaseAck() { //send base ack that the order is received |
| aungriah | 0:a3b83d366423 | 413 | rangingFrame.source = address; |
| aungriah | 0:a3b83d366423 | 414 | rangingFrame.destination = BASE_STATION_ADDR; |
| aungriah | 0:a3b83d366423 | 415 | rangingFrame.type = BASE_ORDER_ACK; |
| aungriah | 0:a3b83d366423 | 416 | //dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame)); |
| bberabi | 4:120ff05a7c27 | 417 | dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), 0, 0); //function for sending messages(frames) |
| 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 | #pragma Otime // Compiler optimize Runtime at the cost of image size |
| aungriah | 0:a3b83d366423 | 425 | void AnchorNode::sendTransferFrame(uint8_t destination, int timeDiffsReceiver) { |
| aungriah | 0:a3b83d366423 | 426 | ExtendedRangingFrame transferFrame; |
| aungriah | 0:a3b83d366423 | 427 | |
| aungriah | 0:a3b83d366423 | 428 | transferFrame.source = address; |
| aungriah | 0:a3b83d366423 | 429 | transferFrame.destination = destination; |
| aungriah | 0:a3b83d366423 | 430 | transferFrame.type = TRANSFER_FRAME; |
| aungriah | 0:a3b83d366423 | 431 | transferFrame.signedTime = timeDiffsReceiver; //cast the time difference |
| aungriah | 0:a3b83d366423 | 432 | dw.sendFrame((uint8_t*)&transferFrame, sizeof(transferFrame), 0, 0); |
| aungriah | 0:a3b83d366423 | 433 | } |
| aungriah | 0:a3b83d366423 | 434 | |
| aungriah | 0:a3b83d366423 | 435 | #pragma Otime // Compiler optimize Runtime at the cost of image size |
| aungriah | 0:a3b83d366423 | 436 | void AnchorNode::sendAnswer(uint8_t destination, uint8_t type) { |
| aungriah | 0:a3b83d366423 | 437 | |
| aungriah | 0:a3b83d366423 | 438 | rangingFrame.source = address; |
| aungriah | 0:a3b83d366423 | 439 | rangingFrame.destination = destination; |
| aungriah | 0:a3b83d366423 | 440 | rangingFrame.type = type; |
| aungriah | 0:a3b83d366423 | 441 | |
| aungriah | 0:a3b83d366423 | 442 | dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), 0, 0); |
| aungriah | 0:a3b83d366423 | 443 | } |
| aungriah | 0:a3b83d366423 | 444 | |
| aungriah | 0:a3b83d366423 | 445 | #pragma Otime // Compiler optimize Runtime at the cost of image size |
| aungriah | 0:a3b83d366423 | 446 | void AnchorNode::correctReceiverTimestamps(uint8_t source){ |
| aungriah | 0:a3b83d366423 | 447 | |
| aungriah | 0:a3b83d366423 | 448 | if(receiverTimestamps[source][0] > receiverTimestamps[source][1]){ |
| aungriah | 0:a3b83d366423 | 449 | receiverTimestamps[source][1] += MMRANGING_2POWER40; |
| aungriah | 0:a3b83d366423 | 450 | receiverTimestamps[source][2] += MMRANGING_2POWER40; |
| aungriah | 0:a3b83d366423 | 451 | } |
| aungriah | 0:a3b83d366423 | 452 | |
| aungriah | 0:a3b83d366423 | 453 | if(receiverTimestamps[source][1] - dw.getAntennaDelay() > receiverTimestamps[source][2]){ |
| aungriah | 0:a3b83d366423 | 454 | receiverTimestamps[source][2] += MMRANGING_2POWER40; |
| aungriah | 0:a3b83d366423 | 455 | } |
| aungriah | 0:a3b83d366423 | 456 | |
| aungriah | 0:a3b83d366423 | 457 | } |
| aungriah | 0:a3b83d366423 | 458 | |
| aungriah | 0:a3b83d366423 | 459 | |
| aungriah | 0:a3b83d366423 | 460 | |
| aungriah | 0:a3b83d366423 | 461 | uint8* AnchorNode::getRecFrameRef(){ |
| aungriah | 0:a3b83d366423 | 462 | return (uint8 *) &receivedFrame; |
| aungriah | 0:a3b83d366423 | 463 | } |
| aungriah | 0:a3b83d366423 | 464 | uint16 AnchorNode::getRecFrameLength(){ |
| aungriah | 0:a3b83d366423 | 465 | return sizeof(receivedFrame); |
| aungriah | 0:a3b83d366423 | 466 | } |
| aungriah | 0:a3b83d366423 | 467 | |
| aungriah | 0:a3b83d366423 | 468 | |
| aungriah | 0:a3b83d366423 | 469 | //------- BaseStationNode Class ------------------- |
| bberabi | 4:120ff05a7c27 | 470 | |
| aungriah | 0:a3b83d366423 | 471 | BaseStationNode::BaseStationNode(DecaWave& DW) : Node(DW) { |
| aungriah | 0:a3b83d366423 | 472 | Anchor = false; |
| aungriah | 0:a3b83d366423 | 473 | LocalTimer.start(); |
| aungriah | 0:a3b83d366423 | 474 | } |
| aungriah | 0:a3b83d366423 | 475 | |
| aungriah | 0:a3b83d366423 | 476 | uint8* BaseStationNode::getRecFrameRef(){ |
| aungriah | 0:a3b83d366423 | 477 | return (uint8 *) &receivedStreamFrame; |
| aungriah | 0:a3b83d366423 | 478 | } |
| aungriah | 0:a3b83d366423 | 479 | uint16 BaseStationNode::getRecFrameLength(){ |
| aungriah | 0:a3b83d366423 | 480 | return sizeof(receivedStreamFrame); |
| aungriah | 0:a3b83d366423 | 481 | } |
| aungriah | 0:a3b83d366423 | 482 | |
| aungriah | 0:a3b83d366423 | 483 | |
| aungriah | 0:a3b83d366423 | 484 | #pragma Otime // Compiler optimize Runtime at the cost of image size |
| aungriah | 0:a3b83d366423 | 485 | // Two Way Ranging Actions when Frame was received |
| aungriah | 0:a3b83d366423 | 486 | void BaseStationNode::callbackRX(uint64_t RxTime) { |
| aungriah | 0:a3b83d366423 | 487 | //pc.printf("Got a frame, adress: %d source: %d \r\n", receivedStreamFrame.destination, receivedStreamFrame.source); |
| aungriah | 0:a3b83d366423 | 488 | if (receivedStreamFrame.destination == address){ |
| aungriah | 0:a3b83d366423 | 489 | switch(receivedStreamFrame.type){ |
| bberabi | 4:120ff05a7c27 | 490 | case STREAM_TO_BASE: //send the received string from tag to the matlab with pc.printf |
| aungriah | 0:a3b83d366423 | 491 | |
| aungriah | 0:a3b83d366423 | 492 | //pc.printf("#%d to #%d %f(%f) \r\n", receivedStreamFrame.source, receivedStreamFrame.anchor_adress, receivedStreamFrame.distance, receivedStreamFrame.signalStrength); |
| aungriah | 0:a3b83d366423 | 493 | 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 | 494 | break; |
| aungriah | 0:a3b83d366423 | 495 | case BASE_ORDER_ACK: |
| aungriah | 0:a3b83d366423 | 496 | |
| bberabi | 4:120ff05a7c27 | 497 | ack = true; // so that base station knows order was received |
| aungriah | 0:a3b83d366423 | 498 | break; |
| aungriah | 3:8bee1711d186 | 499 | |
| aungriah | 3:8bee1711d186 | 500 | default: |
| aungriah | 3:8bee1711d186 | 501 | //dw.turnonrx(); |
| aungriah | 3:8bee1711d186 | 502 | break; |
| bberabi | 2:5adf0b785944 | 503 | |
| bberabi | 2:5adf0b785944 | 504 | |
| aungriah | 0:a3b83d366423 | 505 | |
| aungriah | 0:a3b83d366423 | 506 | |
| aungriah | 0:a3b83d366423 | 507 | } |
| aungriah | 0:a3b83d366423 | 508 | } |
| aungriah | 0:a3b83d366423 | 509 | dw.turnonrx(); // start listening again |
| aungriah | 0:a3b83d366423 | 510 | |
| aungriah | 0:a3b83d366423 | 511 | |
| aungriah | 0:a3b83d366423 | 512 | } |
| aungriah | 0:a3b83d366423 | 513 | |
| aungriah | 0:a3b83d366423 | 514 | #pragma Otime // Compiler optimize Runtime at the cost of image size |
| aungriah | 0:a3b83d366423 | 515 | // Two Way Ranging Actions when Frame was transmitted |
| aungriah | 0:a3b83d366423 | 516 | void BaseStationNode::callbackTX(uint64_t TxTime) { |
| aungriah | 0:a3b83d366423 | 517 | dw.turnonrx(); // start listening again |
| aungriah | 0:a3b83d366423 | 518 | } |
| aungriah | 0:a3b83d366423 | 519 | |
| aungriah | 3:8bee1711d186 | 520 | |
| aungriah | 3:8bee1711d186 | 521 | |
| aungriah | 0:a3b83d366423 | 522 | #pragma Otime // Compiler optimize Runtime at the cost of image size |
| aungriah | 0:a3b83d366423 | 523 | void BaseStationNode::sendOrder(uint8_t beacon_destination, uint8_t anchor_destination, uint8_t action, uint16_t repetitions, uint8_t type) { |
| aungriah | 0:a3b83d366423 | 524 | ExtendedRangingFrame orderFrame; |
| aungriah | 0:a3b83d366423 | 525 | ack = false; |
| bberabi | 4:120ff05a7c27 | 526 | //the function is used to send orders one has to create and order frame |
| aungriah | 0:a3b83d366423 | 527 | orderFrame.source = address; |
| aungriah | 0:a3b83d366423 | 528 | orderFrame.destination = beacon_destination; |
| aungriah | 0:a3b83d366423 | 529 | orderFrame.type = type; |
| aungriah | 3:8bee1711d186 | 530 | if(action == 1){ |
| aungriah | 3:8bee1711d186 | 531 | orderFrame.signedTime = action << 24 | anchor_destination << 16 | 1; |
| aungriah | 3:8bee1711d186 | 532 | } |
| aungriah | 3:8bee1711d186 | 533 | else |
| aungriah | 3:8bee1711d186 | 534 | orderFrame.signedTime = action << 24 | anchor_destination << 16 | repetitions; |
| aungriah | 0:a3b83d366423 | 535 | |
| aungriah | 0:a3b83d366423 | 536 | int i = 0; |
| bberabi | 2:5adf0b785944 | 537 | for(i = 0; i < 3 && !ack; i++){ |
| aungriah | 0:a3b83d366423 | 538 | float time_before = LocalTimer.read(); |
| aungriah | 0:a3b83d366423 | 539 | dw.sendFrame((uint8_t*)&orderFrame, sizeof(orderFrame), 0, 0); |
| aungriah | 0:a3b83d366423 | 540 | while(!ack && (LocalTimer.read() < time_before + 0.010 + 0.01*i)); // One Ranging normaly takes less than 1.5 miliseconds |
| bberabi | 2:5adf0b785944 | 541 | } |
| aungriah | 0:a3b83d366423 | 542 | if(!ack) |
| aungriah | 0:a3b83d366423 | 543 | { |
| aungriah | 0:a3b83d366423 | 544 | pc.printf("ERROR: Tag #%d did not respond \r\n", beacon_destination); |
| aungriah | 3:8bee1711d186 | 545 | |
| aungriah | 0:a3b83d366423 | 546 | } |
| aungriah | 0:a3b83d366423 | 547 | else |
| aungriah | 0:a3b83d366423 | 548 | { |
| bberabi | 1:346279def7ac | 549 | // pc.printf("Order sent, %d tries \r\n", i); |
| aungriah | 0:a3b83d366423 | 550 | } |
| aungriah | 3:8bee1711d186 | 551 | |
| aungriah | 0:a3b83d366423 | 552 | |
| aungriah | 0:a3b83d366423 | 553 | |
| aungriah | 0:a3b83d366423 | 554 | } |