asymmetric two-way ranging (less error prone)

examples/RangingAnchor/RangingAnchor.ino

@@ -29,9 +29,12 @@
 #define RANGE 2
 #define RANGE_REPORT 3
 #define RANGE_FAILED 255
+// message flow state
 volatile byte expectedMsgId = POLL;
+// message sent/received state
 volatile boolean sentAck = false;
 volatile boolean receivedAck = false;
+// protocol error state
 boolean protocolFailed = false;
 // timestamps to remember
 DW1000Time timePollSent;
@@ -50,6 +53,8 @@ int RST = 9;
 // watchdog and reset period
 unsigned long lastActivity;
 unsigned long resetPeriod = 250;
+// reply times (same on both sides for symm. ranging)
+unsigned int replyDelayTimeMS = 10;
 
 void setup() {
   // DEBUG monitoring
@@ -112,7 +117,7 @@ void transmitPollAck() {
   DW1000.setDefaults();
   data[0] = POLL_ACK;
   // delay the same amount as ranging tag
-  DW1000Time deltaTime = DW1000Time(10, DW1000Time::MILLISECONDS);
+  DW1000Time deltaTime = DW1000Time(replyDelayTimeMS, DW1000Time::MILLISECONDS);
   DW1000.setDelay(deltaTime);
   DW1000.setData(data, LEN_DATA);
   DW1000.startTransmit();
@@ -144,18 +149,48 @@ void receiver() {
   DW1000.startReceive();
 }
 
-void computeRange() {
+/*
+ * RANGING ALGORITHMS
+ * ------------------
+ * Either of the below functions can be used for range computation (see line "CHOSEN 
+ * RANGING ALGORITHM" in the code).
+ * - Asymmetric is more computation intense but least error prone
+ * - Symmetric is less computation intense but more error prone to clock drifts
+ * 
+ * The anchors and tags of this reference example use the same reply delay times, hence
+ * are capable of symmetric ranging (and of asymmetric ranging anyway).
+ */
+
+void computeRangeAsymmetric() {
   // correct timestamps (in case system time counter wrap-arounds occured)
   // TODO
   /*if(timePollAckReceived < timePollSent) {
     timePollAckReceived += ...
   }*/
-  // two roundtrip times - each minus message preparation times / 4
+  // 
+  // asymmetric two-way ranging (more computation intense, less error prone)
+  DW1000Time round1 = (timePollAckReceived-timePollSent);
+  DW1000Time reply1 = (timePollAckSent-timePollReceived);
+  DW1000Time round2 = (timeRangeReceived-timePollAckSent);
+  DW1000Time reply2 = (timeRangeSent-timePollAckReceived);
+  DW1000Time tof = (round1 * round2 - reply1 * reply2) / (round1 + round2 + reply1 + reply2);
+  // set tof timestamp
+  timeComputedRange.setTimestamp(tof);
+}
+
+void computeRangeSymmetric() {
+  // symmetric two-way ranging (less computation intense, more error prone on clock drift)
   DW1000Time tof = ((timePollAckReceived-timePollSent)-(timePollAckSent-timePollReceived) +
-      (timeRangeReceived-timePollAckSent)-(timeRangeSent-timePollAckReceived)) * 0.25f;
+    (timeRangeReceived-timePollAckSent)-(timeRangeSent-timePollAckReceived)) * 0.25f;
+  // set tof timestamp
   timeComputedRange.setTimestamp(tof);
 }
 
+/*
+ * END RANGING ALGORITHMS
+ * ----------------------
+ */
+
 void loop() {
   if(!sentAck && !receivedAck) {
     // check if inactive
@@ -197,9 +232,11 @@ void loop() {
         timePollAckReceived.setTimestamp(data+6);
         timeRangeSent.setTimestamp(data+11);
         // (re-)compute range as two-way ranging is done
-        computeRange();
+        computeRangeAsymmetric(); // CHOSEN RANGING ALGORITHM
         transmitRangeReport(timeComputedRange.getAsFloat());
         Serial.print("Range is [m] "); Serial.println(timeComputedRange.getAsMeters());
+        //Serial.print("FP power is [dBm] ... "); Serial.println(DW1000.getFirstPathPower());
+        //Serial.print("RX power is [dBm] ... "); Serial.println(DW1000.getReceivePower());
       } else {
         transmitRangeFailed();
       }

examples/RangingTag/RangingTag.ino

@@ -29,7 +29,9 @@
 #define RANGE 2
 #define RANGE_REPORT 3
 #define RANGE_FAILED 255
+// message flow state
 volatile byte expectedMsgId = POLL_ACK;
+// message sent/received state
 volatile boolean sentAck = false;
 volatile boolean receivedAck = false;
 // timestamps to remember
@@ -44,6 +46,8 @@ int RST = 9;
 // watchdog and reset period
 unsigned long lastActivity;
 unsigned long resetPeriod = 250;
+// reply times (same on both sides for symm. ranging)
+unsigned int replyDelayTimeMS = 10;
 
 void setup() {
   // DEBUG monitoring
@@ -115,7 +119,7 @@ void transmitRange() {
   DW1000.setDefaults();
   data[0] = RANGE;
   // delay sending the message and remember expected future sent timestamp
-  DW1000Time deltaTime = DW1000Time(10, DW1000Time::MILLISECONDS);
+  DW1000Time deltaTime = DW1000Time(replyDelayTimeMS, DW1000Time::MILLISECONDS);
   timeRangeSent = DW1000.setDelay(deltaTime);
   timePollSent.getTimestamp(data+1);
   timePollAckReceived.getTimestamp(data+6);

src/DW1000.cpp

@@ -909,20 +909,32 @@ void DW1000Class::setDataRate(byte rate) {
 	rate &= 0x03;
 	_txfctrl[1] &= 0x83;
 	_txfctrl[1] |= (byte)((rate << 5) & 0xFF);
+	// special 110kbps flag
 	if(rate == TRX_RATE_110KBPS) {
 		setBit(_syscfg, LEN_SYS_CFG, RXM110K_BIT, true);
-		// TODO first set TN/RNSSFD zero then set SFD length in USR_SDF then use
-		//setBit(_chanctrl, LEN_CHAN_CTRL, DWSFD_BIT, true);
 	} else {
 		setBit(_syscfg, LEN_SYS_CFG, RXM110K_BIT, false);
 	}
+	// SFD mode and type (non-configurable, as in Table )
 	if(rate == TRX_RATE_6800KBPS) {
 		setBit(_chanctrl, LEN_CHAN_CTRL, DWSFD_BIT, false);
+		setBit(_chanctrl, LEN_CHAN_CTRL, TNSSFD_BIT, false);
+		setBit(_chanctrl, LEN_CHAN_CTRL, RNSSFD_BIT, false);
+	} else {
+		setBit(_chanctrl, LEN_CHAN_CTRL, DWSFD_BIT, true);
+		setBit(_chanctrl, LEN_CHAN_CTRL, TNSSFD_BIT, true);
+		setBit(_chanctrl, LEN_CHAN_CTRL, RNSSFD_BIT, true);	
+
 	}
-	if(rate == TRX_RATE_850KBPS) {
-		//setBit(_chanctrl, LEN_CHAN_CTRL, DWSFD_BIT, true);
-		// TODO set length to 8 or 16 in USR_SFD (0x21)
+	byte sfdLength;
+	if(rate == TRX_RATE_6800KBPS) {
+		sfdLength = 0x08;
+	} else if(rate == TRX_RATE_850KBPS) {
+		sfdLength = 0x10;
+	} else {
+		sfdLength = 0x40;
 	}
+	writeBytes(USR_SFD, SFD_LENGTH_SUB, &sfdLength, LEN_SFD_LENGTH);
 	_dataRate = rate;
 }
 

src/DW1000.h

@@ -154,6 +154,14 @@
 #define CHAN_CTRL 0x1F
 #define LEN_CHAN_CTRL 4
 #define DWSFD_BIT 17
+#define TNSSFD_BIT 20
+#define RNSSFD_BIT 21
+
+// user-defined SFD
+#define USR_SFD 0x21
+#define LEN_USR_SFD 41
+#define SFD_LENGTH_SUB 0x00
+#define LEN_SFD_LENGTH 1
 
 // OTP control (for LDE micro code loading only)
 #define OTP_IF 0x2D

src/DW1000Time.cpp

@@ -119,14 +119,33 @@ const DW1000Time DW1000Time::operator*(float factor) const {
 	return DW1000Time(*this) *= factor;
 }
 
+DW1000Time& DW1000Time::operator*=(const DW1000Time &factor) {
+	_timestamp *= factor.getTimestamp();
+	return *this;
+}
+
+const DW1000Time DW1000Time::operator*(const DW1000Time &factor) const {
+	return DW1000Time(*this) *= factor;
+}
+
 DW1000Time& DW1000Time::operator/=(float factor) {
-	return *this *= (1.0f/factor);
+	_timestamp *= (1.0f/factor);
+	return *this;
 }
 
 const DW1000Time DW1000Time::operator/(float factor) const {
 	return DW1000Time(*this) /= factor;
 }
 
+DW1000Time& DW1000Time::operator/=(const DW1000Time &factor) {
+	_timestamp /= factor.getTimestamp(); 
+	return *this;
+}
+
+const DW1000Time DW1000Time::operator/(const DW1000Time &factor) const {
+	return DW1000Time(*this) /= factor;
+}
+
 boolean DW1000Time::operator==(const DW1000Time &cmp) const {
 	return _timestamp == cmp.getTimestamp();
 }

src/DW1000Time.h

@@ -63,9 +63,13 @@ class DW1000Time {
 	DW1000Time& operator-=(const DW1000Time &sub);
 	const DW1000Time operator-(const DW1000Time &sub) const;
 	DW1000Time& operator*=(float factor);
+	const DW1000Time operator*(const DW1000Time &factor) const;
+	DW1000Time& operator*=(const DW1000Time &factor);
 	const DW1000Time operator*(float factor) const;
 	DW1000Time& operator/=(float factor);
 	const DW1000Time operator/(float factor) const;
+	DW1000Time& operator/=(const DW1000Time &factor);
+	const DW1000Time operator/(const DW1000Time &factor) const;
 	boolean operator==(const DW1000Time &cmp) const;
 	boolean operator!=(const DW1000Time &cmp) const;