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@3:8bee1711d186, 2018-02-28 (annotated)

Committer:
aungriah
Date:
Wed Feb 28 16:10:21 2018 +0000
Revision:
3:8bee1711d186
Parent:
2:5adf0b785944
Child:
4:120ff05a7c27
ILBE

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