From e17a76f016e8b29d82402348d9d9e0a96fb0dd60 Mon Sep 17 00:00:00 2001 From: Andrea Salvatori - IU6FZL Date: Mon, 5 Nov 2018 17:14:18 +0100 Subject: [PATCH 1/5] Write single bit to register (#129) Co-authored-by: an4cr0n --- src/DW1000Ng.cpp | 20 ++++++++++++++++++++ 1 file changed, 20 insertions(+) diff --git a/src/DW1000Ng.cpp b/src/DW1000Ng.cpp index fa4711ee..474bd5e4 100644 --- a/src/DW1000Ng.cpp +++ b/src/DW1000Ng.cpp @@ -233,6 +233,26 @@ namespace DW1000Ng { // end read mode _writeByte(OTP_IF, OTP_CTRL_SUB, 0x00); } + + void _writeBitToRegister(byte bitRegister, uint16_t RegisterOffset, uint16_t bitRegister_LEN, uint16_t selectedBit, boolean value) { + uint16_t idx; + uint8_t bitPosition; + + idx = selectedBit/8; + if(idx >= bitRegister_LEN) { + return; // TODO proper error handling: out of bounds + } + byte targetByte; memset(&targetByte, 0, 1); + bitPosition = selectedBit%8; + _readBytes(bitRegister, RegisterOffset+idx, &targetByte, 1); + + value ? bitSet(targetByte, bitPosition) : bitClear(targetByte, bitPosition); + + if(RegisterOffset == NO_SUB) + RegisterOffset = 0x00; + + _writeBytesToRegister(bitRegister, RegisterOffset+idx, &targetByte, 1); + } /* Steps used to get Temp and Voltage */ void _vbatAndTempSteps() { From 0fe90312eea9697c592402ff21bcef0210a31ea6 Mon Sep 17 00:00:00 2001 From: Michele Biondi Date: Tue, 13 Nov 2018 15:14:26 +0100 Subject: [PATCH 2/5] Update README.md --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index 02da9bb3..6ba553e5 100644 --- a/README.md +++ b/README.md @@ -23,6 +23,7 @@ The main reason for this fork is because the development there is really slow (d We also wanted to change how the ranging logic works.
Rather than providing a ranging loop, we wanted to provide functions to send the different Two way ranging flow frames, giving the user more control over the program.
The base driver was also re-written in a lot of places, to make every API function more independent from each other. +We also plan to make very high-level abstractions to hack projects fast. Features ------------ From 4ae4c26ae56d79ee9f775c009ea4aa8aa99300b2 Mon Sep 17 00:00:00 2001 From: Michele Biondi Date: Tue, 13 Nov 2018 17:32:15 +0100 Subject: [PATCH 3/5] Add Ranging RTLS Abstractions (#135) --- .../StandardRTLSAnchorB_TWR.ino | 22 +--- .../StandardRTLSAnchorC_TWR.ino | 26 +--- .../StandardRTLSAnchorMain_TWR.ino | 34 +----- .../StandardRTLSTag_TWR.ino | 55 +-------- src/DW1000Ng.hpp | 6 +- src/DW1000NgRTLS.cpp | 112 ++++++++++++++++++ src/DW1000NgRTLS.hpp | 12 +- 7 files changed, 141 insertions(+), 126 deletions(-) create mode 100644 src/DW1000NgRTLS.cpp diff --git a/examples/StandardRTLSAnchorB_TWR/StandardRTLSAnchorB_TWR.ino b/examples/StandardRTLSAnchorB_TWR/StandardRTLSAnchorB_TWR.ino index 072e57a3..27f8f803 100644 --- a/examples/StandardRTLSAnchorB_TWR/StandardRTLSAnchorB_TWR.ino +++ b/examples/StandardRTLSAnchorB_TWR/StandardRTLSAnchorB_TWR.ino @@ -150,24 +150,6 @@ void handleReceived() { receivedAck = true; } -void transmitResponseToPoll() { - byte pollAck[] = {DATA, SHORT_SRC_AND_DEST, SEQ_NUMBER++, 0,0, 0,0, 0,0, ACTIVITY_CONTROL, RANGING_CONTINUE, 0, 0}; - DW1000Ng::getNetworkId(&pollAck[3]); - memcpy(&pollAck[5], tag_shortAddress, 2); - DW1000Ng::getDeviceAddress(&pollAck[7]); - DW1000Ng::setTransmitData(pollAck, sizeof(pollAck)); - DW1000Ng::startTransmit(); -} - -void transmitRangingConfirm() { - byte rangingConfirm[] = {DATA, SHORT_SRC_AND_DEST, SEQ_NUMBER++, 0,0, 0,0, 0,0, ACTIVITY_CONTROL, RANGING_CONFIRM, next_anchor_range[0], next_anchor_range[1]}; - DW1000Ng::getNetworkId(&rangingConfirm[3]); - memcpy(&rangingConfirm[5], tag_shortAddress, 2); - DW1000Ng::getDeviceAddress(&rangingConfirm[7]); - DW1000Ng::setTransmitData(rangingConfirm, sizeof(rangingConfirm)); - DW1000Ng::startTransmit(); -} - void transmitRangeReport() { byte rangingReport[] = {DATA, SHORT_SRC_AND_DEST, SEQ_NUMBER++, 0,0, 0,0, 0,0, 0x60, 0,0 }; DW1000Ng::getNetworkId(&rangingReport[3]); @@ -201,7 +183,7 @@ void loop() { if (recv_data[9] == RANGING_TAG_POLL) { timePollReceived = DW1000Ng::getReceiveTimestamp(); - transmitResponseToPoll(); + DW1000NgRTLS::transmitResponseToPoll(tag_shortAddress); noteActivity(); return; } @@ -232,7 +214,7 @@ void loop() { rangeString += "\t RX power: "; rangeString += DW1000Ng::getReceivePower(); rangeString += " dBm"; Serial.println(rangeString); - transmitRangingConfirm(); + DW1000NgRTLS::transmitRangingConfirm(tag_shortAddress, next_anchor_range); noteActivity(); delay(1);//Sending message to the DW1000 chip too frequently, the earlier messages won't send out successfully. transmitRangeReport(); diff --git a/examples/StandardRTLSAnchorC_TWR/StandardRTLSAnchorC_TWR.ino b/examples/StandardRTLSAnchorC_TWR/StandardRTLSAnchorC_TWR.ino index 7c2ba4eb..584ca80c 100644 --- a/examples/StandardRTLSAnchorC_TWR/StandardRTLSAnchorC_TWR.ino +++ b/examples/StandardRTLSAnchorC_TWR/StandardRTLSAnchorC_TWR.ino @@ -50,6 +50,8 @@ byte tag_shortAddress[] = {0x05, 0x00}; byte main_anchor_address[] = {0x01, 0x00}; +byte blink_rate[2] = {0xC8, 0x00}; + device_configuration_t DEFAULT_CONFIG = { false, true, @@ -148,26 +150,6 @@ void handleReceived() { receivedAck = true; } -void transmitResponseToPoll() { - byte pollAck[] = {DATA, SHORT_SRC_AND_DEST, SEQ_NUMBER++, 0,0, 0,0, 0,0, ACTIVITY_CONTROL, RANGING_CONTINUE, 0, 0}; - DW1000Ng::getNetworkId(&pollAck[3]); - memcpy(&pollAck[5], tag_shortAddress, 2); - DW1000Ng::getDeviceAddress(&pollAck[7]); - DW1000Ng::setTransmitData(pollAck, sizeof(pollAck)); - DW1000Ng::startTransmit(); -} - -void transmitActivityFinished() { - /* I send the new blink rate to the tag */ - byte MS_200 = 0xC8; - byte rangingConfirm[] = {DATA, SHORT_SRC_AND_DEST, SEQ_NUMBER++, 0,0, 0,0, 0,0, ACTIVITY_CONTROL, ACTIVITY_FINISHED, MS_200, 0x00}; - DW1000Ng::getNetworkId(&rangingConfirm[3]); - memcpy(&rangingConfirm[5], tag_shortAddress, 2); - DW1000Ng::getDeviceAddress(&rangingConfirm[7]); - DW1000Ng::setTransmitData(rangingConfirm, sizeof(rangingConfirm)); - DW1000Ng::startTransmit(); -} - void transmitRangeReport() { byte rangingReport[] = {DATA, SHORT_SRC_AND_DEST, SEQ_NUMBER++, 0,0, 0,0, 0,0, 0x60, 0,0 }; DW1000Ng::getNetworkId(&rangingReport[3]); @@ -201,7 +183,7 @@ void loop() { if (recv_data[9] == RANGING_TAG_POLL) { timePollReceived = DW1000Ng::getReceiveTimestamp(); - transmitResponseToPoll(); + DW1000NgRTLS::transmitResponseToPoll(tag_shortAddress); noteActivity(); return; } @@ -232,7 +214,7 @@ void loop() { rangeString += "\t RX power: "; rangeString += DW1000Ng::getReceivePower(); rangeString += " dBm"; Serial.println(rangeString); - transmitActivityFinished(); + DW1000NgRTLS::transmitActivityFinished(tag_shortAddress, blink_rate); delay(1);//Sending message to the DW1000 chip too frequently, the earlier messages won't send out successfully. noteActivity(); transmitRangeReport(); diff --git a/examples/StandardRTLSAnchorMain_TWR/StandardRTLSAnchorMain_TWR.ino b/examples/StandardRTLSAnchorMain_TWR/StandardRTLSAnchorMain_TWR.ino index a96b7185..325f322d 100644 --- a/examples/StandardRTLSAnchorMain_TWR/StandardRTLSAnchorMain_TWR.ino +++ b/examples/StandardRTLSAnchorMain_TWR/StandardRTLSAnchorMain_TWR.ino @@ -160,34 +160,6 @@ void handleReceived() { receivedAck = true; } -void transmitRangingInitiation() { - byte RangingInitiation[] = {DATA, SHORT_SRC_LONG_DEST, SEQ_NUMBER++, 0,0, 0,0,0,0,0,0,0,0, 0,0, RANGING_INITIATION, 0,0}; - DW1000Ng::getNetworkId(&RangingInitiation[3]); - memcpy(&RangingInitiation[5], target_eui, 8); - DW1000Ng::getDeviceAddress(&RangingInitiation[13]); - memcpy(&RangingInitiation[16], tag_shortAddress, 2); - DW1000Ng::setTransmitData(RangingInitiation, sizeof(RangingInitiation)); - DW1000Ng::startTransmit(); -} - -void transmitResponseToPoll() { - byte pollAck[] = {DATA, SHORT_SRC_AND_DEST, SEQ_NUMBER++, 0,0, 0,0, 0,0, ACTIVITY_CONTROL, RANGING_CONTINUE, 0, 0}; - DW1000Ng::getNetworkId(&pollAck[3]); - memcpy(&pollAck[5], tag_shortAddress, 2); - DW1000Ng::getDeviceAddress(&pollAck[7]); - DW1000Ng::setTransmitData(pollAck, sizeof(pollAck)); - DW1000Ng::startTransmit(); -} - -void transmitRangingConfirm() { - byte rangingConfirm[] = {DATA, SHORT_SRC_AND_DEST, SEQ_NUMBER++, 0,0, 0,0, 0,0, ACTIVITY_CONTROL, RANGING_CONFIRM, anchor_b[0], anchor_b[1]}; - DW1000Ng::getNetworkId(&rangingConfirm[3]); - memcpy(&rangingConfirm[5], tag_shortAddress, 2); - DW1000Ng::getDeviceAddress(&rangingConfirm[7]); - DW1000Ng::setTransmitData(rangingConfirm, sizeof(rangingConfirm)); - DW1000Ng::startTransmit(); -} - /* using https://math.stackexchange.com/questions/884807/find-x-location-using-3-known-x-y-location-using-trilateration */ void calculatePosition(double &x, double &y) { @@ -226,7 +198,7 @@ void loop() { if (recv_data[9] == RANGING_TAG_POLL) { timePollReceived = DW1000Ng::getReceiveTimestamp(); - transmitResponseToPoll(); + DW1000NgRTLS::transmitResponseToPoll(tag_shortAddress); noteActivity(); return; } @@ -257,7 +229,7 @@ void loop() { rangeString += "\t RX power: "; rangeString += DW1000Ng::getReceivePower(); rangeString += " dBm"; Serial.println(rangeString); - transmitRangingConfirm(); + DW1000NgRTLS::transmitRangingConfirm(tag_shortAddress, anchor_b); noteActivity(); return; } @@ -286,7 +258,7 @@ void loop() { if(recv_data[0] == BLINK) { /* Is blink */ memcpy(target_eui, &recv_data[2], 8); - transmitRangingInitiation(); + DW1000NgRTLS::transmitRangingInitiation(target_eui, tag_shortAddress); noteActivity(); return; } diff --git a/examples/StandardRTLSTag_TWR/StandardRTLSTag_TWR.ino b/examples/StandardRTLSTag_TWR/StandardRTLSTag_TWR.ino index 899a91c7..e450e75f 100644 --- a/examples/StandardRTLSTag_TWR/StandardRTLSTag_TWR.ino +++ b/examples/StandardRTLSTag_TWR/StandardRTLSTag_TWR.ino @@ -37,7 +37,6 @@ byte anchor_address[2]; // timestamps to remember volatile uint64_t timePollSent; volatile uint64_t timePollAckReceived; -volatile uint64_t timeRangeSent; // watchdog and reset period volatile uint32_t lastActivity; volatile uint32_t resetPeriod = 250; @@ -111,7 +110,7 @@ void setup() { DW1000Ng::attachSentHandler(handleSent); DW1000Ng::attachReceivedHandler(handleReceived); // anchor starts by transmitting a POLL message - transmitBlink(); + DW1000NgRTLS::transmitShortBlink(); noteActivity(); } @@ -123,7 +122,7 @@ void noteActivity() { void reset() { // tag returns to Idle and sends POLL DW1000Ng::forceTRxOff(); - transmitBlink(); + DW1000NgRTLS::transmitShortBlink(); noteActivity(); } @@ -135,48 +134,6 @@ void handleReceived() { receivedAck = true; } -void transmitBlink() { - byte Blink[] = {BLINK, SEQ_NUMBER++, 0,0,0,0,0,0,0,0, NO_BATTERY_STATUS | NO_EX_ID, TAG_LISTENING_NOW}; - DW1000Ng::getEUI(&Blink[2]); - DW1000Ng::setTransmitData(Blink, sizeof(Blink)); - DW1000Ng::startTransmit(); -} - -void transmitPoll() { - byte Poll[] = {DATA, SHORT_SRC_AND_DEST, SEQ_NUMBER++, 0,0, 0,0, 0,0 , RANGING_TAG_POLL}; - DW1000Ng::getNetworkId(&Poll[3]); - memcpy(&Poll[5], anchor_address, 2); - DW1000Ng::getDeviceAddress(&Poll[7]); - DW1000Ng::setTransmitData(Poll, sizeof(Poll)); - DW1000Ng::startTransmit(); -} - - -void transmitFinalMessage() { - /* Calculation of future time */ - byte futureTimeBytes[LENGTH_TIMESTAMP]; - - timeRangeSent = DW1000Ng::getSystemTimestamp(); - timeRangeSent += DW1000NgTime::microsecondsToUWBTime(replyDelayTimeUS); - DW1000NgUtils::writeValueToBytes(futureTimeBytes, timeRangeSent, LENGTH_TIMESTAMP); - DW1000Ng::setDelayedTRX(futureTimeBytes); - timeRangeSent += DW1000Ng::getTxAntennaDelay(); - - byte finalMessage[] = {DATA, SHORT_SRC_AND_DEST, SEQ_NUMBER++, 0,0, 0,0, 0,0, RANGING_TAG_FINAL_RESPONSE_EMBEDDED, - 0,0,0,0,0,0,0,0,0,0,0,0 - }; - - DW1000Ng::getNetworkId(&finalMessage[3]); - memcpy(&finalMessage[5], anchor_address, 2); - DW1000Ng::getDeviceAddress(&finalMessage[7]); - - DW1000NgUtils::writeValueToBytes(finalMessage + 10, (uint32_t) timePollSent, 4); - DW1000NgUtils::writeValueToBytes(finalMessage + 14, (uint32_t) timePollAckReceived, 4); - DW1000NgUtils::writeValueToBytes(finalMessage + 18, (uint32_t) timeRangeSent, 4); - DW1000Ng::setTransmitData(finalMessage, sizeof(finalMessage)); - DW1000Ng::startTransmit(TransmitMode::DELAYED); -} - void loop() { if (!sentAck && !receivedAck) { // check if inactive @@ -206,7 +163,7 @@ void loop() { /* Received Response to poll */ timePollSent = DW1000Ng::getTransmitTimestamp(); timePollAckReceived = DW1000Ng::getReceiveTimestamp(); - transmitFinalMessage(); + DW1000NgRTLS::transmitFinalMessage(anchor_address, replyDelayTimeUS, timePollSent, timePollAckReceived); String tempString= "Receiving messages from:" ; tempString += (char)anchor_address[0] + (char)anchor_address[1]; tempString +=" and send it back."; Serial.println(tempString); @@ -215,7 +172,7 @@ void loop() { } else if (recv_data[10] == RANGING_CONFIRM) { /* Received ranging confirm */ memcpy(anchor_address, &recv_data[11], 2); - transmitPoll(); + DW1000NgRTLS::transmitPoll(anchor_address); String tempString= "Sending messages to NEW Anchor:" ; tempString += (char) anchor_address[0] + (char)anchor_address[1]; Serial.println(tempString); noteActivity(); @@ -235,7 +192,7 @@ void loop() { delay(resetPeriod); DW1000Ng::spiWakeup(); - transmitBlink(); + DW1000NgRTLS::transmitShortBlink(); noteActivity(); return; } else { @@ -247,7 +204,7 @@ void loop() { if(recv_data[15] == RANGING_INITIATION) { DW1000Ng::setDeviceAddress(DW1000NgUtils::bytesAsValue(&recv_data[16], 2)); memcpy(anchor_address, &recv_data[13], 2); - transmitPoll(); + DW1000NgRTLS::transmitPoll(anchor_address); String tempString= "Receiving messages from:" ; tempString += (char)anchor_address[0] + (char)anchor_address[1]; tempString +=" and send it back."; Serial.println(tempString); diff --git a/src/DW1000Ng.hpp b/src/DW1000Ng.hpp index 3822ecb1..cbef84da 100644 --- a/src/DW1000Ng.hpp +++ b/src/DW1000Ng.hpp @@ -463,18 +463,18 @@ namespace DW1000Ng { @param [in] mode IMMEDIATE or DELAYED transmission */ void startTransmit(TransmitMode mode = TransmitMode::IMMEDIATE); - + /** Gets the temperature inside the DW1000 Device - @param [out] temp The temperature + returns The temperature */ float getTemperature(); /** Gets the voltage in input of the DW1000 - @param [out] vbat The input voltage + returns The input voltage */ float getBatteryVoltage(); diff --git a/src/DW1000NgRTLS.cpp b/src/DW1000NgRTLS.cpp new file mode 100644 index 00000000..2b4e2b3f --- /dev/null +++ b/src/DW1000NgRTLS.cpp @@ -0,0 +1,112 @@ +/* + * MIT License + * + * Copyright (c) 2018 Michele Biondi, Andrea Salvatori + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. +*/ + +#include +#include "DW1000NgRTLS.hpp" +#include "DW1000Ng.hpp" +#include "DW1000NgUtils.hpp" +#include "DW1000NgTime.hpp" + +static byte SEQ_NUMBER = 0; + +namespace DW1000NgRTLS { + void transmitShortBlink() { + byte Blink[] = {BLINK, SEQ_NUMBER++, 0,0,0,0,0,0,0,0, NO_BATTERY_STATUS | NO_EX_ID, TAG_LISTENING_NOW}; + DW1000Ng::getEUI(&Blink[2]); + DW1000Ng::setTransmitData(Blink, sizeof(Blink)); + DW1000Ng::startTransmit(); + } + + void transmitRangingInitiation(byte tag_eui[], byte tag_short_address[]) { + byte RangingInitiation[] = {DATA, SHORT_SRC_LONG_DEST, SEQ_NUMBER++, 0,0, 0,0,0,0,0,0,0,0, 0,0, RANGING_INITIATION, 0,0}; + DW1000Ng::getNetworkId(&RangingInitiation[3]); + memcpy(&RangingInitiation[5], tag_eui, 8); + DW1000Ng::getDeviceAddress(&RangingInitiation[13]); + memcpy(&RangingInitiation[16], tag_short_address, 2); + DW1000Ng::setTransmitData(RangingInitiation, sizeof(RangingInitiation)); + DW1000Ng::startTransmit(); + } + + void transmitPoll(byte anchor_address[]){ + byte Poll[] = {DATA, SHORT_SRC_AND_DEST, SEQ_NUMBER++, 0,0, 0,0, 0,0 , RANGING_TAG_POLL}; + DW1000Ng::getNetworkId(&Poll[3]); + memcpy(&Poll[5], anchor_address, 2); + DW1000Ng::getDeviceAddress(&Poll[7]); + DW1000Ng::setTransmitData(Poll, sizeof(Poll)); + DW1000Ng::startTransmit(); + } + + void transmitResponseToPoll(byte tag_short_address[]) { + byte pollAck[] = {DATA, SHORT_SRC_AND_DEST, SEQ_NUMBER++, 0,0, 0,0, 0,0, ACTIVITY_CONTROL, RANGING_CONTINUE, 0, 0}; + DW1000Ng::getNetworkId(&pollAck[3]); + memcpy(&pollAck[5], tag_short_address, 2); + DW1000Ng::getDeviceAddress(&pollAck[7]); + DW1000Ng::setTransmitData(pollAck, sizeof(pollAck)); + DW1000Ng::startTransmit(); + } + + void transmitFinalMessage(byte anchor_address[], uint16_t reply_delay, uint64_t timePollSent, uint64_t timeResponseToPollReceived) { + /* Calculation of future time */ + byte futureTimeBytes[LENGTH_TIMESTAMP]; + + uint64_t timeFinalMessageSent = DW1000Ng::getSystemTimestamp(); + timeFinalMessageSent += DW1000NgTime::microsecondsToUWBTime(reply_delay); + DW1000NgUtils::writeValueToBytes(futureTimeBytes, timeFinalMessageSent, LENGTH_TIMESTAMP); + DW1000Ng::setDelayedTRX(futureTimeBytes); + timeFinalMessageSent += DW1000Ng::getTxAntennaDelay(); + + byte finalMessage[] = {DATA, SHORT_SRC_AND_DEST, SEQ_NUMBER++, 0,0, 0,0, 0,0, RANGING_TAG_FINAL_RESPONSE_EMBEDDED, + 0,0,0,0,0,0,0,0,0,0,0,0 + }; + + DW1000Ng::getNetworkId(&finalMessage[3]); + memcpy(&finalMessage[5], anchor_address, 2); + DW1000Ng::getDeviceAddress(&finalMessage[7]); + + DW1000NgUtils::writeValueToBytes(finalMessage + 10, (uint32_t) timePollSent, 4); + DW1000NgUtils::writeValueToBytes(finalMessage + 14, (uint32_t) timeResponseToPollReceived, 4); + DW1000NgUtils::writeValueToBytes(finalMessage + 18, (uint32_t) timeFinalMessageSent, 4); + DW1000Ng::setTransmitData(finalMessage, sizeof(finalMessage)); + DW1000Ng::startTransmit(TransmitMode::DELAYED); + } + + void transmitRangingConfirm(byte tag_short_address[], byte next_anchor[]) { + byte rangingConfirm[] = {DATA, SHORT_SRC_AND_DEST, SEQ_NUMBER++, 0,0, 0,0, 0,0, ACTIVITY_CONTROL, RANGING_CONFIRM, next_anchor[0], next_anchor[1]}; + DW1000Ng::getNetworkId(&rangingConfirm[3]); + memcpy(&rangingConfirm[5], tag_short_address, 2); + DW1000Ng::getDeviceAddress(&rangingConfirm[7]); + DW1000Ng::setTransmitData(rangingConfirm, sizeof(rangingConfirm)); + DW1000Ng::startTransmit(); + } + + void transmitActivityFinished(byte tag_short_address[], byte blink_rate[]) { + /* I send the new blink rate to the tag */ + byte rangingConfirm[] = {DATA, SHORT_SRC_AND_DEST, SEQ_NUMBER++, 0,0, 0,0, 0,0, ACTIVITY_CONTROL, ACTIVITY_FINISHED, blink_rate[0], blink_rate[1]}; + DW1000Ng::getNetworkId(&rangingConfirm[3]); + memcpy(&rangingConfirm[5], tag_short_address, 2); + DW1000Ng::getDeviceAddress(&rangingConfirm[7]); + DW1000Ng::setTransmitData(rangingConfirm, sizeof(rangingConfirm)); + DW1000Ng::startTransmit(); + } +} \ No newline at end of file diff --git a/src/DW1000NgRTLS.hpp b/src/DW1000NgRTLS.hpp index addde004..1e74e298 100644 --- a/src/DW1000NgRTLS.hpp +++ b/src/DW1000NgRTLS.hpp @@ -65,4 +65,14 @@ constexpr byte NO_EX_ID = 0x40; /* BLINK Ext Header */ constexpr byte BLINK_RATE_AND_LISTENING = 0x01; -constexpr byte TAG_LISTENING_NOW = 0x02; \ No newline at end of file +constexpr byte TAG_LISTENING_NOW = 0x02; + +namespace DW1000NgRTLS { + void transmitShortBlink(); + void transmitRangingInitiation(byte tag_eui[], byte tag_short_address[]); + void transmitPoll(byte anchor_address[]); + void transmitResponseToPoll(byte tag_short_address[]); + void transmitFinalMessage(byte anchor_address[], uint16_t reply_delay, uint64_t timePollSent, uint64_t timeResponseToPollReceived); + void transmitRangingConfirm(byte tag_short_address[], byte next_anchor[]); + void transmitActivityFinished(byte tag_short_address[], byte blink_rate[]); +} \ No newline at end of file From 47dee55b7ecb3038dedeb5fea2ce3c2b0e12ebd9 Mon Sep 17 00:00:00 2001 From: Michele Biondi Date: Wed, 5 Dec 2018 15:54:28 +0100 Subject: [PATCH 4/5] Implemented polling abstraction of TWR algorithm (#136) Co-authored-by: Sonic0 --- .../BasicConnectivityTest.ino | 7 + examples/BasicReceiver/BasicReceiver.ino | 72 +++---- examples/BasicSender/BasicSender.ino | 45 ++-- .../StandardRTLSAnchorB_TWR.ino | 158 +++----------- .../StandardRTLSAnchorC_TWR.ino | 160 +++------------ .../StandardRTLSAnchorMain_TWR.ino | 168 ++++----------- .../StandardRTLSTag_TWR.ino | 158 +++----------- src/DW1000Ng.cpp | 52 ++++- src/DW1000Ng.hpp | 22 +- src/DW1000NgRTLS.cpp | 193 +++++++++++++++++- src/DW1000NgRTLS.hpp | 57 +++++- 11 files changed, 483 insertions(+), 609 deletions(-) diff --git a/examples/BasicConnectivityTest/BasicConnectivityTest.ino b/examples/BasicConnectivityTest/BasicConnectivityTest.ino index 30c291da..4f1840b7 100644 --- a/examples/BasicConnectivityTest/BasicConnectivityTest.ino +++ b/examples/BasicConnectivityTest/BasicConnectivityTest.ino @@ -51,9 +51,16 @@ #include // connection pins +#if defined(ESP8266) +const uint8_t PIN_RST = 5; // reset pin +const uint8_t PIN_IRQ = 4; // irq pin +const uint8_t PIN_SS = 15; // spi select pin +#else const uint8_t PIN_RST = 9; // reset pin const uint8_t PIN_IRQ = 2; // irq pin const uint8_t PIN_SS = SS; // spi select pin +#endif + void setup() { // DEBUG monitoring diff --git a/examples/BasicReceiver/BasicReceiver.ino b/examples/BasicReceiver/BasicReceiver.ino index 0bc063a2..24f517cf 100644 --- a/examples/BasicReceiver/BasicReceiver.ino +++ b/examples/BasicReceiver/BasicReceiver.ino @@ -50,15 +50,17 @@ #include #include -// connection pins -const uint8_t PIN_RST = 9; // reset pin -const uint8_t PIN_IRQ = 2; // irq pin +#if defined(ESP8266) +//const uint8_t PIN_RST = 5; // reset pin +//const uint8_t PIN_IRQ = 4; // irq pin +const uint8_t PIN_SS = 15; // spi select pin +#else +//const uint8_t PIN_RST = 9; // reset pin +//const uint8_t PIN_IRQ = 2; // irq pin const uint8_t PIN_SS = SS; // spi select pin +#endif -// DEBUG packet sent status and count -volatile boolean received = false; -volatile boolean error = false; -volatile int16_t numReceived = 0; // todo check int type +int16_t numReceived = 0; // todo check int type String message; device_configuration_t DEFAULT_CONFIG = { @@ -75,24 +77,15 @@ device_configuration_t DEFAULT_CONFIG = { PreambleCode::CODE_3 }; -interrupt_configuration_t DEFAULT_INTERRUPT_CONFIG = { - true, - true, - true, - false, - true -}; - void setup() { // DEBUG monitoring Serial.begin(9600); Serial.println(F("### DW1000Ng-arduino-receiver-test ###")); // initialize the driver - DW1000Ng::initialize(PIN_SS, PIN_IRQ, PIN_RST); + DW1000Ng::initializeNoInterrupt(PIN_SS); Serial.println(F("DW1000Ng initialized ...")); DW1000Ng::applyConfiguration(DEFAULT_CONFIG); - DW1000Ng::applyInterruptConfiguration(DEFAULT_INTERRUPT_CONFIG); DW1000Ng::setDeviceAddress(6); DW1000Ng::setNetworkId(10); @@ -109,40 +102,21 @@ void setup() { Serial.print("Network ID & Device Address: "); Serial.println(msg); DW1000Ng::getPrintableDeviceMode(msg); Serial.print("Device mode: "); Serial.println(msg); - // attach callback for (successfully) received messages - DW1000Ng::attachReceivedHandler(handleReceived); - DW1000Ng::attachReceiveFailedHandler(handleError); - DW1000Ng::attachErrorHandler(handleError); - // start reception - DW1000Ng::startReceive(); -} - -void handleReceived() { - // status change on reception success - received = true; -} - -void handleError() { - error = true; } void loop() { - // enter on confirmation of ISR status change (successfully received) - if (received) { - received = false; - numReceived++; - // get data as string - DW1000Ng::getReceivedData(message); - Serial.print("Received message ... #"); Serial.println(numReceived); - Serial.print("Data is ... "); Serial.println(message); - Serial.print("RX power is [dBm] ... "); Serial.println(DW1000Ng::getReceivePower()); - Serial.print("Signal quality is ... "); Serial.println(DW1000Ng::getReceiveQuality()); - DW1000Ng::startReceive(); - } - if (error) { - error = false; - Serial.println("Error receiving a message"); - DW1000Ng::getReceivedData(message); - Serial.print("Error data is ... "); Serial.println(message); + DW1000Ng::startReceive(); + while(!DW1000Ng::isReceiveDone()) { + #if defined(ESP8266) + yield(); + #endif } + DW1000Ng::clearReceiveStatus(); + numReceived++; + // get data as string + DW1000Ng::getReceivedData(message); + Serial.print("Received message ... #"); Serial.println(numReceived); + Serial.print("Data is ... "); Serial.println(message); + Serial.print("RX power is [dBm] ... "); Serial.println(DW1000Ng::getReceivePower()); + Serial.print("Signal quality is ... "); Serial.println(DW1000Ng::getReceiveQuality()); } diff --git a/examples/BasicSender/BasicSender.ino b/examples/BasicSender/BasicSender.ino index 9dfe4129..f95ac043 100644 --- a/examples/BasicSender/BasicSender.ino +++ b/examples/BasicSender/BasicSender.ino @@ -49,14 +49,17 @@ #include #include -// connection pins -const uint8_t PIN_RST = 9; // reset pin -const uint8_t PIN_IRQ = 2; // irq pin +#if defined(ESP8266) +//const uint8_t PIN_RST = 5; // reset pin +//const uint8_t PIN_IRQ = 4; // irq pin +const uint8_t PIN_SS = 15; // spi select pin +#else +//const uint8_t PIN_RST = 9; // reset pin +//const uint8_t PIN_IRQ = 2; // irq pin const uint8_t PIN_SS = SS; // spi select pin +#endif // DEBUG packet sent status and count -boolean sent = false; -volatile boolean sentAck = false; volatile unsigned long delaySent = 0; int16_t sentNum = 0; // todo check int type @@ -74,24 +77,16 @@ device_configuration_t DEFAULT_CONFIG = { PreambleCode::CODE_3 }; -interrupt_configuration_t DEFAULT_INTERRUPT_CONFIG = { - true, - true, - true, - false, - true -}; - void setup() { // DEBUG monitoring Serial.begin(9600); Serial.println(F("### DW1000Ng-arduino-sender-test ###")); // initialize the driver - DW1000Ng::initialize(PIN_SS, PIN_IRQ, PIN_RST); + DW1000Ng::initializeNoInterrupt(PIN_SS); Serial.println(F("DW1000Ng initialized ...")); DW1000Ng::applyConfiguration(DEFAULT_CONFIG); - DW1000Ng::applyInterruptConfiguration(DEFAULT_INTERRUPT_CONFIG); + //DW1000Ng::applyInterruptConfiguration(DEFAULT_INTERRUPT_CONFIG); DW1000Ng::setDeviceAddress(5); DW1000Ng::setNetworkId(10); @@ -109,15 +104,17 @@ void setup() { DW1000Ng::getPrintableDeviceMode(msg); Serial.print("Device mode: "); Serial.println(msg); // attach callback for (successfully) sent messages - DW1000Ng::attachSentHandler(handleSent); + //DW1000Ng::attachSentHandler(handleSent); // start a transmission transmit(); } +/* void handleSent() { // status change on sent success sentAck = true; } +*/ void transmit() { // transmit some data @@ -128,19 +125,19 @@ void transmit() { delay(1000); DW1000Ng::startTransmit(TransmitMode::IMMEDIATE); delaySent = millis(); + while(!DW1000Ng::isTransmitDone()) { + #if defined(ESP8266) + yield(); + #endif + } + sentNum++; + DW1000Ng::clearTransmitStatus(); } void loop() { - if (sentAck) { - // continue on success confirmation - // (we are here after the given amount of send delay time has passed) - sentAck = false; + transmit(); // update and print some information about the sent message Serial.print("ARDUINO delay sent [ms] ... "); Serial.println(millis() - delaySent); uint64_t newSentTime = DW1000Ng::getTransmitTimestamp(); Serial.print("Processed packet ... #"); Serial.println(sentNum); - sentNum++; - // again, transmit some data - transmit(); - } } diff --git a/examples/StandardRTLSAnchorB_TWR/StandardRTLSAnchorB_TWR.ino b/examples/StandardRTLSAnchorB_TWR/StandardRTLSAnchorB_TWR.ino index 27f8f803..d485a4f0 100644 --- a/examples/StandardRTLSAnchorB_TWR/StandardRTLSAnchorB_TWR.ino +++ b/examples/StandardRTLSAnchorB_TWR/StandardRTLSAnchorB_TWR.ino @@ -19,38 +19,17 @@ #include // connection pins -const uint8_t PIN_RST = 9; // reset pin -const uint8_t PIN_IRQ = 2; // irq pin +#if defined(ESP8266) +const uint8_t PIN_SS = 15; +#else +const uint8_t PIN_RST = 9; const uint8_t PIN_SS = SS; // spi select pin - -// message sent/received state -volatile boolean sentAck = false; -volatile boolean receivedAck = false; - -volatile byte SEQ_NUMBER = 0; - -// timestamps to remember -volatile uint64_t timePollSent; -volatile uint64_t timePollReceived; -volatile uint64_t timePollAckSent; -volatile uint64_t timePollAckReceived; -volatile uint64_t timeRangeSent; -volatile uint64_t timeRangeReceived; - -volatile double distance; - -// watchdog and reset period -volatile uint32_t lastActivity; -volatile uint32_t resetPeriod = 250; -// reply times (same on both sides for symm. ranging) -uint16_t replyDelayTimeUS = 3000; - -byte target_eui[8]; -byte tag_shortAddress[] = {0x05, 0x00}; +#endif byte main_anchor_address[] = {0x01, 0x00}; -byte next_anchor_range[] = {0x03, 0x00}; +uint16_t next_anchor = 3; +double range_self; device_configuration_t DEFAULT_CONFIG = { false, @@ -66,15 +45,6 @@ device_configuration_t DEFAULT_CONFIG = { PreambleCode::CODE_3 }; -interrupt_configuration_t DEFAULT_INTERRUPT_CONFIG = { - true, - true, - false, - false, - false, - false -}; - frame_filtering_configuration_t ANCHOR_FRAME_FILTER_CONFIG = { false, false, @@ -91,15 +61,22 @@ void setup() { Serial.begin(115200); Serial.println(F("### arduino-DW1000Ng-ranging-anchor-B ###")); // initialize the driver - DW1000Ng::initialize(PIN_SS, PIN_IRQ, PIN_RST); + #if defined(ESP8266) + DW1000Ng::initializeNoInterrupt(PIN_SS); + #else + DW1000Ng::initializeNoInterrupt(PIN_SS, PIN_RST); + #endif Serial.println(F("DW1000Ng initialized ...")); // general configuration DW1000Ng::applyConfiguration(DEFAULT_CONFIG); - DW1000Ng::applyInterruptConfiguration(DEFAULT_INTERRUPT_CONFIG); DW1000Ng::enableFrameFiltering(ANCHOR_FRAME_FILTER_CONFIG); DW1000Ng::setEUI("AA:BB:CC:DD:EE:FF:00:02"); + DW1000Ng::setPreambleDetectionTimeout(64); + DW1000Ng::setSfdDetectionTimeout(273); + DW1000Ng::setReceiveFrameWaitTimeoutPeriod(5000); + DW1000Ng::setNetworkId(RTLS_APP_ID); DW1000Ng::setDeviceAddress(2); @@ -116,111 +93,28 @@ void setup() { Serial.print("Network ID & Device Address: "); Serial.println(msg); DW1000Ng::getPrintableDeviceMode(msg); Serial.print("Device mode: "); Serial.println(msg); - // attach callback for (successfully) sent and received messages - DW1000Ng::attachSentHandler(handleSent); - DW1000Ng::attachReceivedHandler(handleReceived); - - // anchor starts in receiving mode, awaiting a ranging poll message - receive(); -} - -void noteActivity() { - // update activity timestamp, so that we do not reach "resetPeriod" - lastActivity = millis(); -} - -void receive() { - DW1000Ng::startReceive(); - noteActivity(); -} - -void resetInactive() { - // anchor listens for POLL - DW1000Ng::forceTRxOff(); - receive(); -} - -void handleSent() { - // status change on sent success - sentAck = true; -} - -void handleReceived() { - // status change on received success - receivedAck = true; + } void transmitRangeReport() { - byte rangingReport[] = {DATA, SHORT_SRC_AND_DEST, SEQ_NUMBER++, 0,0, 0,0, 0,0, 0x60, 0,0 }; + byte rangingReport[] = {DATA, SHORT_SRC_AND_DEST, DW1000NgRTLS::increaseSequenceNumber(), 0,0, 0,0, 0,0, 0x60, 0,0 }; DW1000Ng::getNetworkId(&rangingReport[3]); memcpy(&rangingReport[5], main_anchor_address, 2); DW1000Ng::getDeviceAddress(&rangingReport[7]); - DW1000NgUtils::writeValueToBytes(&rangingReport[10], static_cast((distance*1000)), 2); + DW1000NgUtils::writeValueToBytes(&rangingReport[10], static_cast((range_self*1000)), 2); DW1000Ng::setTransmitData(rangingReport, sizeof(rangingReport)); DW1000Ng::startTransmit(); } -void loop() { - if (!sentAck && !receivedAck) { - // check if inactive - if (millis() - lastActivity > resetPeriod) { - resetInactive(); - } - return; - } - - if (sentAck) { - sentAck = false; - DW1000Ng::startReceive(); - } - - if (receivedAck) { - receivedAck = false; - // get message and parse - size_t recv_len = DW1000Ng::getReceivedDataLength(); - byte recv_data[recv_len]; - DW1000Ng::getReceivedData(recv_data, recv_len); - - if (recv_data[9] == RANGING_TAG_POLL) { - timePollReceived = DW1000Ng::getReceiveTimestamp(); - DW1000NgRTLS::transmitResponseToPoll(tag_shortAddress); - noteActivity(); - return; - } - - if (recv_data[9] == RANGING_TAG_FINAL_RESPONSE_EMBEDDED) { - - timePollAckSent = DW1000Ng::getTransmitTimestamp(); - timeRangeReceived = DW1000Ng::getReceiveTimestamp(); - - timePollSent = DW1000NgUtils::bytesAsValue(recv_data + 10, LENGTH_TIMESTAMP); - timePollAckReceived = DW1000NgUtils::bytesAsValue(recv_data + 14, LENGTH_TIMESTAMP); - timeRangeSent = DW1000NgUtils::bytesAsValue(recv_data + 18, LENGTH_TIMESTAMP); - // (re-)compute range as two-way ranging is done - distance = DW1000NgRanging::computeRangeAsymmetric(timePollSent, - timePollReceived, - timePollAckSent, - timePollAckReceived, - timeRangeSent, - timeRangeReceived); - /* Apply bias correction */ - distance = DW1000NgRanging::correctRange(distance); +void loop() { + RangeAcceptResult result = DW1000NgRTLS::anchorRangeAccept(NextActivity::RANGING_CONFIRM, next_anchor); + if(result.success) { + delay(3); // Tweak based on your hardware + range_self = result.range; + transmitRangeReport(); - /* In case of wrong read due to bad device calibration */ - if(distance <= 0) - distance = 0.001; - - String rangeString = "Range: "; rangeString += distance; rangeString += " m"; + String rangeString = "Range: "; rangeString += range_self; rangeString += " m"; rangeString += "\t RX power: "; rangeString += DW1000Ng::getReceivePower(); rangeString += " dBm"; Serial.println(rangeString); - - DW1000NgRTLS::transmitRangingConfirm(tag_shortAddress, next_anchor_range); - noteActivity(); - delay(1);//Sending message to the DW1000 chip too frequently, the earlier messages won't send out successfully. - transmitRangeReport(); - noteActivity(); - return; } - } } - diff --git a/examples/StandardRTLSAnchorC_TWR/StandardRTLSAnchorC_TWR.ino b/examples/StandardRTLSAnchorC_TWR/StandardRTLSAnchorC_TWR.ino index 584ca80c..281aaa92 100644 --- a/examples/StandardRTLSAnchorC_TWR/StandardRTLSAnchorC_TWR.ino +++ b/examples/StandardRTLSAnchorC_TWR/StandardRTLSAnchorC_TWR.ino @@ -19,38 +19,18 @@ #include // connection pins -const uint8_t PIN_RST = 9; // reset pin -const uint8_t PIN_IRQ = 2; // irq pin +#if defined(ESP8266) +const uint8_t PIN_SS = 15; +#else +const uint8_t PIN_RST = 9; const uint8_t PIN_SS = SS; // spi select pin +#endif -// message sent/received state -volatile boolean sentAck = false; -volatile boolean receivedAck = false; - -volatile byte SEQ_NUMBER = 0; - -// timestamps to remember -volatile uint64_t timePollSent; -volatile uint64_t timePollReceived; -volatile uint64_t timePollAckSent; -volatile uint64_t timePollAckReceived; -volatile uint64_t timeRangeSent; -volatile uint64_t timeRangeReceived; - -volatile double distance; - -// watchdog and reset period -volatile uint32_t lastActivity; -volatile uint32_t resetPeriod = 250; -// reply times (same on both sides for symm. ranging) -uint16_t replyDelayTimeUS = 3000; - -byte target_eui[8]; -byte tag_shortAddress[] = {0x05, 0x00}; +double range_self; byte main_anchor_address[] = {0x01, 0x00}; -byte blink_rate[2] = {0xC8, 0x00}; +uint16_t blink_rate = 200; device_configuration_t DEFAULT_CONFIG = { false, @@ -66,15 +46,6 @@ device_configuration_t DEFAULT_CONFIG = { PreambleCode::CODE_3 }; -interrupt_configuration_t DEFAULT_INTERRUPT_CONFIG = { - true, - true, - false, - false, - false, - false -}; - frame_filtering_configuration_t ANCHOR_FRAME_FILTER_CONFIG = { false, false, @@ -91,15 +62,22 @@ void setup() { Serial.begin(115200); Serial.println(F("### arduino-DW1000Ng-ranging-anchor-C ###")); // initialize the driver - DW1000Ng::initialize(PIN_SS, PIN_IRQ, PIN_RST); + #if defined(ESP8266) + DW1000Ng::initializeNoInterrupt(PIN_SS); + #else + DW1000Ng::initializeNoInterrupt(PIN_SS, PIN_RST); + #endif Serial.println(F("DW1000Ng initialized ...")); // general configuration DW1000Ng::applyConfiguration(DEFAULT_CONFIG); - DW1000Ng::applyInterruptConfiguration(DEFAULT_INTERRUPT_CONFIG); DW1000Ng::enableFrameFiltering(ANCHOR_FRAME_FILTER_CONFIG); DW1000Ng::setEUI("AA:BB:CC:DD:EE:FF:00:03"); + DW1000Ng::setPreambleDetectionTimeout(64); + DW1000Ng::setSfdDetectionTimeout(273); + DW1000Ng::setReceiveFrameWaitTimeoutPeriod(5000); + DW1000Ng::setNetworkId(RTLS_APP_ID); DW1000Ng::setDeviceAddress(3); @@ -116,110 +94,28 @@ void setup() { Serial.print("Network ID & Device Address: "); Serial.println(msg); DW1000Ng::getPrintableDeviceMode(msg); Serial.print("Device mode: "); Serial.println(msg); - // attach callback for (successfully) sent and received messages - DW1000Ng::attachSentHandler(handleSent); - DW1000Ng::attachReceivedHandler(handleReceived); - - // anchor starts in receiving mode, awaiting a ranging poll message - receive(); -} - -void noteActivity() { - // update activity timestamp, so that we do not reach "resetPeriod" - lastActivity = millis(); -} - -void receive() { - DW1000Ng::startReceive(); - noteActivity(); -} - -void resetInactive() { - // anchor listens for POLL - DW1000Ng::forceTRxOff(); - receive(); -} - -void handleSent() { - // status change on sent success - sentAck = true; -} - -void handleReceived() { - // status change on received success - receivedAck = true; } void transmitRangeReport() { - byte rangingReport[] = {DATA, SHORT_SRC_AND_DEST, SEQ_NUMBER++, 0,0, 0,0, 0,0, 0x60, 0,0 }; + byte rangingReport[] = {DATA, SHORT_SRC_AND_DEST, DW1000NgRTLS::increaseSequenceNumber(), 0,0, 0,0, 0,0, 0x60, 0,0 }; DW1000Ng::getNetworkId(&rangingReport[3]); memcpy(&rangingReport[5], main_anchor_address, 2); DW1000Ng::getDeviceAddress(&rangingReport[7]); - DW1000NgUtils::writeValueToBytes(&rangingReport[10], static_cast((distance*1000)), 2); + DW1000NgUtils::writeValueToBytes(&rangingReport[10], static_cast((range_self*1000)), 2); DW1000Ng::setTransmitData(rangingReport, sizeof(rangingReport)); DW1000Ng::startTransmit(); } void loop() { - if (!sentAck && !receivedAck) { - // check if inactive - if (millis() - lastActivity > resetPeriod) { - resetInactive(); - } - return; - } - - if (sentAck) { - sentAck = false; - DW1000Ng::startReceive(); - } - - if (receivedAck) { - receivedAck = false; - // get message and parse - size_t recv_len = DW1000Ng::getReceivedDataLength(); - byte recv_data[recv_len]; - DW1000Ng::getReceivedData(recv_data, recv_len); - - if (recv_data[9] == RANGING_TAG_POLL) { - timePollReceived = DW1000Ng::getReceiveTimestamp(); - DW1000NgRTLS::transmitResponseToPoll(tag_shortAddress); - noteActivity(); - return; - } - - if (recv_data[9] == RANGING_TAG_FINAL_RESPONSE_EMBEDDED) { - - timePollAckSent = DW1000Ng::getTransmitTimestamp(); - timeRangeReceived = DW1000Ng::getReceiveTimestamp(); - - timePollSent = DW1000NgUtils::bytesAsValue(recv_data + 10, LENGTH_TIMESTAMP); - timePollAckReceived = DW1000NgUtils::bytesAsValue(recv_data + 14, LENGTH_TIMESTAMP); - timeRangeSent = DW1000NgUtils::bytesAsValue(recv_data + 18, LENGTH_TIMESTAMP); - // (re-)compute range as two-way ranging is done - distance = DW1000NgRanging::computeRangeAsymmetric(timePollSent, - timePollReceived, - timePollAckSent, - timePollAckReceived, - timeRangeSent, - timeRangeReceived); - /* Apply bias correction */ - distance = DW1000NgRanging::correctRange(distance); - - /* In case of wrong read due to bad device calibration */ - if(distance <= 0) - distance = 0.001; - - String rangeString = "Range: "; rangeString += distance; rangeString += " m"; - rangeString += "\t RX power: "; rangeString += DW1000Ng::getReceivePower(); rangeString += " dBm"; - Serial.println(rangeString); - - DW1000NgRTLS::transmitActivityFinished(tag_shortAddress, blink_rate); - delay(1);//Sending message to the DW1000 chip too frequently, the earlier messages won't send out successfully. - noteActivity(); - transmitRangeReport(); - return; - } - } + RangeAcceptResult result = DW1000NgRTLS::anchorRangeAccept(NextActivity::ACTIVITY_FINISHED, blink_rate); + if(result.success) { + delay(1); // Tweak based on your hardware + range_self = result.range; + transmitRangeReport(); + + String rangeString = "Range: "; rangeString += range_self; rangeString += " m"; + rangeString += "\t RX power: "; rangeString += DW1000Ng::getReceivePower(); rangeString += " dBm"; + Serial.println(rangeString); + } } diff --git a/examples/StandardRTLSAnchorMain_TWR/StandardRTLSAnchorMain_TWR.ino b/examples/StandardRTLSAnchorMain_TWR/StandardRTLSAnchorMain_TWR.ino index 325f322d..352300e7 100644 --- a/examples/StandardRTLSAnchorMain_TWR/StandardRTLSAnchorMain_TWR.ino +++ b/examples/StandardRTLSAnchorMain_TWR/StandardRTLSAnchorMain_TWR.ino @@ -24,42 +24,29 @@ typedef struct Position { } Position; // connection pins -const uint8_t PIN_RST = 9; // reset pin -const uint8_t PIN_IRQ = 2; // irq pin +#if defined(ESP8266) +const uint8_t PIN_SS = 15; +#else +const uint8_t PIN_RST = 9; const uint8_t PIN_SS = SS; // spi select pin +#endif -// message sent/received state -volatile boolean sentAck = false; -volatile boolean receivedAck = false; - -volatile byte SEQ_NUMBER = 0; - -// timestamps to remember -volatile uint64_t timePollSent; -volatile uint64_t timePollReceived; -volatile uint64_t timePollAckSent; -volatile uint64_t timePollAckReceived; -volatile uint64_t timeRangeSent; -volatile uint64_t timeRangeReceived; Position position_self = {0,0}; Position position_B = {3,0}; Position position_C = {3,2.5}; -volatile double range_self; -volatile double range_B; -volatile double range_C; +double range_self; +double range_B; +double range_C; -// watchdog and reset period -volatile uint32_t lastActivity; -volatile uint32_t resetPeriod = 250; -// reply times (same on both sides for symm. ranging) -uint16_t replyDelayTimeUS = 3000; +boolean received_B = false; byte target_eui[8]; byte tag_shortAddress[] = {0x05, 0x00}; byte anchor_b[] = {0x02, 0x00}; +uint16_t next_anchor = 2; byte anchor_c[] = {0x03, 0x00}; device_configuration_t DEFAULT_CONFIG = { @@ -76,15 +63,6 @@ device_configuration_t DEFAULT_CONFIG = { PreambleCode::CODE_3 }; -interrupt_configuration_t DEFAULT_INTERRUPT_CONFIG = { - true, - true, - false, - false, - false, - false -}; - frame_filtering_configuration_t ANCHOR_FRAME_FILTER_CONFIG = { false, false, @@ -101,15 +79,22 @@ void setup() { Serial.begin(115200); Serial.println(F("### DW1000Ng-arduino-ranging-anchorMain ###")); // initialize the driver - DW1000Ng::initialize(PIN_SS, PIN_IRQ, PIN_RST); + #if defined(ESP8266) + DW1000Ng::initializeNoInterrupt(PIN_SS); + #else + DW1000Ng::initializeNoInterrupt(PIN_SS, PIN_RST); + #endif Serial.println(F("DW1000Ng initialized ...")); // general configuration DW1000Ng::applyConfiguration(DEFAULT_CONFIG); - DW1000Ng::applyInterruptConfiguration(DEFAULT_INTERRUPT_CONFIG); DW1000Ng::enableFrameFiltering(ANCHOR_FRAME_FILTER_CONFIG); DW1000Ng::setEUI("AA:BB:CC:DD:EE:FF:00:01"); + DW1000Ng::setPreambleDetectionTimeout(64); + DW1000Ng::setSfdDetectionTimeout(273); + DW1000Ng::setReceiveFrameWaitTimeoutPeriod(5000); + DW1000Ng::setNetworkId(RTLS_APP_ID); DW1000Ng::setDeviceAddress(1); @@ -125,39 +110,7 @@ void setup() { DW1000Ng::getPrintableNetworkIdAndShortAddress(msg); Serial.print("Network ID & Device Address: "); Serial.println(msg); DW1000Ng::getPrintableDeviceMode(msg); - Serial.print("Device mode: "); Serial.println(msg); - // attach callback for (successfully) sent and received messages - DW1000Ng::attachSentHandler(handleSent); - DW1000Ng::attachReceivedHandler(handleReceived); - - // anchor starts in receiving mode, awaiting a ranging poll message - receive(); -} - -void noteActivity() { - // update activity timestamp, so that we do not reach "resetPeriod" - lastActivity = millis(); -} - -void receive() { - DW1000Ng::startReceive(); - noteActivity(); -} - -void resetInactive() { - // anchor listens for POLL - DW1000Ng::forceTRxOff(); - receive(); -} - -void handleSent() { - // status change on sent success - sentAck = true; -} - -void handleReceived() { - // status change on received success - receivedAck = true; + Serial.print("Device mode: "); Serial.println(msg); } /* using https://math.stackexchange.com/questions/884807/find-x-location-using-3-known-x-y-location-using-trilateration */ @@ -174,74 +127,35 @@ void calculatePosition(double &x, double &y) { x = (C*E-F*B) / (E*A-B*D); y = (C*D-A*F) / (B*D-A*E); } - -void loop() { - if (!sentAck && !receivedAck) { - // check if inactive - if (millis() - lastActivity > resetPeriod) { - resetInactive(); - } - return; - } - - if (sentAck) { - sentAck = false; - DW1000Ng::startReceive(); - } - if (receivedAck) { - receivedAck = false; - // get message and parse +void loop() { + if(DW1000NgRTLS::receiveFrame()){ size_t recv_len = DW1000Ng::getReceivedDataLength(); byte recv_data[recv_len]; DW1000Ng::getReceivedData(recv_data, recv_len); - if (recv_data[9] == RANGING_TAG_POLL) { - timePollReceived = DW1000Ng::getReceiveTimestamp(); - DW1000NgRTLS::transmitResponseToPoll(tag_shortAddress); - noteActivity(); - return; - } - - if (recv_data[9] == RANGING_TAG_FINAL_RESPONSE_EMBEDDED) { - timePollAckSent = DW1000Ng::getTransmitTimestamp(); - timeRangeReceived = DW1000Ng::getReceiveTimestamp(); + if(recv_data[0] == BLINK) { + DW1000NgRTLS::transmitRangingInitiation(&recv_data[2], tag_shortAddress); + DW1000NgRTLS::waitForTransmission(); - timePollSent = DW1000NgUtils::bytesAsValue(recv_data + 10, LENGTH_TIMESTAMP); - timePollAckReceived = DW1000NgUtils::bytesAsValue(recv_data + 14, LENGTH_TIMESTAMP); - timeRangeSent = DW1000NgUtils::bytesAsValue(recv_data + 18, LENGTH_TIMESTAMP); - // (re-)compute range as two-way ranging is done - range_self = DW1000NgRanging::computeRangeAsymmetric(timePollSent, - timePollReceived, - timePollAckSent, - timePollAckReceived, - timeRangeSent, - timeRangeReceived); - /* Apply bias correction */ - range_self = DW1000NgRanging::correctRange(range_self); + RangeAcceptResult result = DW1000NgRTLS::anchorRangeAccept(NextActivity::RANGING_CONFIRM, next_anchor); + if(!result.success) return; + range_self = result.range; - /* In case of wrong read due to bad device calibration */ - if(range_self <= 0) - range_self = 0.001; - String rangeString = "Range: "; rangeString += range_self; rangeString += " m"; rangeString += "\t RX power: "; rangeString += DW1000Ng::getReceivePower(); rangeString += " dBm"; Serial.println(rangeString); - - DW1000NgRTLS::transmitRangingConfirm(tag_shortAddress, anchor_b); - noteActivity(); - return; - } - if(recv_data[9] == 0x60) { + } else if(recv_data[9] == 0x60) { double range = static_cast(DW1000NgUtils::bytesAsValue(&recv_data[10],2) / 1000.0); String rangeReportString = "Range from: "; rangeReportString += recv_data[7]; rangeReportString += " = "; rangeReportString += range; Serial.println(rangeReportString); - if(recv_data[7] == anchor_b[0] && recv_data[8] == anchor_b[1]) { + if(received_B == false && recv_data[7] == anchor_b[0] && recv_data[8] == anchor_b[1]) { range_B = range; - } else if(recv_data[7] == anchor_c[0] && recv_data[8] == anchor_c[1]){ + received_B = true; + } else if(received_B == true && recv_data[7] == anchor_c[0] && recv_data[8] == anchor_c[1]){ range_C = range; double x,y; calculatePosition(x,y); @@ -249,20 +163,12 @@ void loop() { positioning += x; positioning +=" y: "; positioning += y; Serial.println(positioning); + received_B = false; + } else { + received_B = false; } - DW1000Ng::startReceive(); - noteActivity(); - return; - } - - if(recv_data[0] == BLINK) { - /* Is blink */ - memcpy(target_eui, &recv_data[2], 8); - DW1000NgRTLS::transmitRangingInitiation(target_eui, tag_shortAddress); - noteActivity(); - return; } - } -} + +} \ No newline at end of file diff --git a/examples/StandardRTLSTag_TWR/StandardRTLSTag_TWR.ino b/examples/StandardRTLSTag_TWR/StandardRTLSTag_TWR.ino index e450e75f..e638da9b 100644 --- a/examples/StandardRTLSTag_TWR/StandardRTLSTag_TWR.ino +++ b/examples/StandardRTLSTag_TWR/StandardRTLSTag_TWR.ino @@ -21,27 +21,14 @@ #include // connection pins -const uint8_t PIN_RST = 9; // reset pin -const uint8_t PIN_IRQ = 2; // irq pin +#if defined(ESP8266) +const uint8_t PIN_SS = 15; +#else const uint8_t PIN_SS = SS; // spi select pin +const uint8_t PIN_RST = 9; +#endif -// message flow state -// message sent/received state -volatile boolean sentAck = false; -volatile boolean receivedAck = false; - -byte SEQ_NUMBER = 0; - -byte anchor_address[2]; - -// timestamps to remember -volatile uint64_t timePollSent; -volatile uint64_t timePollAckReceived; -// watchdog and reset period -volatile uint32_t lastActivity; -volatile uint32_t resetPeriod = 250; -// reply times (same on both sides for symm. ranging) -uint16_t replyDelayTimeUS = 3000; +volatile uint32_t blink_rate = 200; device_configuration_t DEFAULT_CONFIG = { false, @@ -57,15 +44,6 @@ device_configuration_t DEFAULT_CONFIG = { PreambleCode::CODE_3 }; -interrupt_configuration_t DEFAULT_INTERRUPT_CONFIG = { - true, - true, - false, - false, - false, - false -}; - frame_filtering_configuration_t TAG_FRAME_FILTER_CONFIG = { false, false, @@ -82,11 +60,14 @@ void setup() { Serial.begin(115200); Serial.println(F("### DW1000Ng-arduino-ranging-tag ###")); // initialize the driver - DW1000Ng::initialize(PIN_SS, PIN_IRQ, PIN_RST); + #if defined(ESP8266) + DW1000Ng::initializeNoInterrupt(PIN_SS); + #else + DW1000Ng::initializeNoInterrupt(PIN_SS, PIN_RST); + #endif Serial.println("DW1000Ng initialized ..."); // general configuration DW1000Ng::applyConfiguration(DEFAULT_CONFIG); - DW1000Ng::applyInterruptConfiguration(DEFAULT_INTERRUPT_CONFIG); DW1000Ng::enableFrameFiltering(TAG_FRAME_FILTER_CONFIG); DW1000Ng::setEUI("AA:BB:CC:DD:EE:FF:00:00"); @@ -94,6 +75,10 @@ void setup() { DW1000Ng::setNetworkId(RTLS_APP_ID); DW1000Ng::setAntennaDelay(16436); + + DW1000Ng::setPreambleDetectionTimeout(15); + DW1000Ng::setSfdDetectionTimeout(273); + DW1000Ng::setReceiveFrameWaitTimeoutPeriod(2000); Serial.println(F("Committed configuration ...")); // DEBUG chip info and registers pretty printed @@ -105,111 +90,16 @@ void setup() { DW1000Ng::getPrintableNetworkIdAndShortAddress(msg); Serial.print("Network ID & Device Address: "); Serial.println(msg); DW1000Ng::getPrintableDeviceMode(msg); - Serial.print("Device mode: "); Serial.println(msg); - // attach callback for (successfully) sent and received messages - DW1000Ng::attachSentHandler(handleSent); - DW1000Ng::attachReceivedHandler(handleReceived); - // anchor starts by transmitting a POLL message - DW1000NgRTLS::transmitShortBlink(); - noteActivity(); -} - -void noteActivity() { - // update activity timestamp, so that we do not reach "resetPeriod" - lastActivity = millis(); -} - -void reset() { - // tag returns to Idle and sends POLL - DW1000Ng::forceTRxOff(); - DW1000NgRTLS::transmitShortBlink(); - noteActivity(); -} - -void handleSent() { - sentAck = true; -} - -void handleReceived() { - receivedAck = true; + Serial.print("Device mode: "); Serial.println(msg); } void loop() { - if (!sentAck && !receivedAck) { - // check if inactive - if (millis() - lastActivity > resetPeriod) { - String tempString= "Time out! The lost anchor is:" ; tempString += (char)anchor_address[0] + (char)anchor_address[1]; - Serial.println(tempString); - reset(); - } - return; - } - - if (sentAck) { - sentAck = false; - DW1000Ng::startReceive(); - } - - if (receivedAck) { - receivedAck = false; - /* Parse received message */ - size_t recv_len = DW1000Ng::getReceivedDataLength(); - byte recv_data[recv_len]; - DW1000Ng::getReceivedData(recv_data, recv_len); - - /* RTLS standard message */ - if(recv_data[9] == ACTIVITY_CONTROL) { - if (recv_data[10] == RANGING_CONTINUE) { - /* Received Response to poll */ - timePollSent = DW1000Ng::getTransmitTimestamp(); - timePollAckReceived = DW1000Ng::getReceiveTimestamp(); - DW1000NgRTLS::transmitFinalMessage(anchor_address, replyDelayTimeUS, timePollSent, timePollAckReceived); - String tempString= "Receiving messages from:" ; tempString += (char)anchor_address[0] + (char)anchor_address[1]; - tempString +=" and send it back."; - Serial.println(tempString); - noteActivity(); - return; - } else if (recv_data[10] == RANGING_CONFIRM) { - /* Received ranging confirm */ - memcpy(anchor_address, &recv_data[11], 2); - DW1000NgRTLS::transmitPoll(anchor_address); - String tempString= "Sending messages to NEW Anchor:" ; tempString += (char) anchor_address[0] + (char)anchor_address[1]; - Serial.println(tempString); - noteActivity(); - return; - } else if(recv_data[10] == ACTIVITY_FINISHED) { - resetPeriod = recv_data[11] + static_cast(((recv_data[12] & 0x3F) << 8)); - byte multiplier = ((recv_data[12] & 0xC0) >> 6); - if(multiplier == 0x01) { - resetPeriod *= 25; - } else if(multiplier == 0x02) { - resetPeriod *= 1000; - } - Serial.println("One Range is finishend, and the modules is going to sleep."); - - /* Sleep until next blink to save power */ - DW1000Ng::deepSleep(); - delay(resetPeriod); - DW1000Ng::spiWakeup(); + DW1000Ng::deepSleep(); + delay(blink_rate); + DW1000Ng::spiWakeup(); + DW1000Ng::setEUI("AA:BB:CC:DD:EE:FF:00:00"); - DW1000NgRTLS::transmitShortBlink(); - noteActivity(); - return; - } else { - reset(); - return; - } - } - - if(recv_data[15] == RANGING_INITIATION) { - DW1000Ng::setDeviceAddress(DW1000NgUtils::bytesAsValue(&recv_data[16], 2)); - memcpy(anchor_address, &recv_data[13], 2); - DW1000NgRTLS::transmitPoll(anchor_address); - String tempString= "Receiving messages from:" ; tempString += (char)anchor_address[0] + (char)anchor_address[1]; - tempString +=" and send it back."; - Serial.println(tempString); - noteActivity(); - return; - } - } -} \ No newline at end of file + RangeInfrastructureResult res = DW1000NgRTLS::tagTwrLocalize(1500); + if(res.success) + blink_rate = res.new_blink_rate; +} diff --git a/src/DW1000Ng.cpp b/src/DW1000Ng.cpp index 474bd5e4..b804b8a0 100644 --- a/src/DW1000Ng.cpp +++ b/src/DW1000Ng.cpp @@ -1244,7 +1244,8 @@ namespace DW1000Ng { // pin and basic member setup // attach interrupt // TODO throw error if pin is not a interrupt pin - attachInterrupt(digitalPinToInterrupt(_irq), pollForEvents, RISING); + if(_irq != 0xff) + attachInterrupt(digitalPinToInterrupt(_irq), interruptServiceRoutine, RISING); select(); // reset chip (either soft or hard) @@ -1277,9 +1278,14 @@ namespace DW1000Ng { _writeToRegister(AON, AON_CFG1_SUB, 0x00, LEN_AON_CFG1); } + void initializeNoInterrupt(uint8_t ss, uint8_t rst) { + initialize(ss, 0xff, rst); + } + void select() { #if !defined(ESP32) && !defined(ESP8266) - SPI.usingInterrupt(digitalPinToInterrupt(_irq)); + if(_irq != 0xff) + SPI.usingInterrupt(digitalPinToInterrupt(_irq)); #endif pinMode(_ss, OUTPUT); digitalWrite(_ss, HIGH); @@ -1314,7 +1320,7 @@ namespace DW1000Ng { _handleReceiveTimestampAvailable = handleReceiveTimestampAvailable; } - void pollForEvents() { + void interruptServiceRoutine() { // read current status and handle via callbacks _readSystemEventStatusRegister(); if(_isClockProblem() /* TODO and others */ && _handleError != 0) { @@ -1349,6 +1355,46 @@ namespace DW1000Ng { } } + boolean isTransmitDone(){ + _readSystemEventStatusRegister(); + return _isTransmitDone(); + } + + void clearTransmitStatus() { + _clearTransmitStatus(); + } + + boolean isReceiveDone() { + _readSystemEventStatusRegister(); + return _isReceiveDone(); + } + + void clearReceiveStatus() { + _clearReceiveStatus(); + } + + boolean isReceiveFailed() { + _readSystemEventStatusRegister(); + return _isReceiveFailed(); + } + + void clearReceiveFailedStatus() { + _clearReceiveFailedStatus(); + forceTRxOff(); + _resetReceiver(); + } + + boolean isReceiveTimeout() { + _readSystemEventMaskRegister(); + return _isReceiveTimeout(); + } + + void clearReceiveTimeoutStatus() { + _clearReceiveTimeoutStatus(); + forceTRxOff(); + _resetReceiver(); + } + void enableDebounceClock() { byte pmscctrl0[LEN_PMSC_CTRL0]; memset(pmscctrl0, 0, LEN_PMSC_CTRL0); diff --git a/src/DW1000Ng.hpp b/src/DW1000Ng.hpp index cbef84da..33a629d0 100644 --- a/src/DW1000Ng.hpp +++ b/src/DW1000Ng.hpp @@ -62,6 +62,8 @@ namespace DW1000Ng { @param[in] rst The reset line/pin for hard resets of ICs that connect to the Arduino. Value 0xff means soft reset. */ void initialize(uint8_t ss, uint8_t irq, uint8_t rst = 0xff); + + void initializeNoInterrupt(uint8_t ss, uint8_t rst = 0xff); /** (Re-)selects a specific DW1000 chip for communication. Used in case you switched SPI to another device. @@ -379,11 +381,27 @@ namespace DW1000Ng { void attachReceiveTimestampAvailableHandler(void (* handleReceiveTimestampAvailable)(void)); /** - Poll the DW1000 for system events and handles them + Handles dw1000 events triggered by interrupt By default this is attached to the interrupt pin callback */ - void pollForEvents(); + void interruptServiceRoutine(); + boolean isTransmitDone(); + + void clearTransmitStatus(); + + boolean isReceiveDone(); + + void clearReceiveStatus(); + + boolean isReceiveFailed(); + + void clearReceiveFailedStatus(); + + boolean isReceiveTimeout(); + + void clearReceiveTimeoutStatus(); + /** Stops the transceiver immediately, this actually sets the device in Idle mode. */ diff --git a/src/DW1000NgRTLS.cpp b/src/DW1000NgRTLS.cpp index 2b4e2b3f..0ea686e6 100644 --- a/src/DW1000NgRTLS.cpp +++ b/src/DW1000NgRTLS.cpp @@ -27,11 +27,17 @@ #include "DW1000Ng.hpp" #include "DW1000NgUtils.hpp" #include "DW1000NgTime.hpp" +#include "DW1000NgRanging.hpp" static byte SEQ_NUMBER = 0; namespace DW1000NgRTLS { - void transmitShortBlink() { + + byte increaseSequenceNumber(){ + return ++SEQ_NUMBER; + } + + void transmitTwrShortBlink() { byte Blink[] = {BLINK, SEQ_NUMBER++, 0,0,0,0,0,0,0,0, NO_BATTERY_STATUS | NO_EX_ID, TAG_LISTENING_NOW}; DW1000Ng::getEUI(&Blink[2]); DW1000Ng::setTransmitData(Blink, sizeof(Blink)); @@ -109,4 +115,189 @@ namespace DW1000NgRTLS { DW1000Ng::setTransmitData(rangingConfirm, sizeof(rangingConfirm)); DW1000Ng::startTransmit(); } + + static uint32_t calculateNewBlinkRate(byte frame[]) { + uint32_t blinkRate = frame[11] + static_cast(((frame[12] & 0x3F) << 8)); + byte multiplier = ((frame[12] & 0xC0) >> 6); + if(multiplier == 0x01) { + blinkRate *= 25; + } else if(multiplier == 0x02) { + blinkRate *= 1000; + } + + return blinkRate; + } + + void waitForTransmission() { + while(!DW1000Ng::isTransmitDone()) { + #if defined(ESP8266) + yield(); + #endif + } + DW1000Ng::clearTransmitStatus(); + } + + boolean receiveFrame() { + DW1000Ng::startReceive(); + while(!DW1000Ng::isReceiveDone()) { + if(DW1000Ng::isReceiveTimeout() ) { + DW1000Ng::clearReceiveTimeoutStatus(); + return false; + } + #if defined(ESP8266) + yield(); + #endif + } + DW1000Ng::clearReceiveStatus(); + return true; + } + + static boolean waitForNextRangingStep() { + DW1000NgRTLS::waitForTransmission(); + if(!DW1000NgRTLS::receiveFrame()) return false; + return true; + } + + RangeRequestResult tagRangeRequest() { + DW1000NgRTLS::transmitTwrShortBlink(); + + if(!DW1000NgRTLS::waitForNextRangingStep()) return {false, 0}; + + size_t init_len = DW1000Ng::getReceivedDataLength(); + byte init_recv[init_len]; + DW1000Ng::getReceivedData(init_recv, init_len); + + if(!(init_len > 17 && init_recv[15] == RANGING_INITIATION)) { + return { false, 0}; + } + + DW1000Ng::setDeviceAddress(DW1000NgUtils::bytesAsValue(&init_recv[16], 2)); + return { true, DW1000NgUtils::bytesAsValue(&init_recv[13], 2) }; + } + + static RangeResult tagFinishRange(uint16_t anchor, uint16_t replyDelayUs) { + byte target_anchor[2]; + DW1000NgUtils::writeValueToBytes(target_anchor, anchor, 2); + DW1000NgRTLS::transmitPoll(target_anchor); + /* Start of poll control for range */ + if(!DW1000NgRTLS::waitForNextRangingStep()) return {false, false, 0, 0}; + size_t cont_len = DW1000Ng::getReceivedDataLength(); + byte cont_recv[cont_len]; + DW1000Ng::getReceivedData(cont_recv, cont_len); + + if (cont_len > 10 && cont_recv[9] == ACTIVITY_CONTROL && cont_recv[10] == RANGING_CONTINUE) { + /* Received Response to poll */ + DW1000NgRTLS::transmitFinalMessage( + &cont_recv[7], + replyDelayUs, + DW1000Ng::getTransmitTimestamp(), // Poll transmit time + DW1000Ng::getReceiveTimestamp() // Response to poll receive time + ); + } else { + return {false, false, 0, 0}; + } + + if(!DW1000NgRTLS::waitForNextRangingStep()) return {false, false, 0, 0}; + + size_t act_len = DW1000Ng::getReceivedDataLength(); + byte act_recv[act_len]; + DW1000Ng::getReceivedData(act_recv, act_len); + + if(act_len > 10 && act_recv[9] == ACTIVITY_CONTROL) { + if (act_len > 12 && act_recv[10] == RANGING_CONFIRM) { + return {true, true, DW1000NgUtils::bytesAsValue(&act_recv[11], 2), 0}; + } else if(act_len > 12 && act_recv[10] == ACTIVITY_FINISHED) { + return {true, false, 0, calculateNewBlinkRate(act_recv)}; + } + } else { + return {false, false, 0, 0}; + } + } + + RangeInfrastructureResult tagRangeInfrastructure(uint16_t target_anchor, uint16_t finalMessageDelay) { + RangeResult result = tagFinishRange(target_anchor, finalMessageDelay); + if(!result.success) return {false , 0}; + + while(result.success && result.next) { + result = tagFinishRange(result.next_anchor, finalMessageDelay); + if(!result.success) return {false , 0}; + + #if defined(ESP8266) + yield(); + #endif + } + + if(result.success && result.new_blink_rate != 0) { + return { true, result.new_blink_rate }; + } else { + if(!result.success) + return { false , 0 }; + } + } + + RangeInfrastructureResult tagTwrLocalize(uint16_t finalMessageDelay) { + RangeRequestResult request_result = DW1000NgRTLS::tagRangeRequest(); + + if(request_result.success) { + + RangeInfrastructureResult result = DW1000NgRTLS::tagRangeInfrastructure(request_result.target_anchor, finalMessageDelay); + + if(result.success) + return result; + } + return {false, 0}; + } + + RangeAcceptResult anchorRangeAccept(NextActivity next, uint16_t value) { + double range; + if(!DW1000NgRTLS::receiveFrame()) return {false, 0}; + + size_t poll_len = DW1000Ng::getReceivedDataLength(); + byte poll_data[poll_len]; + DW1000Ng::getReceivedData(poll_data, poll_len); + + if(poll_len > 9 && poll_data[9] == RANGING_TAG_POLL) { + uint64_t timePollReceived = DW1000Ng::getReceiveTimestamp(); + DW1000NgRTLS::transmitResponseToPoll(&poll_data[7]); + DW1000NgRTLS::waitForTransmission(); + uint64_t timeResponseToPoll = DW1000Ng::getTransmitTimestamp(); + delayMicroseconds(1500); + if(!DW1000NgRTLS::receiveFrame()) return {false, 0}; + + size_t rfinal_len = DW1000Ng::getReceivedDataLength(); + byte rfinal_data[rfinal_len]; + DW1000Ng::getReceivedData(rfinal_data, rfinal_len); + if(rfinal_len > 18 && rfinal_data[9] == RANGING_TAG_FINAL_RESPONSE_EMBEDDED) { + uint64_t timeFinalMessageReceive = DW1000Ng::getReceiveTimestamp(); + + byte finishValue[2]; + DW1000NgUtils::writeValueToBytes(finishValue, value, 2); + + if(next == NextActivity::RANGING_CONFIRM) + DW1000NgRTLS::transmitRangingConfirm(&rfinal_data[7], finishValue); + else + DW1000NgRTLS::transmitActivityFinished(&rfinal_data[7], finishValue); + + DW1000NgRTLS::waitForTransmission(); + + range = DW1000NgRanging::computeRangeAsymmetric( + DW1000NgUtils::bytesAsValue(rfinal_data + 10, LENGTH_TIMESTAMP), // Poll send time + timePollReceived, + timeResponseToPoll, // Response to poll sent time + DW1000NgUtils::bytesAsValue(rfinal_data + 14, LENGTH_TIMESTAMP), // Response to Poll Received + DW1000NgUtils::bytesAsValue(rfinal_data + 18, LENGTH_TIMESTAMP), // Final Message send time + timeFinalMessageReceive // Final message receive time + ); + + range = DW1000NgRanging::correctRange(range); + + /* In case of wrong read due to bad device calibration */ + if(range <= 0) + range = 0.000001; + + return {true, range}; + } + } + } + } \ No newline at end of file diff --git a/src/DW1000NgRTLS.hpp b/src/DW1000NgRTLS.hpp index 1e74e298..30abe999 100644 --- a/src/DW1000NgRTLS.hpp +++ b/src/DW1000NgRTLS.hpp @@ -67,12 +67,67 @@ constexpr byte NO_EX_ID = 0x40; constexpr byte BLINK_RATE_AND_LISTENING = 0x01; constexpr byte TAG_LISTENING_NOW = 0x02; +enum class NextActivity { + ACTIVITY_FINISHED, + RANGING_CONFIRM +}; + +typedef struct RangeRequestResult { + boolean success; + uint16_t target_anchor; +} RangeRequestResult; + +typedef struct RangeResult { + boolean success; + boolean next; + uint16_t next_anchor; + uint32_t new_blink_rate; +} RangeResult; + +typedef struct RangeInfrastructureResult { + boolean success; + uint16_t new_blink_rate; +} RangeInfrastructureResult; + +typedef struct RangeAcceptResult { + boolean success; + double range; +} RangeAcceptResult; + namespace DW1000NgRTLS { - void transmitShortBlink(); + /*** TWR functions used in ISO/IEC 24730-62:2013, refer to the standard or the decawave manual for details about TWR ***/ + byte increaseSequenceNumber(); + void transmitTwrShortBlink(); void transmitRangingInitiation(byte tag_eui[], byte tag_short_address[]); void transmitPoll(byte anchor_address[]); void transmitResponseToPoll(byte tag_short_address[]); void transmitFinalMessage(byte anchor_address[], uint16_t reply_delay, uint64_t timePollSent, uint64_t timeResponseToPollReceived); void transmitRangingConfirm(byte tag_short_address[], byte next_anchor[]); void transmitActivityFinished(byte tag_short_address[], byte blink_rate[]); + + boolean receiveFrame(); + void waitForTransmission(); + /*** End of TWR functions ***/ + + /* Send a request range from tag to the rtls infrastructure */ + RangeRequestResult tagRangeRequest(); + + /* Used by an anchor to accept an incoming tagRangeRequest by means of the infrastructure + NextActivity is used to indicate the tag what to do next after the ranging process (Activity finished is to return to blink (range request), + Continue range is to tell the tag to range a new anchor) + value is the value relative to the next activity (Activity finished = new blink rante, continue range = new anchor address) + */ + RangeAcceptResult anchorRangeAccept(NextActivity next, uint16_t value); + + /* Used by tag to range after range request accept of the infrastructure + Target anchor is given after a range request success + Finalmessagedelay is used in the process of TWR, a value of 1500 works on 8mhz-80mhz range devices, + you could try to decrease it to improve system performance. + */ + RangeInfrastructureResult tagRangeInfrastructure(uint16_t target_anchor, uint16_t finalMessageDelay); + + /* Can be used as a single function start the localization process from the tag. + Finalmessagedelay is the same as in function tagRangeInfrastructure + */ + RangeInfrastructureResult tagTwrLocalize(uint16_t finalMessageDelay); } \ No newline at end of file From 0a7d32ce980006f76974b8edf2dc57ffd023dbda Mon Sep 17 00:00:00 2001 From: Michele Biondi Date: Wed, 5 Dec 2018 16:08:11 +0100 Subject: [PATCH 5/5] Update library.properties --- library.properties | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/library.properties b/library.properties index 270f9630..c7e5916c 100644 --- a/library.properties +++ b/library.properties @@ -1,5 +1,5 @@ name=DW1000-ng -version=0.0.0 +version=0.1.0 author=Michele Biondi , Andrea Salvatori maintainer=Michele Biondi , Andrea Salvatori sentence=Arduino library to use Decawave's DW1000 IC.