MM2WayRanging.cpp

00001 #include "MM2WayRanging.h"
00002 
00003 MM2WayRanging::MM2WayRanging(DW1000& DW) : dw(DW) {
00004     isAnchor = true;
00005     overflow = false;
00006     address = 0;
00007     rxTimestamp = 0;
00008     timediffRec = 0;
00009     timediffSend = 0;
00010     for (int i = 0; i < 10; i++)
00011         acknowledgement[i] = true;
00012 
00013     dw.setCallbacks(this, &MM2WayRanging::callbackRX, &MM2WayRanging::callbackTX);
00014 
00015     LocalTimer.start();
00016 
00017     dw.startRX();
00018 }
00019 
00020 void MM2WayRanging::callbackRX() {
00021     dw.readRegister(DW1000_RX_BUFFER, 0, (uint8_t*)&receivedFrame, dw.getFramelength());
00022 
00023     if (receivedFrame.destination == address)
00024         switch (receivedFrame.type) {
00025             case PING:
00026                 rxTimestamp = dw.getRXTimestamp();
00027                 receiverTimestamps[receivedFrame.source][0] = rxTimestamp;      //Save the first timestamp on the receiving node/anchor (T_rp)
00028                 sendDelayedAnswer(receivedFrame.source, ANCHOR_RESPONSE, rxTimestamp);
00029                 break;
00030             case ANCHOR_RESPONSE:
00031                 rxTimestamp = dw.getRXTimestamp();
00032                 senderTimestamps[receivedFrame.source][1] = rxTimestamp;        //Save the second timestamp on the sending node/beacon (T_rr)
00033                 sendDelayedAnswer(receivedFrame.source, 3, rxTimestamp);
00034                 break;
00035             case BEACON_RESPONSE:
00036                 rxTimestamp = dw.getRXTimestamp();
00037                 receiverTimestamps[receivedFrame.source][2] = rxTimestamp;      //Save the third timestamp on the receiving node/anchor (T_rf)
00038 
00039                 correctReceiverTimestamps(receivedFrame.source);                //Correct the timestamps for the case of a counter overflow
00040                 //calculation of the summand on the receiving node/anchor
00041                 timediffRec = - 2*receiverTimestamps[receivedFrame.source][1] + receiverTimestamps[receivedFrame.source][0] + receiverTimestamps[receivedFrame.source][2];
00042                 sendTransferFrame(receivedFrame.source, timediffRec );
00043                 break;
00044             case TRANSFER_FRAME:
00045                 //calculation of the summand on the sending node/beacon
00046                 timediffSend = 2 * senderTimestamps[receivedFrame.source][1] - senderTimestamps[receivedFrame.source][0] - senderTimestamps[receivedFrame.source][2];
00047                 //calculation of the resulting sum of all four ToFs.
00048                 tofs[receivedFrame.source] = receivedFrame.signedTime + timediffSend;
00049                 acknowledgement[receivedFrame.source] = true;
00050                 break;
00051             default : break;
00052         }
00053 
00054     dw.startRX();
00055 }
00056 
00057 void MM2WayRanging::callbackTX() {
00058     switch (rangingFrame.type) {
00059     case PING:
00060         senderTimestamps[rangingFrame.destination][0] = dw.getTXTimestamp();    //Save the first timestamp on the sending node/beacon (T_sp)
00061         break;
00062     case ANCHOR_RESPONSE:
00063         receiverTimestamps[rangingFrame.destination][1] = dw.getTXTimestamp();  //Save the second timestamp on the receiving node/anchor (T_sr)
00064         break;
00065     case BEACON_RESPONSE:
00066         senderTimestamps[rangingFrame.destination][2] = dw.getTXTimestamp();    //Save the third timestamp on the sending node/beacon (T_sr)
00067         correctSenderTimestamps(rangingFrame.destination);                      //Correct the timestamps for the case of a counter overflow
00068         break;
00069     default:
00070         break;
00071     }
00072 
00073 }
00074 
00075 /**
00076  *  Get the distance to the Anchor with address @param destination.
00077  *
00078  *   @param destination The address of the anchor
00079  */
00080 void MM2WayRanging::requestRanging(uint8_t destination) {
00081     acknowledgement[destination] = false;
00082     float time_before = LocalTimer.read();
00083 
00084     sendPingFrame(destination);
00085 
00086     while(!acknowledgement[destination] && (LocalTimer.read() < time_before + 0.02f)); // wait for succeeding ranging or timeout
00087 
00088     roundtriptimes[destination] = LocalTimer.read() - time_before;
00089 
00090     if(acknowledgement[destination]){
00091     distances[destination] = calibratedDistance(destination);
00092     } else {
00093         distances[destination] = -1;
00094     }
00095 }
00096 
00097 inline float MM2WayRanging::calibratedDistance(uint8_t destination) {
00098 
00099     float rawDistance = (tofs[destination] * 300 * TIMEUNITS_TO_US / 4);
00100 
00101 
00102 
00103  // Calibration for Nucleo 0 (and 1)
00104 
00105  //   if (this->address == 1) rawDistance+= 10;
00106 //    switch(destination){
00107 //        case 2:
00108 //            return  rawDistance * 0.9754 - 0.5004;
00109 //        case 3:
00110 //            return  rawDistance * 0.9759 - 0.4103;
00111 //        case 4:
00112 //            return  rawDistance * 0.9798 - 0.5499;
00113 //        case 5:
00114 //            return  rawDistance * 0.9765 - 0.5169;
00115 //        }
00116 
00117     return rawDistance;
00118 
00119 }
00120 
00121 void MM2WayRanging::requestRangingAll() {
00122     for (int i = 1; i <= 4; i++) {  // Request ranging to all anchors
00123         requestRanging(i);
00124     }
00125 }
00126 
00127 void MM2WayRanging::sendPingFrame(uint8_t destination) {
00128     rangingFrame.source = address;
00129     rangingFrame.destination = destination;
00130     rangingFrame.type = PING;
00131     dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame));
00132 }
00133 
00134 void MM2WayRanging::sendTransferFrame(uint8_t destination, int timeDiffsReceiver) {
00135     transferFrame.source = address;
00136     transferFrame.destination = destination;
00137     transferFrame.type = TRANSFER_FRAME;
00138     transferFrame.signedTime =  timeDiffsReceiver;                      //cast the time difference
00139     dw.sendFrame((uint8_t*)&transferFrame, sizeof(transferFrame));
00140 }
00141 
00142 void MM2WayRanging::sendDelayedAnswer(uint8_t destination, uint8_t type, uint64_t rxTimestamp) {
00143 
00144     rangingFrame.source = address;
00145     rangingFrame.destination = destination;
00146     rangingFrame.type = type;
00147 
00148     if(rxTimestamp + ANSWER_DELAY_TIMEUNITS > MMRANGING_2POWER40)
00149         dw.sendDelayedFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), rxTimestamp + ANSWER_DELAY_TIMEUNITS - MMRANGING_2POWER40);
00150     else
00151         dw.sendDelayedFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), rxTimestamp + ANSWER_DELAY_TIMEUNITS);
00152 }
00153 
00154 void MM2WayRanging::correctReceiverTimestamps(uint8_t source){
00155 
00156     if(receiverTimestamps[source][0] > receiverTimestamps[source][1]){
00157         receiverTimestamps[source][1] += MMRANGING_2POWER40;
00158         receiverTimestamps[source][2] += MMRANGING_2POWER40;
00159     }
00160 
00161     if(receiverTimestamps[source][1] > receiverTimestamps[source][2]){
00162             receiverTimestamps[source][2] += MMRANGING_2POWER40;
00163         }
00164 
00165 }
00166 
00167 void MM2WayRanging::correctSenderTimestamps(uint8_t source){
00168 
00169     if (senderTimestamps[source][0] > senderTimestamps[source][1]) {
00170         senderTimestamps[source][1] += MMRANGING_2POWER40;
00171         senderTimestamps[source][2] += MMRANGING_2POWER40;
00172         overflow = true;
00173     } else if (senderTimestamps[source][1] > senderTimestamps[source][2]) {
00174         senderTimestamps[source][2] += MMRANGING_2POWER40;
00175         overflow = true;
00176     }else overflow = false;
00177 
00178 }