This is the DW1000 driver and our self developed distance measurement application based on it. We do this as a semester thesis at ETH Zürich under the Automatic Control Laboratory in the Department of electrical engineering.
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 }
Generated on Tue Jul 12 2022 15:21:24 by 1.7.2