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.

Dependencies:   mbed-dev

Fork of GA-Berkay_Alex by Alejandro Ungria Hirte

nodes/nodes.cpp@2:5adf0b785944, 2018-01-10 (annotated)

Committer:
bberabi
Date:
Wed Jan 10 16:14:49 2018 +0000
Revision:
2:5adf0b785944
Parent:
1:346279def7ac
Child:
3:8bee1711d186
milc

Who changed what in which revision?

UserRevisionLine numberNew 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
bberabi 2:5adf0b785944 90 // dw.turnonrx(); // start listening again
aungriah 0:a3b83d366423 91 break;
aungriah 0:a3b83d366423 92 }
bberabi 2:5adf0b785944 93 default : dw.turnonrx();
bberabi 2:5adf0b785944 94 break;
aungriah 0:a3b83d366423 95 }
aungriah 0:a3b83d366423 96 }
aungriah 0:a3b83d366423 97 else{
aungriah 0:a3b83d366423 98 dw.turnonrx(); // start listening again
aungriah 0:a3b83d366423 99 }
aungriah 0:a3b83d366423 100 }
aungriah 0:a3b83d366423 101
aungriah 0:a3b83d366423 102
aungriah 0:a3b83d366423 103 #pragma Otime // Compiler optimize Runtime at the cost of image size
aungriah 0:a3b83d366423 104 // Two Way Ranging Actions when Frame was transmitted
aungriah 0:a3b83d366423 105 void BeaconNode::callbackTX(uint64_t TxTime) {
aungriah 0:a3b83d366423 106 //pc.printf("TXCallback: %d %d %d \r\n", acknowledgement[0], acknowledgement[1], acknowledgement[2]);
aungriah 0:a3b83d366423 107 dw.turnonrx(); // start listening again
aungriah 0:a3b83d366423 108
aungriah 0:a3b83d366423 109 switch (rangingFrame.type) {
aungriah 0:a3b83d366423 110 case PING:
aungriah 0:a3b83d366423 111 senderTimestamps[rangingFrame.destination][0] = TxTime; //Save the first timestamp on the sending node/beacon (T_sp)
aungriah 0:a3b83d366423 112 break;
aungriah 0:a3b83d366423 113 case BEACON_RESPONSE:
aungriah 0:a3b83d366423 114 senderTimestamps[rangingFrame.destination][2] = TxTime; //Save the third timestamp on the sending node/beacon (T_sr)
aungriah 0:a3b83d366423 115 correctSenderTimestamps(rangingFrame.destination); //Correct the timestamps for the case of a counter overflow
aungriah 0:a3b83d366423 116 break;
aungriah 0:a3b83d366423 117 default:
aungriah 0:a3b83d366423 118 break;
aungriah 0:a3b83d366423 119 }
aungriah 0:a3b83d366423 120 if(acknowledgement[1] == true){
aungriah 0:a3b83d366423 121 acknowledgement[2] = true;
aungriah 0:a3b83d366423 122 // pc.printf("Ack edited %d \r\n", acknowledgement[2]);
aungriah 0:a3b83d366423 123 }
aungriah 0:a3b83d366423 124 }
aungriah 0:a3b83d366423 125
aungriah 0:a3b83d366423 126
aungriah 0:a3b83d366423 127 #pragma Otime // Compiler optimize Runtime at the cost of image size
aungriah 0:a3b83d366423 128 /**
aungriah 0:a3b83d366423 129 * Get the distance to the Anchor with address @param destination.
aungriah 0:a3b83d366423 130 *
aungriah 0:a3b83d366423 131 * @param destination The address of the anchor
aungriah 0:a3b83d366423 132 */
aungriah 0:a3b83d366423 133 void BeaconNode::requestRanging(uint8_t destination) {
aungriah 0:a3b83d366423 134 if(noRec[destination] <= MAX_TRIES){
bberabi 2:5adf0b785944 135 pc.printf(" max try %d\n\r",MAX_TRIES);
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 {
bberabi 2:5adf0b785944 155 acknowledgement[1]=1;
aungriah 0:a3b83d366423 156 distances[destination] = -10;
bberabi 2:5adf0b785944 157 noRec[destination] = 0;
bberabi 2:5adf0b785944 158 sendStreamFrame(destination);
bberabi 2:5adf0b785944 159
bberabi 2:5adf0b785944 160
aungriah 0:a3b83d366423 161 }
aungriah 0:a3b83d366423 162 }
bberabi 2:5adf0b785944 163 pc.printf("no rec dest %d\n\r",noRec[destination]);
aungriah 0:a3b83d366423 164 }
aungriah 0:a3b83d366423 165
aungriah 0:a3b83d366423 166 #pragma Otime // Compiler optimize Runtime at the cost of image size
aungriah 0:a3b83d366423 167 inline float BeaconNode::calibratedDistance(uint8_t destination) {
aungriah 0:a3b83d366423 168
aungriah 0:a3b83d366423 169 float rawDistance = (tofs[destination] * 300 * TIMEUNITS_TO_US / 4);
aungriah 0:a3b83d366423 170 //float correctDistance = rawDistance + dwt_getrangebias(7, rawDistance, DWT_PRF_64M);
aungriah 0:a3b83d366423 171
aungriah 0:a3b83d366423 172 //if(rawDistance <= 8.458)
aungriah 0:a3b83d366423 173 // rawDistance -= 0.0541*rawDistance; // Correction Term 22-03-2017
aungriah 0:a3b83d366423 174 //else
aungriah 0:a3b83d366423 175 if(rawDistance >= 22.7)
aungriah 0:a3b83d366423 176 rawDistance += -0.0004*rawDistance - 0.3971;
aungriah 0:a3b83d366423 177 else if (rawDistance >= 14.3)
aungriah 0:a3b83d366423 178 rawDistance += -0.0015*rawDistance - 0.372;
aungriah 0:a3b83d366423 179 else if (rawDistance >= 8)
aungriah 0:a3b83d366423 180 rawDistance += -0.0029*rawDistance - 0.352;
aungriah 0:a3b83d366423 181 else if (rawDistance >= 3.93)
aungriah 0:a3b83d366423 182 rawDistance += 0.001*rawDistance - 0.370;
aungriah 0:a3b83d366423 183 else
aungriah 0:a3b83d366423 184 rawDistance += -0.0235*rawDistance - 0.273;
aungriah 0:a3b83d366423 185
aungriah 0:a3b83d366423 186 //else if (rawDistance >= 3)
aungriah 0:a3b83d366423 187 // rawDistance += 0.0004*rawDistance - 0.5556
aungriah 0:a3b83d366423 188 /* else
aungriah 0:a3b83d366423 189 rawDistance += -0.01799*rawDistance - 0.2724;
aungriah 0:a3b83d366423 190
aungriah 0:a3b83d366423 191 else if ()*/
aungriah 0:a3b83d366423 192
aungriah 0:a3b83d366423 193
aungriah 0:a3b83d366423 194 //Non-Correction-Term: rawDistance -= 0.458;
aungriah 0:a3b83d366423 195
aungriah 0:a3b83d366423 196
aungriah 0:a3b83d366423 197 // Calibration for Nucleo 0 (and 1)
aungriah 0:a3b83d366423 198
aungriah 0:a3b83d366423 199 // if (this->address == 1) rawDistance+= 10;
aungriah 0:a3b83d366423 200 // switch(destination){
aungriah 0:a3b83d366423 201 // case 2:
aungriah 0:a3b83d366423 202 // return rawDistance * 0.9754 - 0.5004;
aungriah 0:a3b83d366423 203 // case 3:
aungriah 0:a3b83d366423 204 // return rawDistance * 0.9759 - 0.4103;
aungriah 0:a3b83d366423 205 // case 4:
aungriah 0:a3b83d366423 206 // return rawDistance * 0.9798 - 0.5499;
aungriah 0:a3b83d366423 207 // case 5:
aungriah 0:a3b83d366423 208 // return rawDistance * 0.9765 - 0.5169;
aungriah 0:a3b83d366423 209 // }
aungriah 0:a3b83d366423 210
aungriah 0:a3b83d366423 211 return rawDistance;
aungriah 0:a3b83d366423 212
aungriah 0:a3b83d366423 213 }
aungriah 0:a3b83d366423 214
aungriah 0:a3b83d366423 215 #pragma Otime // Compiler optimize Runtime at the cost of image size
aungriah 0:a3b83d366423 216 void BeaconNode::requestRangingAll() {
aungriah 0:a3b83d366423 217 for (int i = 0; i < ADRESSES_COUNT; i++) { // Request ranging to all anchors
aungriah 0:a3b83d366423 218 if(i != address){
aungriah 0:a3b83d366423 219 requestRanging(i);
aungriah 0:a3b83d366423 220 }
aungriah 0:a3b83d366423 221 else
aungriah 0:a3b83d366423 222 distances[i] = -10;
aungriah 0:a3b83d366423 223 }
aungriah 0:a3b83d366423 224 }
aungriah 0:a3b83d366423 225
aungriah 0:a3b83d366423 226 #pragma Otime // Compiler optimize Runtime at the cost of image size
aungriah 0:a3b83d366423 227 void BeaconNode::sendPingFrame(uint8_t destination) {
aungriah 0:a3b83d366423 228 rangingFrame.source = address;
aungriah 0:a3b83d366423 229 rangingFrame.destination = destination;
aungriah 0:a3b83d366423 230 rangingFrame.type = PING;
aungriah 0:a3b83d366423 231 //dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame));
aungriah 0:a3b83d366423 232 dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), 0, 0);
aungriah 0:a3b83d366423 233 }
aungriah 0:a3b83d366423 234
aungriah 0:a3b83d366423 235 #pragma Otime // Compiler optimize Runtime at the cost of image size
aungriah 0:a3b83d366423 236 void BeaconNode::sendBaseAck() {
aungriah 0:a3b83d366423 237 rangingFrame.source = address;
aungriah 0:a3b83d366423 238 rangingFrame.destination = BASE_STATION_ADDR;
aungriah 0:a3b83d366423 239 rangingFrame.type = BASE_ORDER_ACK;
aungriah 0:a3b83d366423 240 //dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame));
aungriah 0:a3b83d366423 241 dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), 0, 0);
aungriah 0:a3b83d366423 242 }
aungriah 0:a3b83d366423 243
aungriah 0:a3b83d366423 244 #pragma Otime // Compiler optimize Runtime at the cost of image size
aungriah 0:a3b83d366423 245 void BeaconNode::sendStreamFrame(uint8_t anchor_addr) {
aungriah 0:a3b83d366423 246 StreamFrame streamFrame;
aungriah 0:a3b83d366423 247
aungriah 0:a3b83d366423 248 streamFrame.source = address;
aungriah 0:a3b83d366423 249 streamFrame.destination = BASE_STATION_ADDR;
aungriah 0:a3b83d366423 250 streamFrame.type = STREAM_TO_BASE;
aungriah 0:a3b83d366423 251 streamFrame.anchor_adress = anchor_addr;
aungriah 0:a3b83d366423 252 streamFrame.distance = getDistance(anchor_addr);
aungriah 0:a3b83d366423 253 streamFrame.signalStrength = getSignalStrength(anchor_addr);
aungriah 0:a3b83d366423 254 streamFrame.FPLevel = dw.getFPLevel();
aungriah 0:a3b83d366423 255
aungriah 0:a3b83d366423 256 dw.sendFrame((uint8_t*)&streamFrame, sizeof(streamFrame), 0, 0);
aungriah 0:a3b83d366423 257 }
aungriah 0:a3b83d366423 258
aungriah 0:a3b83d366423 259 #pragma Otime // Compiler optimize Runtime at the cost of image size
aungriah 0:a3b83d366423 260 void BeaconNode::sendAnswer(uint8_t destination, uint8_t type) {
aungriah 0:a3b83d366423 261
aungriah 0:a3b83d366423 262 rangingFrame.source = address;
aungriah 0:a3b83d366423 263 rangingFrame.destination = destination;
aungriah 0:a3b83d366423 264 rangingFrame.type = type;
aungriah 0:a3b83d366423 265
aungriah 0:a3b83d366423 266 dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), 0, 0);
aungriah 0:a3b83d366423 267 }
aungriah 0:a3b83d366423 268
aungriah 0:a3b83d366423 269 #pragma Otime // Compiler optimize Runtime at the cost of image size
aungriah 0:a3b83d366423 270 void BeaconNode::correctSenderTimestamps(uint8_t source){
aungriah 0:a3b83d366423 271
aungriah 0:a3b83d366423 272 if (senderTimestamps[source][0] - dw.getAntennaDelay() > senderTimestamps[source][1]) {
aungriah 0:a3b83d366423 273 senderTimestamps[source][1] += MMRANGING_2POWER40;
aungriah 0:a3b83d366423 274 senderTimestamps[source][2] += MMRANGING_2POWER40;
aungriah 0:a3b83d366423 275 }
aungriah 0:a3b83d366423 276 if (senderTimestamps[source][1] > senderTimestamps[source][2]) {
aungriah 0:a3b83d366423 277 senderTimestamps[source][2] += MMRANGING_2POWER40;
aungriah 0:a3b83d366423 278 }
aungriah 0:a3b83d366423 279
aungriah 0:a3b83d366423 280 }
aungriah 0:a3b83d366423 281
aungriah 0:a3b83d366423 282 float BeaconNode::getSignalStrength(uint8_t index){
aungriah 0:a3b83d366423 283 return signalStrength[index];
aungriah 0:a3b83d366423 284 }
aungriah 0:a3b83d366423 285
aungriah 0:a3b83d366423 286 float BeaconNode::getDistance(uint8_t index){
aungriah 0:a3b83d366423 287 return distances[index];
aungriah 0:a3b83d366423 288 }
aungriah 0:a3b83d366423 289
aungriah 0:a3b83d366423 290 uint8* BeaconNode::getRecFrameRef(){
aungriah 0:a3b83d366423 291 return (uint8 *) &receivedFrame;
aungriah 0:a3b83d366423 292 }
aungriah 0:a3b83d366423 293 uint16 BeaconNode::getRecFrameLength(){
aungriah 0:a3b83d366423 294 return sizeof(receivedFrame);
aungriah 0:a3b83d366423 295 }
aungriah 0:a3b83d366423 296
aungriah 0:a3b83d366423 297 int BeaconNode::getMode(){
aungriah 0:a3b83d366423 298 return mode;
aungriah 0:a3b83d366423 299 }
aungriah 0:a3b83d366423 300
aungriah 0:a3b83d366423 301 uint16_t BeaconNode::getRepetitions(){
aungriah 0:a3b83d366423 302 return repetitions;
aungriah 0:a3b83d366423 303 }
aungriah 0:a3b83d366423 304 void BeaconNode::decreaseRepetitions(){
aungriah 0:a3b83d366423 305 repetitions--;
aungriah 0:a3b83d366423 306 }
aungriah 0:a3b83d366423 307 uint8_t BeaconNode::getDestination(){
aungriah 0:a3b83d366423 308 return destination;
aungriah 0:a3b83d366423 309 }
aungriah 0:a3b83d366423 310
aungriah 0:a3b83d366423 311 void BeaconNode::clearRec(){
aungriah 0:a3b83d366423 312 for(int j = 0; j < ADRESSES_COUNT; j++)
aungriah 0:a3b83d366423 313 noRec[j] = 0;
aungriah 0:a3b83d366423 314 }
aungriah 0:a3b83d366423 315
aungriah 0:a3b83d366423 316
aungriah 0:a3b83d366423 317 //------- AnchorNode Class -------------------
aungriah 0:a3b83d366423 318 AnchorNode::AnchorNode(DecaWave& DW) : Node(DW) {
aungriah 0:a3b83d366423 319 Anchor = true;
aungriah 0:a3b83d366423 320 }
aungriah 0:a3b83d366423 321
aungriah 0:a3b83d366423 322 #pragma Otime // Compiler optimize Runtime at the cost of image size
aungriah 0:a3b83d366423 323 // Two Way Ranging Actions when Frame was received
aungriah 0:a3b83d366423 324 void AnchorNode::callbackRX(uint64_t RxTime) {
aungriah 0:a3b83d366423 325 // pc.printf("Got a frame, adress: %d source: %d \r\n", receivedFrame.destination, receivedFrame.source);
aungriah 0:a3b83d366423 326 // if(!isBaseStation()){
aungriah 0:a3b83d366423 327 if (receivedFrame.destination == address){
aungriah 0:a3b83d366423 328 switch (receivedFrame.type) {
aungriah 0:a3b83d366423 329 case SWITCH_TYPE:
aungriah 0:a3b83d366423 330 sendBaseAck();
aungriah 0:a3b83d366423 331 if((receivedFrame.signedTime >> 24) == BECOME_BEACON){
aungriah 0:a3b83d366423 332 node = &beaconNode;
aungriah 0:a3b83d366423 333 pc.printf("\r\n \r\n ---This Node is a beacon now ---\r\n \r\n");
aungriah 0:a3b83d366423 334 }
aungriah 0:a3b83d366423 335 else if((receivedFrame.signedTime >> 24) == BECOME_ANCHOR){
aungriah 0:a3b83d366423 336 pc.printf("\r\n \r\n ---This Node is already an anchor ---\r\n \r\n");
aungriah 0:a3b83d366423 337 }
aungriah 0:a3b83d366423 338 break;
aungriah 0:a3b83d366423 339 case PING:
aungriah 0:a3b83d366423 340 sendAnswer(receivedFrame.source, ANCHOR_RESPONSE);
aungriah 0:a3b83d366423 341 receiverTimestamps[receivedFrame.source][0] = RxTime; //Save the first timestamp on the receiving node/anchor (T_rp)
aungriah 0:a3b83d366423 342 break;
aungriah 0:a3b83d366423 343 case BEACON_RESPONSE:
aungriah 0:a3b83d366423 344 {
aungriah 0:a3b83d366423 345 receiverTimestamps[receivedFrame.source][2] = RxTime; //Save the third timestamp on the receiving node/anchor (T_rf)
aungriah 0:a3b83d366423 346 correctReceiverTimestamps(receivedFrame.source); //Correct the timestamps for the case of a counter overflow
aungriah 0:a3b83d366423 347 //timediffRec = receiverTimestamps[receivedFrame.source][0] + receiverTimestamps[receivedFrame.source][2] - 2*receiverTimestamps[receivedFrame.source][1];
aungriah 0:a3b83d366423 348 //if(timediffRec < 0)
aungriah 0:a3b83d366423 349 // timediffRec = 0;
aungriah 0:a3b83d366423 350 sendTransferFrame(receivedFrame.source, receiverTimestamps[receivedFrame.source][0] + receiverTimestamps[receivedFrame.source][2] - 2*receiverTimestamps[receivedFrame.source][1]);
aungriah 0:a3b83d366423 351 break;
aungriah 0:a3b83d366423 352 }
aungriah 0:a3b83d366423 353 default : break;
aungriah 0:a3b83d366423 354 }
aungriah 0:a3b83d366423 355 }
aungriah 0:a3b83d366423 356 else{
aungriah 0:a3b83d366423 357 dw.turnonrx(); // start listening again
aungriah 0:a3b83d366423 358 }
aungriah 0:a3b83d366423 359 }
aungriah 0:a3b83d366423 360
aungriah 0:a3b83d366423 361
aungriah 0:a3b83d366423 362 #pragma Otime // Compiler optimize Runtime at the cost of image size
aungriah 0:a3b83d366423 363 // Two Way Ranging Actions when Frame was transmitted
aungriah 0:a3b83d366423 364 void AnchorNode::callbackTX(uint64_t TxTime) {
aungriah 0:a3b83d366423 365 dw.turnonrx(); // start listening again
aungriah 0:a3b83d366423 366
aungriah 0:a3b83d366423 367 switch (rangingFrame.type) {
aungriah 0:a3b83d366423 368 case ANCHOR_RESPONSE:
aungriah 0:a3b83d366423 369 receiverTimestamps[rangingFrame.destination][1] = TxTime; //Save the second timestamp on the receiving node/anchor (T_sr)
aungriah 0:a3b83d366423 370 break;
aungriah 0:a3b83d366423 371 default:
aungriah 0:a3b83d366423 372 break;
aungriah 0:a3b83d366423 373 }
aungriah 0:a3b83d366423 374 }
aungriah 0:a3b83d366423 375
aungriah 0:a3b83d366423 376
aungriah 0:a3b83d366423 377
aungriah 0:a3b83d366423 378
aungriah 0:a3b83d366423 379 #pragma Otime // Compiler optimize Runtime at the cost of image size
aungriah 0:a3b83d366423 380 void AnchorNode::sendBaseAck() {
aungriah 0:a3b83d366423 381 rangingFrame.source = address;
aungriah 0:a3b83d366423 382 rangingFrame.destination = BASE_STATION_ADDR;
aungriah 0:a3b83d366423 383 rangingFrame.type = BASE_ORDER_ACK;
aungriah 0:a3b83d366423 384 //dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame));
aungriah 0:a3b83d366423 385 dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), 0, 0);
aungriah 0:a3b83d366423 386 }
aungriah 0:a3b83d366423 387
aungriah 0:a3b83d366423 388
aungriah 0:a3b83d366423 389
aungriah 0:a3b83d366423 390
aungriah 0:a3b83d366423 391
aungriah 0:a3b83d366423 392 #pragma Otime // Compiler optimize Runtime at the cost of image size
aungriah 0:a3b83d366423 393 void AnchorNode::sendTransferFrame(uint8_t destination, int timeDiffsReceiver) {
aungriah 0:a3b83d366423 394 ExtendedRangingFrame transferFrame;
aungriah 0:a3b83d366423 395
aungriah 0:a3b83d366423 396 transferFrame.source = address;
aungriah 0:a3b83d366423 397 transferFrame.destination = destination;
aungriah 0:a3b83d366423 398 transferFrame.type = TRANSFER_FRAME;
aungriah 0:a3b83d366423 399 transferFrame.signedTime = timeDiffsReceiver; //cast the time difference
aungriah 0:a3b83d366423 400 dw.sendFrame((uint8_t*)&transferFrame, sizeof(transferFrame), 0, 0);
aungriah 0:a3b83d366423 401 }
aungriah 0:a3b83d366423 402
aungriah 0:a3b83d366423 403 #pragma Otime // Compiler optimize Runtime at the cost of image size
aungriah 0:a3b83d366423 404 void AnchorNode::sendAnswer(uint8_t destination, uint8_t type) {
aungriah 0:a3b83d366423 405
aungriah 0:a3b83d366423 406 rangingFrame.source = address;
aungriah 0:a3b83d366423 407 rangingFrame.destination = destination;
aungriah 0:a3b83d366423 408 rangingFrame.type = type;
aungriah 0:a3b83d366423 409
aungriah 0:a3b83d366423 410 dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), 0, 0);
aungriah 0:a3b83d366423 411 }
aungriah 0:a3b83d366423 412
aungriah 0:a3b83d366423 413 #pragma Otime // Compiler optimize Runtime at the cost of image size
aungriah 0:a3b83d366423 414 void AnchorNode::correctReceiverTimestamps(uint8_t source){
aungriah 0:a3b83d366423 415
aungriah 0:a3b83d366423 416 if(receiverTimestamps[source][0] > receiverTimestamps[source][1]){
aungriah 0:a3b83d366423 417 receiverTimestamps[source][1] += MMRANGING_2POWER40;
aungriah 0:a3b83d366423 418 receiverTimestamps[source][2] += MMRANGING_2POWER40;
aungriah 0:a3b83d366423 419 }
aungriah 0:a3b83d366423 420
aungriah 0:a3b83d366423 421 if(receiverTimestamps[source][1] - dw.getAntennaDelay() > receiverTimestamps[source][2]){
aungriah 0:a3b83d366423 422 receiverTimestamps[source][2] += MMRANGING_2POWER40;
aungriah 0:a3b83d366423 423 }
aungriah 0:a3b83d366423 424
aungriah 0:a3b83d366423 425 }
aungriah 0:a3b83d366423 426
aungriah 0:a3b83d366423 427
aungriah 0:a3b83d366423 428
aungriah 0:a3b83d366423 429 uint8* AnchorNode::getRecFrameRef(){
aungriah 0:a3b83d366423 430 return (uint8 *) &receivedFrame;
aungriah 0:a3b83d366423 431 }
aungriah 0:a3b83d366423 432 uint16 AnchorNode::getRecFrameLength(){
aungriah 0:a3b83d366423 433 return sizeof(receivedFrame);
aungriah 0:a3b83d366423 434 }
aungriah 0:a3b83d366423 435
aungriah 0:a3b83d366423 436
aungriah 0:a3b83d366423 437 //------- BaseStationNode Class -------------------
aungriah 0:a3b83d366423 438 BaseStationNode::BaseStationNode(DecaWave& DW) : Node(DW) {
aungriah 0:a3b83d366423 439 Anchor = false;
aungriah 0:a3b83d366423 440 LocalTimer.start();
aungriah 0:a3b83d366423 441 }
aungriah 0:a3b83d366423 442
aungriah 0:a3b83d366423 443 uint8* BaseStationNode::getRecFrameRef(){
aungriah 0:a3b83d366423 444 return (uint8 *) &receivedStreamFrame;
aungriah 0:a3b83d366423 445 }
aungriah 0:a3b83d366423 446 uint16 BaseStationNode::getRecFrameLength(){
aungriah 0:a3b83d366423 447 return sizeof(receivedStreamFrame);
aungriah 0:a3b83d366423 448 }
aungriah 0:a3b83d366423 449
aungriah 0:a3b83d366423 450
aungriah 0:a3b83d366423 451 #pragma Otime // Compiler optimize Runtime at the cost of image size
aungriah 0:a3b83d366423 452 // Two Way Ranging Actions when Frame was received
aungriah 0:a3b83d366423 453 void BaseStationNode::callbackRX(uint64_t RxTime) {
aungriah 0:a3b83d366423 454 //pc.printf("Got a frame, adress: %d source: %d \r\n", receivedStreamFrame.destination, receivedStreamFrame.source);
aungriah 0:a3b83d366423 455 if (receivedStreamFrame.destination == address){
aungriah 0:a3b83d366423 456 switch(receivedStreamFrame.type){
aungriah 0:a3b83d366423 457 case STREAM_TO_BASE:
aungriah 0:a3b83d366423 458
aungriah 0:a3b83d366423 459 //pc.printf("#%d to #%d %f(%f) \r\n", receivedStreamFrame.source, receivedStreamFrame.anchor_adress, receivedStreamFrame.distance, receivedStreamFrame.signalStrength);
aungriah 0:a3b83d366423 460 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 461 break;
aungriah 0:a3b83d366423 462 case BASE_ORDER_ACK:
aungriah 0:a3b83d366423 463
aungriah 0:a3b83d366423 464 ack = true;
aungriah 0:a3b83d366423 465 break;
aungriah 0:a3b83d366423 466
bberabi 2:5adf0b785944 467
bberabi 2:5adf0b785944 468
aungriah 0:a3b83d366423 469
aungriah 0:a3b83d366423 470
aungriah 0:a3b83d366423 471 }
aungriah 0:a3b83d366423 472 }
aungriah 0:a3b83d366423 473 dw.turnonrx(); // start listening again
aungriah 0:a3b83d366423 474
aungriah 0:a3b83d366423 475
aungriah 0:a3b83d366423 476 }
aungriah 0:a3b83d366423 477
aungriah 0:a3b83d366423 478 #pragma Otime // Compiler optimize Runtime at the cost of image size
aungriah 0:a3b83d366423 479 // Two Way Ranging Actions when Frame was transmitted
aungriah 0:a3b83d366423 480 void BaseStationNode::callbackTX(uint64_t TxTime) {
aungriah 0:a3b83d366423 481 dw.turnonrx(); // start listening again
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 void BaseStationNode::sendOrder(uint8_t beacon_destination, uint8_t anchor_destination, uint8_t action, uint16_t repetitions, uint8_t type) {
aungriah 0:a3b83d366423 486 ExtendedRangingFrame orderFrame;
aungriah 0:a3b83d366423 487 ack = false;
bberabi 2:5adf0b785944 488
aungriah 0:a3b83d366423 489 orderFrame.source = address;
aungriah 0:a3b83d366423 490 orderFrame.destination = beacon_destination;
aungriah 0:a3b83d366423 491 orderFrame.type = type;
aungriah 0:a3b83d366423 492 orderFrame.signedTime = action << 24 | anchor_destination << 16 | repetitions;
aungriah 0:a3b83d366423 493
aungriah 0:a3b83d366423 494 int i = 0;
bberabi 2:5adf0b785944 495 for(i = 0; i < 3 && !ack; i++){
aungriah 0:a3b83d366423 496 float time_before = LocalTimer.read();
aungriah 0:a3b83d366423 497 dw.sendFrame((uint8_t*)&orderFrame, sizeof(orderFrame), 0, 0);
aungriah 0:a3b83d366423 498 while(!ack && (LocalTimer.read() < time_before + 0.010 + 0.01*i)); // One Ranging normaly takes less than 1.5 miliseconds
bberabi 2:5adf0b785944 499 }
aungriah 0:a3b83d366423 500 if(!ack)
aungriah 0:a3b83d366423 501 {
aungriah 0:a3b83d366423 502 pc.printf("ERROR: Tag #%d did not respond \r\n", beacon_destination);
aungriah 0:a3b83d366423 503 }
aungriah 0:a3b83d366423 504 else
aungriah 0:a3b83d366423 505 {
bberabi 1:346279def7ac 506 // pc.printf("Order sent, %d tries \r\n", i);
aungriah 0:a3b83d366423 507 }
aungriah 0:a3b83d366423 508
aungriah 0:a3b83d366423 509
aungriah 0:a3b83d366423 510
aungriah 0:a3b83d366423 511 }