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@1:346279def7ac, 2017-12-09 (annotated)

Committer:
bberabi
Date:
Sat Dec 09 16:02:58 2017 +0000
Revision:
1:346279def7ac
Parent:
0:a3b83d366423
Child:
2:5adf0b785944
errvv

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