MMRanging.cpp

00001 #include "MMRanging.h"
00002 
00003 MMRanging::MMRanging(DW1000& DW) : dw(DW) {
00004     event_i = 0;
00005     counter = 0;
00006     dw.setCallbacks(this, &MMRanging::callbackRX, &MMRanging::callbackTX);
00007     for (int i = 0; i < 10; i++) {
00008         acknowledgement[i] = true;
00009         distances[i] = -1;
00010     }
00011     LocalTimer.start();
00012     dw.startRX();
00013 }
00014 
00015 void MMRanging::callbackRX() {
00016     rangingframe RX;
00017     dw.readRegister(DW1000_RX_BUFFER, 0, (uint8_t*)&RX, dw.getFramelength());  // get data from buffer
00018     
00019     if (RX.destination == address)                                                  // only if received packet is for me
00020         switch (RX.type) {
00021             case 1:
00022                 rangingtimingsReceiver[RX.source][0] = dw.getRXTimestamp();
00023                 sendRangingframe(RX.source, RX.sequence_number, 2, 0);
00024                 break;
00025             case 2:
00026                 rangingtimingsSender[RX.source][1] = dw.getRXTimestamp();
00027                 sendRangingframe(RX.source, counter, 3, 0);
00028                 counter++;
00029                 break;
00030             case 3:
00031                 sendRangingframe(RX.source, RX.sequence_number, 4, timeDifference40Bit(rangingtimingsReceiver[RX.source][0], rangingtimingsReceiver[RX.source][1]));
00032                 break;
00033             case 4:
00034                 tofs[RX.source] = timeDifference40Bit(rangingtimingsSender[RX.source][0], rangingtimingsSender[RX.source][1]) - RX.time_difference_receiver;
00035                 acknowledgement[RX.source] = true;
00036                 break;
00037             default : break;
00038         }
00039     
00040     #ifdef EVENTS
00041         sprintf(event[event_i], "!R %d>%d / %d %d", RX.source, RX.destination, RX.sequence_number, RX.type);
00042         if (event_i == 8)
00043             event_i = 0;
00044         else
00045             event_i++;
00046     #endif
00047     
00048     dw.startRX();
00049 }
00050 
00051 void MMRanging::callbackTX() {
00052     switch (TX.type) {
00053         case 1:
00054             rangingtimingsSender[TX.destination][0] = dw.getTXTimestamp();
00055             break;
00056         case 2:
00057             rangingtimingsReceiver[TX.destination][1] = dw.getTXTimestamp();
00058             break;
00059         default: break;
00060     }
00061     
00062     #ifdef EVENTS
00063         sprintf(event[event_i], "!S %d>%d / %d %d", TX.source, TX.destination, TX.sequence_number, TX.type);
00064         if (event_i == 8)
00065             event_i = 0;
00066         else
00067             event_i++;
00068     #endif
00069 }
00070 
00071 void MMRanging::requestRanging(uint8_t destination) {
00072     acknowledgement[destination] = false;
00073     float time_before = LocalTimer.read();
00074     sendRangingframe(destination, counter, 1, 0);
00075     while(!acknowledgement[destination] && (LocalTimer.read() < time_before + 0.5f)); // wait for succeeding ranging or timeout
00076     roundtriptimes[destination] = LocalTimer.read() - time_before;
00077     distances[destination] = (tofs[destination] * 300 / MMRANGING_TIMEUNIT_US / 2);
00078 }
00079 
00080 void MMRanging::requestRangingAll() {
00081     for (int i = 1; i <= 4; i++) {  // Request ranging to all anchors
00082         requestRanging(i);
00083     }
00084 }
00085 
00086 void MMRanging::sendRangingframe(uint8_t destination, uint8_t sequence_number, uint8_t type, uint64_t time_difference_receiver) {
00087     TX.source = address;
00088     TX.destination = destination;
00089     TX.sequence_number = sequence_number;
00090     TX.type = type;
00091     TX.time_difference_receiver = time_difference_receiver;
00092     dw.sendFrame((uint8_t*)&TX, sizeof(TX));
00093 }
00094 
00095 uint64_t MMRanging::timeDifference40Bit(uint64_t early, uint64_t late) {
00096     int64_t difference = late - early;
00097     if ((difference < -MMRANGING_2POWER40+10000000000) && (difference > -MMRANGING_2POWER40-10000000000)) // if the timestamps differ a negative word length +- ~1sec that was potentially measured, correct it
00098         return difference + MMRANGING_2POWER40;
00099     if ((difference < 0) || (difference > 10000000000))
00100         return 10000000000;
00101     return (uint64_t)difference;
00102 }