Tobias Naegeli / Mbed 2 deprecated AIT_UWB_Range

Tobi's ubw test branch

Dependencies:   mavlink_bridge mbed

Fork of AIT_UWB_Range by Benjamin Hepp

MMRanging/MMRanging.cpp@43:d89fe237a684, 2015-03-05 (annotated)

Committer:
manumaet
Date:
Thu Mar 05 10:26:51 2015 +0000
Revision:
43:d89fe237a684
Parent:
41:0a3bb028d4ba 
before fusion with two way ranging

Who changed what in which revision?

UserRevisionLine numberNew contents of line
manumaet 27:71178fdb78e1 1 #include "MMRanging.h"
manumaet 27:71178fdb78e1 2
manumaet 34:f56962030c5c 3 MMRanging::MMRanging(DW1000& DW) : dw(DW) {
manumaet 28:a830131560e8 4 event_i = 0;
manumaet 28:a830131560e8 5 counter = 0;
manumaet 29:019ff388ed76 6 dw.setCallbacks(this, &MMRanging::callbackRX, &MMRanging::callbackTX);
manumaet 43:d89fe237a684 7 for (int i = 0; i < 10; i++) {
manumaet 40:5ce51b7e3118 8 acknowledgement[i] = true;
manumaet 43:d89fe237a684 9 distances[i] = -1;
manumaet 43:d89fe237a684 10 }
manumaet 41:0a3bb028d4ba 11 LocalTimer.start();
manumaet 28:a830131560e8 12 dw.startRX();
manumaet 27:71178fdb78e1 13 }
manumaet 28:a830131560e8 14
manumaet 28:a830131560e8 15 void MMRanging::callbackRX() {
manumaet 32:041dd02e0e3b 16 rangingframe RX;
manumaet 32:041dd02e0e3b 17 dw.readRegister(DW1000_RX_BUFFER, 0, (uint8_t*)&RX, dw.getFramelength()); // get data from buffer
manumaet 31:6f76f3d518ac 18
manumaet 36:883de6f9a73b 19 if (RX.destination == address) // only if received packet is for me
manumaet 36:883de6f9a73b 20 switch (RX.type) {
manumaet 36:883de6f9a73b 21 case 1:
manumaet 36:883de6f9a73b 22 rangingtimingsReceiver[RX.source][0] = dw.getRXTimestamp();
manumaet 36:883de6f9a73b 23 sendRangingframe(RX.source, RX.sequence_number, 2, 0);
manumaet 36:883de6f9a73b 24 break;
manumaet 36:883de6f9a73b 25 case 2:
manumaet 36:883de6f9a73b 26 rangingtimingsSender[RX.source][1] = dw.getRXTimestamp();
manumaet 36:883de6f9a73b 27 sendRangingframe(RX.source, counter, 3, 0);
manumaet 36:883de6f9a73b 28 counter++;
manumaet 36:883de6f9a73b 29 break;
manumaet 36:883de6f9a73b 30 case 3:
manumaet 39:bb57aa77b015 31 sendRangingframe(RX.source, RX.sequence_number, 4, timeDifference40Bit(rangingtimingsReceiver[RX.source][0], rangingtimingsReceiver[RX.source][1]));
manumaet 36:883de6f9a73b 32 break;
manumaet 36:883de6f9a73b 33 case 4:
manumaet 39:bb57aa77b015 34 tofs[RX.source] = timeDifference40Bit(rangingtimingsSender[RX.source][0], rangingtimingsSender[RX.source][1]) - RX.time_difference_receiver;
manumaet 40:5ce51b7e3118 35 acknowledgement[RX.source] = true;
manumaet 36:883de6f9a73b 36 break;
manumaet 36:883de6f9a73b 37 default : break;
manumaet 36:883de6f9a73b 38 }
manumaet 36:883de6f9a73b 39
manumaet 40:5ce51b7e3118 40 #ifdef EVENTS
manumaet 40:5ce51b7e3118 41 sprintf(event[event_i], "!R %d>%d / %d %d", RX.source, RX.destination, RX.sequence_number, RX.type);
manumaet 40:5ce51b7e3118 42 if (event_i == 8)
manumaet 40:5ce51b7e3118 43 event_i = 0;
manumaet 40:5ce51b7e3118 44 else
manumaet 40:5ce51b7e3118 45 event_i++;
manumaet 40:5ce51b7e3118 46 #endif
manumaet 31:6f76f3d518ac 47
manumaet 28:a830131560e8 48 dw.startRX();
manumaet 28:a830131560e8 49 }
manumaet 28:a830131560e8 50
manumaet 28:a830131560e8 51 void MMRanging::callbackTX() {
manumaet 31:6f76f3d518ac 52 switch (TX.type) {
manumaet 31:6f76f3d518ac 53 case 1:
manumaet 36:883de6f9a73b 54 rangingtimingsSender[TX.destination][0] = dw.getTXTimestamp();
manumaet 31:6f76f3d518ac 55 break;
manumaet 31:6f76f3d518ac 56 case 2:
manumaet 36:883de6f9a73b 57 rangingtimingsReceiver[TX.destination][1] = dw.getTXTimestamp();
manumaet 31:6f76f3d518ac 58 break;
manumaet 31:6f76f3d518ac 59 default: break;
manumaet 31:6f76f3d518ac 60 }
manumaet 31:6f76f3d518ac 61
manumaet 40:5ce51b7e3118 62 #ifdef EVENTS
manumaet 40:5ce51b7e3118 63 sprintf(event[event_i], "!S %d>%d / %d %d", TX.source, TX.destination, TX.sequence_number, TX.type);
manumaet 40:5ce51b7e3118 64 if (event_i == 8)
manumaet 40:5ce51b7e3118 65 event_i = 0;
manumaet 40:5ce51b7e3118 66 else
manumaet 40:5ce51b7e3118 67 event_i++;
manumaet 40:5ce51b7e3118 68 #endif
manumaet 28:a830131560e8 69 }
manumaet 28:a830131560e8 70
manumaet 36:883de6f9a73b 71 void MMRanging::requestRanging(uint8_t destination) {
manumaet 41:0a3bb028d4ba 72 acknowledgement[destination] = false;
manumaet 41:0a3bb028d4ba 73 float time_before = LocalTimer.read();
manumaet 36:883de6f9a73b 74 sendRangingframe(destination, counter, 1, 0);
manumaet 41:0a3bb028d4ba 75 while(!acknowledgement[destination] && (LocalTimer.read() < time_before + 0.5f)); // wait for succeeding ranging or timeout
manumaet 41:0a3bb028d4ba 76 roundtriptimes[destination] = LocalTimer.read() - time_before;
manumaet 41:0a3bb028d4ba 77 distances[destination] = (tofs[destination] * 300 / MMRANGING_TIMEUNIT_US / 2);
manumaet 41:0a3bb028d4ba 78 }
manumaet 41:0a3bb028d4ba 79
manumaet 41:0a3bb028d4ba 80 void MMRanging::requestRangingAll() {
manumaet 41:0a3bb028d4ba 81 for (int i = 1; i <= 4; i++) { // Request ranging to all anchors
manumaet 41:0a3bb028d4ba 82 requestRanging(i);
manumaet 41:0a3bb028d4ba 83 }
manumaet 31:6f76f3d518ac 84 }
manumaet 31:6f76f3d518ac 85
manumaet 36:883de6f9a73b 86 void MMRanging::sendRangingframe(uint8_t destination, uint8_t sequence_number, uint8_t type, uint64_t time_difference_receiver) {
manumaet 36:883de6f9a73b 87 TX.source = address;
manumaet 36:883de6f9a73b 88 TX.destination = destination;
manumaet 36:883de6f9a73b 89 TX.sequence_number = sequence_number;
manumaet 34:f56962030c5c 90 TX.type = type;
manumaet 34:f56962030c5c 91 TX.time_difference_receiver = time_difference_receiver;
manumaet 34:f56962030c5c 92 dw.sendFrame((uint8_t*)&TX, sizeof(TX));
manumaet 39:bb57aa77b015 93 }
manumaet 39:bb57aa77b015 94
manumaet 39:bb57aa77b015 95 uint64_t MMRanging::timeDifference40Bit(uint64_t early, uint64_t late) {
manumaet 39:bb57aa77b015 96 int64_t difference = late - early;
manumaet 39:bb57aa77b015 97 if ((difference < -MMRANGING_2POWER40+10000000000) && (difference > -MMRANGING_2POWER40-10000000000)) // if the timestamps differ a negative word length +- ~1sec that was potentially measured, correct it
manumaet 39:bb57aa77b015 98 return difference + MMRANGING_2POWER40;
manumaet 39:bb57aa77b015 99 if ((difference < 0) || (difference > 10000000000))
manumaet 39:bb57aa77b015 100 return 10000000000;
manumaet 39:bb57aa77b015 101 return (uint64_t)difference;
manumaet 28:a830131560e8 102 }