first functional version of two-way ranging + ranging protocol auto r…

…ecovery
thotro · Jul 5, 2015 · 857edcd · 857edcd
1 parent 4daf813
commit 857edcd
Show file tree

Hide file tree

Showing 3 changed files with 54 additions and 20 deletions.
diff --git a/DW1000-arduino-test/DW1000-arduino-ranging-anchor/DW1000-arduino-ranging-anchor.ino b/DW1000-arduino-test/DW1000-arduino-ranging-anchor/DW1000-arduino-ranging-anchor.ino
@@ -47,6 +47,9 @@ DW1000Time timeComputedRange;
 byte data[LEN_DATA];
 // reset line to the chip
 int RST = 9;
+// watchdog and reset period
+unsigned long lastActivity;
+unsigned long resetPeriod = 250;
 
 void setup() {
   // DEBUG monitoring
@@ -78,6 +81,19 @@ void setup() {
   DW1000.attachReceivedHandler(handleReceived);
   // anchor starts in receiving mode, awaiting a ranging poll message
   receiver();
+  noteActivity();
+}
+
+void noteActivity() {
+  // update activity timestamp, so that we do not reach "resetPeriod"
+  lastActivity = millis();
+}
+
+void resetInactive() {
+  // anchor listens for POLL
+  expectedMsgId = POLL;
+  receiver();
+  noteActivity();
 }
 
 void handleSent() {
@@ -138,6 +154,10 @@ void computeRange() {
 
 void loop() {
   if(!sentAck && !receivedAck) {
+    // check if inactive
+    if(millis() - lastActivity > resetPeriod) {
+      resetInactive();
+    }
     return;
   }
   // continue on any success confirmation
@@ -146,7 +166,7 @@ void loop() {
     byte msgId = data[0];
     if(msgId == POLL_ACK) {
       DW1000.getTransmitTimestamp(timePollAckSent);
-      //Serial.print("Sent POLL ACK @ "); Serial.println(timePollAckSent.getAsFloat());
+      noteActivity();
     }
   }
   if(receivedAck) {
@@ -155,16 +175,16 @@ void loop() {
     DW1000.getData(data, LEN_DATA);
     byte msgId = data[0];
     if(msgId != expectedMsgId) {
-      // unexpected message, start over again
-      Serial.print("Received wrong message # "); Serial.println(msgId);
+      // unexpected message, start over again (except if already POLL)
       protocolFailed = true;
     }
     if(msgId == POLL) {
+      // on POLL we (re-)start, so no protocol failure
       protocolFailed = false;
       DW1000.getReceiveTimestamp(timePollReceived);
       expectedMsgId = RANGE;
       transmitPollAck();
-      //Serial.print("Received POLL @ "); Serial.println(timePollReceived.getAsFloat());
+      noteActivity();
     } else if(msgId == RANGE) {
       DW1000.getReceiveTimestamp(timeRangeReceived);
       expectedMsgId = POLL;
@@ -175,15 +195,11 @@ void loop() {
         // (re-)compute range as two-way ranging is done
         computeRange();
         transmitRangeReport(timeComputedRange.getAsFloat());
-        /*Serial.print("Received RANGE @ "); Serial.println(timeRangeReceived.getAsFloat());
-        Serial.print("POLL sent @ "); Serial.println(timePollSent.getAsFloat());
-        Serial.print("POLL ACK received @ "); Serial.println(timePollAckReceived.getAsFloat());
-        Serial.print("RANGE sent @ "); Serial.println(timeRangeSent.getAsFloat());
-        Serial.print("Range time is "); Serial.println(timeComputedRange.getAsFloat(), 4);*/
         Serial.print("Range is [m] "); Serial.println(timeComputedRange.getAsMeters());
       } else {
         transmitRangeFailed();
       }
+      noteActivity();
     }
   }
 }

diff --git a/DW1000-arduino-test/DW1000-arduino-ranging-tag/DW1000-arduino-ranging-tag.ino b/DW1000-arduino-test/DW1000-arduino-ranging-tag/DW1000-arduino-ranging-tag.ino
@@ -41,6 +41,9 @@ DW1000Time timeRangeSent;
 byte data[LEN_DATA];
 // reset line to the chip
 int RST = 9;
+// watchdog and reset period
+unsigned long lastActivity;
+unsigned long resetPeriod = 250;
 
 void setup() {
   // DEBUG monitoring
@@ -73,6 +76,19 @@ void setup() {
   // anchor starts by transmitting a POLL message
   receiver();
   transmitPoll();
+  noteActivity();
+}
+
+void noteActivity() {
+  // update activity timestamp, so that we do not reach "resetPeriod"
+  lastActivity = millis();
+}
+
+void resetInactive() {
+  // tag sends POLL and listens for POLL_ACK
+  expectedMsgId = POLL_ACK;
+  transmitPoll();
+  noteActivity();
 }
 
 void handleSent() {
@@ -118,6 +134,10 @@ void receiver() {
 
 void loop() {
   if(!sentAck && !receivedAck) {
+    // check if inactive
+    if(millis() - lastActivity > resetPeriod) {
+      resetInactive();
+    }
     return;
   }
   // continue on any success confirmation
@@ -129,7 +149,7 @@ void loop() {
       //Serial.print("Sent POLL @ "); Serial.println(timePollSent.getAsFloat());
     } else if(msgId == RANGE) {
       DW1000.getTransmitTimestamp(timeRangeSent);
-      //Serial.print("Sent RANGE @ "); Serial.println(timeRangeSent.getAsFloat());
+      noteActivity();
     }
   }
   if(receivedAck) {
@@ -140,7 +160,6 @@ void loop() {
     if(msgId != expectedMsgId) {
       // unexpected message, start over again
       Serial.print("Received wrong message # "); Serial.println(msgId);
-      delay(100);
       expectedMsgId = POLL_ACK;
       transmitPoll();
       return;
@@ -149,19 +168,18 @@ void loop() {
       DW1000.getReceiveTimestamp(timePollAckReceived);
       expectedMsgId = RANGE_REPORT;
       transmitRange();
-      //Serial.print("Received POLL ACK @ "); Serial.println(timePollAckReceived.getAsFloat());
+      noteActivity();
     } else if(msgId == RANGE_REPORT) {
       expectedMsgId = POLL_ACK;
       float curRange;
       memcpy(&curRange, data+1, 4);
-      //Serial.print("Received RANGE REPORT = "); Serial.println(curRange);
-      delay(100);
       transmitPoll();
+      noteActivity();
     } else if(msgId == RANGE_FAILED) {
       expectedMsgId = POLL_ACK;
       Serial.println("Received RANGE FAILED");
-      delay(100);
       transmitPoll();
+      noteActivity();
     }
   }
 }

diff --git a/DW1000/DW1000.h b/DW1000/DW1000.h
@@ -355,9 +355,9 @@ class DW1000Class {
 
 	/** 
 	Specifies the data transmission rate of the DW1000 chip. One of the values
-	 * `TRX_RATE_110KBPS` (i.e. 110 kb/s)
-	 * `TRX_RATE_850KBPS` (i.e. 850 kb/s)
-	 * `TRX_RATE_6800KBPS` (i.e. 6.8 Mb/s)
+	- `TRX_RATE_110KBPS` (i.e. 110 kb/s)
+	- `TRX_RATE_850KBPS` (i.e. 850 kb/s)
+	- `TRX_RATE_6800KBPS` (i.e. 6.8 Mb/s)
 	has to be provided.
 
 	See `setDefaults()` and `enableMode()` for additional information on data rate settings.
@@ -368,8 +368,8 @@ class DW1000Class {
 
 	/** 
 	Specifies the pulse repetition frequency (PRF) of data transmissions with the DW1000. Either
-	 * `TX_PULSE_FREQ_16MHZ` (i.e. 16 MHz)
-	 * `TX_PULSE_FREQ_64MHZ` (i.e. 64 MHz)
+	- `TX_PULSE_FREQ_16MHZ` (i.e. 16 MHz)
+	- `TX_PULSE_FREQ_64MHZ` (i.e. 64 MHz)
 	has to be chosen.
 
 	Note that the 16 MHz setting is more power efficient, while the 64 MHz setting requires more