From 857edcdea1b6d51ef0be2454e93f5623f34acfec Mon Sep 17 00:00:00 2001 From: Thomas Date: Sun, 5 Jul 2015 02:35:13 +0200 Subject: [PATCH] first functional version of two-way ranging + ranging protocol auto recovery --- .../DW1000-arduino-ranging-anchor.ino | 34 ++++++++++++++----- .../DW1000-arduino-ranging-tag.ino | 30 ++++++++++++---- DW1000/DW1000.h | 10 +++--- 3 files changed, 54 insertions(+), 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 index 57116e81..4440b331 100644 --- 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 index 9ef9637b..803638cb 100644 --- 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 index 83a04860..5bf9846a 100644 --- 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