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

MM2WayRanging/MM2WayRanging.cpp@44:2e0045042a59, 2015-03-05 (annotated)

Committer:
manumaet
Date:
Thu Mar 05 12:18:37 2015 +0000
Revision:
44:2e0045042a59
Child:
45:01a33363bc21
Two way distance ranging with several anchors works now (calibration and onboard trilateration still ahead)

Who changed what in which revision?

UserRevisionLine numberNew contents of line
manumaet 44:2e0045042a59 1 #include "MM2WayRanging.h"
manumaet 44:2e0045042a59 2
manumaet 44:2e0045042a59 3 MM2WayRanging::MM2WayRanging(DW1000& DW) : dw(DW) {
manumaet 44:2e0045042a59 4 isAnchor = true;
manumaet 44:2e0045042a59 5 address = 0;
manumaet 44:2e0045042a59 6 rxTimestamp = 0;
manumaet 44:2e0045042a59 7 timediffRec = 0;
manumaet 44:2e0045042a59 8 timediffSend = 0;
manumaet 44:2e0045042a59 9 for (int i = 0; i < 10; i++)
manumaet 44:2e0045042a59 10 acknowledgement[i] = true;
manumaet 44:2e0045042a59 11
manumaet 44:2e0045042a59 12 dw.setCallbacks(this, &MM2WayRanging::callbackRX, &MM2WayRanging::callbackTX);
manumaet 44:2e0045042a59 13
manumaet 44:2e0045042a59 14 LocalTimer.start(); // Timer for timout during acknowledgement
manumaet 44:2e0045042a59 15
manumaet 44:2e0045042a59 16 dw.startRX();
manumaet 44:2e0045042a59 17 }
manumaet 44:2e0045042a59 18
manumaet 44:2e0045042a59 19 void MM2WayRanging::callbackRX() {
manumaet 44:2e0045042a59 20 dw.readRegister(DW1000_RX_BUFFER, 0, (uint8_t*)&receivedFrame, dw.getFramelength());
manumaet 44:2e0045042a59 21
manumaet 44:2e0045042a59 22 if (receivedFrame.destination == address)
manumaet 44:2e0045042a59 23 switch (receivedFrame.type) {
manumaet 44:2e0045042a59 24 case PING:
manumaet 44:2e0045042a59 25 rxTimestamp = dw.getRXTimestamp();
manumaet 44:2e0045042a59 26 receiverTimestamps[receivedFrame.source][0] = rxTimestamp; //Save the first timestamp on the receiving node/anchor (T_rp)
manumaet 44:2e0045042a59 27 sendDelayedAnswer(receivedFrame.source, ANCHOR_RESPONSE, rxTimestamp);
manumaet 44:2e0045042a59 28 break;
manumaet 44:2e0045042a59 29 case ANCHOR_RESPONSE:
manumaet 44:2e0045042a59 30 rxTimestamp = dw.getRXTimestamp();
manumaet 44:2e0045042a59 31 senderTimestamps[receivedFrame.source][1] = rxTimestamp; //Save the second timestamp on the sending node/beacon (T_rr)
manumaet 44:2e0045042a59 32 sendDelayedAnswer(receivedFrame.source, 3, rxTimestamp);
manumaet 44:2e0045042a59 33 break;
manumaet 44:2e0045042a59 34 case BEACON_RESPONSE:
manumaet 44:2e0045042a59 35 rxTimestamp = dw.getRXTimestamp();
manumaet 44:2e0045042a59 36 receiverTimestamps[receivedFrame.source][2] = rxTimestamp; //Save the third timestamp on the receiving node/anchor (T_rf)
manumaet 44:2e0045042a59 37 //calculation of the summand on the receiving node/anchor
manumaet 44:2e0045042a59 38 timediffRec = receiverTimestamps[receivedFrame.source][0] + receiverTimestamps[receivedFrame.source][2] - 2*receiverTimestamps[receivedFrame.source][1];
manumaet 44:2e0045042a59 39 sendTransferFrame(receivedFrame.source, timediffRec );
manumaet 44:2e0045042a59 40 break;
manumaet 44:2e0045042a59 41 case TRANSFER_FRAME:
manumaet 44:2e0045042a59 42 //calculation of the summand on the sending node/beacon
manumaet 44:2e0045042a59 43 timediffSend = 2 * senderTimestamps[receivedFrame.source][1] - senderTimestamps[receivedFrame.source][0] - senderTimestamps[receivedFrame.source][2];
manumaet 44:2e0045042a59 44 //calculation of the resulting sum of all four ToFs.
manumaet 44:2e0045042a59 45 tofs[receivedFrame.source] = receivedFrame.signedTime + timediffSend;
manumaet 44:2e0045042a59 46 acknowledgement[receivedFrame.source] = true;
manumaet 44:2e0045042a59 47 break;
manumaet 44:2e0045042a59 48 default : break;
manumaet 44:2e0045042a59 49 }
manumaet 44:2e0045042a59 50 dw.startRX();
manumaet 44:2e0045042a59 51 }
manumaet 44:2e0045042a59 52
manumaet 44:2e0045042a59 53 void MM2WayRanging::callbackTX() {
manumaet 44:2e0045042a59 54 switch (rangingFrame.type) {
manumaet 44:2e0045042a59 55 case PING:
manumaet 44:2e0045042a59 56 senderTimestamps[rangingFrame.destination][0] = dw.getTXTimestamp(); //Save the first timestamp on the sending node/beacon (T_sp)
manumaet 44:2e0045042a59 57 break;
manumaet 44:2e0045042a59 58 case ANCHOR_RESPONSE:
manumaet 44:2e0045042a59 59 receiverTimestamps[rangingFrame.destination][1] = dw.getTXTimestamp(); //Save the second timestamp on the receiving node/anchor (T_sr)
manumaet 44:2e0045042a59 60 break;
manumaet 44:2e0045042a59 61 case BEACON_RESPONSE:
manumaet 44:2e0045042a59 62 senderTimestamps[rangingFrame.destination][2] = dw.getTXTimestamp(); //Save the third timestamp on the sending node/beacon (T_sr)
manumaet 44:2e0045042a59 63 break;
manumaet 44:2e0045042a59 64 default:
manumaet 44:2e0045042a59 65 break;
manumaet 44:2e0045042a59 66 }
manumaet 44:2e0045042a59 67 }
manumaet 44:2e0045042a59 68
manumaet 44:2e0045042a59 69 /**
manumaet 44:2e0045042a59 70 * Get the distance to the Anchor with address @param destination.
manumaet 44:2e0045042a59 71 *
manumaet 44:2e0045042a59 72 * @param destination The address of the anchor
manumaet 44:2e0045042a59 73 */
manumaet 44:2e0045042a59 74 void MM2WayRanging::requestRanging(uint8_t destination) {
manumaet 44:2e0045042a59 75 acknowledgement[destination] = false;
manumaet 44:2e0045042a59 76 float time_before = LocalTimer.read();
manumaet 44:2e0045042a59 77
manumaet 44:2e0045042a59 78 sendPingFrame(destination);
manumaet 44:2e0045042a59 79
manumaet 44:2e0045042a59 80 while(!acknowledgement[destination] && (LocalTimer.read() < time_before + 0.1f)); // wait for succeeding ranging or timeout
manumaet 44:2e0045042a59 81
manumaet 44:2e0045042a59 82 roundtriptimes[destination] = LocalTimer.read() - time_before;
manumaet 44:2e0045042a59 83 distances[destination] = (tofs[destination] * 300 * TIMEUNITS_TO_US / 4);
manumaet 44:2e0045042a59 84 }
manumaet 44:2e0045042a59 85
manumaet 44:2e0045042a59 86 void MM2WayRanging::requestRangingAll() {
manumaet 44:2e0045042a59 87 for (int i = 1; i <= 4; i++) { // Request ranging to all anchors
manumaet 44:2e0045042a59 88 requestRanging(i);
manumaet 44:2e0045042a59 89 }
manumaet 44:2e0045042a59 90 }
manumaet 44:2e0045042a59 91
manumaet 44:2e0045042a59 92 void MM2WayRanging::sendPingFrame(uint8_t destination) {
manumaet 44:2e0045042a59 93 rangingFrame.source = address;
manumaet 44:2e0045042a59 94 rangingFrame.destination = destination;
manumaet 44:2e0045042a59 95 rangingFrame.type = PING;
manumaet 44:2e0045042a59 96 dw.sendFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame));
manumaet 44:2e0045042a59 97 }
manumaet 44:2e0045042a59 98
manumaet 44:2e0045042a59 99 void MM2WayRanging::sendTransferFrame(uint8_t destination, int timeDiffsReceiver) {
manumaet 44:2e0045042a59 100 transferFrame.source = address;
manumaet 44:2e0045042a59 101 transferFrame.destination = destination;
manumaet 44:2e0045042a59 102 transferFrame.type = TRANSFER_FRAME;
manumaet 44:2e0045042a59 103 transferFrame.signedTime = timeDiffsReceiver; //cast the time difference
manumaet 44:2e0045042a59 104 dw.sendFrame((uint8_t*)&transferFrame, sizeof(transferFrame));
manumaet 44:2e0045042a59 105 }
manumaet 44:2e0045042a59 106
manumaet 44:2e0045042a59 107 void MM2WayRanging::sendDelayedAnswer(uint8_t destination, uint8_t type, uint64_t rxTimestamp) {
manumaet 44:2e0045042a59 108 rangingFrame.source = address;
manumaet 44:2e0045042a59 109 rangingFrame.destination = destination;
manumaet 44:2e0045042a59 110 rangingFrame.type = type;
manumaet 44:2e0045042a59 111 dw.sendDelayedFrame((uint8_t*)&rangingFrame, sizeof(rangingFrame), rxTimestamp + ANSWER_DELAY_TIMEUNITS);
manumaet 44:2e0045042a59 112 }