diff --git a/examples/Shield/DW1000Ranging_ANCHOR/DW1000Ranging_ANCHOR.ino b/examples/Shield/DW1000Ranging_ANCHOR/DW1000Ranging_ANCHOR.ino new file mode 100644 index 0000000..b59ae5f --- /dev/null +++ b/examples/Shield/DW1000Ranging_ANCHOR/DW1000Ranging_ANCHOR.ino @@ -0,0 +1,110 @@ + +#pragma mark - Depend arduino-dw1000 +/* +cd ~/Arduino/libraries +git clone https://github.com/Xinyuan-LilyGO/arduino-dw1000.git +*/ + +#include "config.h" +#include "DW1000Ranging.h" + +#define SPI_SCLK 14 +#define SPI_MISO 33 +#define SPI_MOSI 15 + +#define BU01_CS 13 +#define BU01_RST 26 +#define BU01_WAKEUP 25 +#define BU01_IRQ 4//34 + +TTGOClass *watch; +SPIClass VSPI1(HSPI); +char buff[128]; + +#ifdef LILYGO_WATCH_HAS_DISPLAY +TFT_eSPI *tft; +TFT_eSprite *eSp; +#endif + +void newRange() +{ + float range = DW1000Ranging.getDistantDevice()->getRange(); + uint16_t addr = DW1000Ranging.getDistantDevice()->getShortAddress(); + float dbm = DW1000Ranging.getDistantDevice()->getRXPower(); + + Serial.print("from: "); Serial.print(addr, HEX); + Serial.print("\t Range: "); Serial.print(range); Serial.print(" m"); + Serial.print("\t RX power: "); Serial.print(dbm); Serial.println(" dBm"); + +#ifdef LILYGO_WATCH_HAS_DISPLAY + snprintf(buff, 128, "From:%x", addr); + tft->drawCentreString(buff, 60, 30, 2); + + eSp->fillSprite(TFT_BLACK); + eSp->setTextColor(TFT_GREEN, TFT_BLACK); + eSp->drawCentreString(String(range), 120, 0, 7); + eSp->pushSprite(0, 80); + + snprintf(buff, 128, "RX power:%.1f", dbm); + tft->drawCentreString(buff, 120, 180, 2); +#endif +} + +void newBlink(DW1000Device *device) +{ + Serial.print("blink; 1 device added ! -> "); + Serial.print(" short:"); + Serial.println(device->getShortAddress(), HEX); +#ifdef LILYGO_WATCH_HAS_DISPLAY + tft->fillScreen(TFT_BLACK); +#endif +} + +void inactiveDevice(DW1000Device *device) +{ + Serial.print("delete inactive device: "); + Serial.println(device->getShortAddress(), HEX); +#ifdef LILYGO_WATCH_HAS_DISPLAY + tft->fillScreen(TFT_BLACK); + tft->drawCentreString("delete inactive device: " + String(device->getShortAddress()), 120, 100, 2); +#endif +} + +void setup() +{ + Serial.begin(115200); + + watch = TTGOClass::getWatch(); + + watch->begin(); + + watch->enableLDO3(); + + VSPI1.begin(SPI_SCLK, SPI_MISO, SPI_MOSI); + + //init the configuration + DW1000Ranging.initCommunication(BU01_RST, BU01_CS, BU01_IRQ, VSPI1); //Reset, CS, IRQ pin + //define the sketch as anchor. It will be great to dynamically change the type of module + DW1000Ranging.attachNewRange(newRange); + DW1000Ranging.attachBlinkDevice(newBlink); + DW1000Ranging.attachInactiveDevice(inactiveDevice); + //Enable the filter to smooth the distance + DW1000Ranging.useRangeFilter(true); + + //we start the module as an anchor + DW1000Ranging.startAsAnchor("7D:00:22:EA:82:60:3B:9C", DW1000.MODE_LONGDATA_RANGE_ACCURACY); + +#ifdef LILYGO_WATCH_HAS_DISPLAY + tft = watch->tft; + watch->openBL(); + tft->drawCentreString("Start the module as an anchor", 120, 100, 2); + eSp = new TFT_eSprite(tft); + eSp->createSprite(240, 60); + eSp->setTextFont(2); +#endif +} + +void loop() +{ + DW1000Ranging.loop(); +} diff --git a/examples/Shield/DW1000Ranging_ANCHOR/config.h b/examples/Shield/DW1000Ranging_ANCHOR/config.h new file mode 100644 index 0000000..90af1a9 --- /dev/null +++ b/examples/Shield/DW1000Ranging_ANCHOR/config.h @@ -0,0 +1,13 @@ +// => Hardware select +#define LILYGO_WATCH_2019_WITH_TOUCH // To use T-Watch2019 with touchscreen, please uncomment this line +// #define LILYGO_WATCH_2019_NO_TOUCH // To use T-Watch2019 Not touchscreen , please uncomment this line +// #define LILYGO_WATCH_BLOCK + +//No SUPPORT!!!! +//#define LILYGO_LILYPI_V1 +//#define LILYGO_WATCH_2020_V1 +//No SUPPORT!!!! + +#include + + diff --git a/examples/Shield/DW1000Ranging_TAG/DW1000Ranging_TAG.ino b/examples/Shield/DW1000Ranging_TAG/DW1000Ranging_TAG.ino new file mode 100644 index 0000000..8a17431 --- /dev/null +++ b/examples/Shield/DW1000Ranging_TAG/DW1000Ranging_TAG.ino @@ -0,0 +1,109 @@ + +#pragma mark - Depend arduino-dw1000 +/* +cd ~/Arduino/libraries +git clone https://github.com/Xinyuan-LilyGO/arduino-dw1000.git +*/ +#include "config.h" +#include "DW1000Ranging.h" + +#define SPI_SCLK 14 +#define SPI_MISO 33 +#define SPI_MOSI 15 + +#define BU01_CS 13 +#define BU01_RST 26 +#define BU01_WAKEUP 25 +#define BU01_IRQ 4//34 + +TTGOClass *watch; +SPIClass VSPI1(HSPI); +char buff[128]; + +#ifdef LILYGO_WATCH_HAS_DISPLAY +TFT_eSPI *tft; +TFT_eSprite *eSp; +#endif + +void newRange() +{ + float range = DW1000Ranging.getDistantDevice()->getRange(); + uint16_t addr = DW1000Ranging.getDistantDevice()->getShortAddress(); + float dbm = DW1000Ranging.getDistantDevice()->getRXPower(); + + Serial.print("from: "); Serial.print(addr, HEX); + Serial.print("\t Range: "); Serial.print(range); Serial.print(" m"); + Serial.print("\t RX power: "); Serial.print(dbm); Serial.println(" dBm"); + +#ifdef LILYGO_WATCH_HAS_DISPLAY + snprintf(buff, 128, "From:%x", addr); + tft->drawCentreString(buff, 60, 30, 2); + + eSp->fillSprite(TFT_BLACK); + eSp->setTextColor(TFT_GREEN, TFT_BLACK); + eSp->drawCentreString(String(range), 120, 0, 7); + eSp->pushSprite(0, 80); + + snprintf(buff, 128, "RX power:%.1f", dbm); + tft->drawCentreString(buff, 120, 180, 2); +#endif +} + +void newDevice(DW1000Device *device) +{ + Serial.print("ranging init; 1 device added ! -> "); + Serial.print(" short:"); + Serial.println(device->getShortAddress(), HEX); +#ifdef LILYGO_WATCH_HAS_DISPLAY + tft->fillScreen(TFT_BLACK); +#endif +} + +void inactiveDevice(DW1000Device *device) +{ + Serial.print("delete inactive device: "); + Serial.println(device->getShortAddress(), HEX); +#ifdef LILYGO_WATCH_HAS_DISPLAY + tft->fillScreen(TFT_BLACK); + tft->drawCentreString("delete inactive device: " + String(device->getShortAddress()), 120, 100, 2); +#endif +} + +void setup() +{ + Serial.begin(115200); + + watch = TTGOClass::getWatch(); + + watch->begin(); + + watch->enableLDO3(); + + VSPI1.begin(SPI_SCLK, SPI_MISO, SPI_MOSI); + + //init the configuration + DW1000Ranging.initCommunication(BU01_RST, BU01_CS, BU01_IRQ, VSPI1); //Reset, CS, IRQ pin + //define the sketch as anchor. It will be great to dynamically change the type of module + DW1000Ranging.attachNewRange(newRange); + DW1000Ranging.attachNewDevice(newDevice); + DW1000Ranging.attachInactiveDevice(inactiveDevice); + //Enable the filter to smooth the distance + DW1000Ranging.useRangeFilter(true); + + //we start the module as a tag + DW1000Ranging.startAsTag("7D:00:22:EA:82:60:3B:9C", DW1000.MODE_LONGDATA_RANGE_ACCURACY); + +#ifdef LILYGO_WATCH_HAS_DISPLAY + tft = watch->tft; + watch->openBL(); + tft->drawCentreString("Start the module as a tag", 120, 100, 2); + eSp = new TFT_eSprite(tft); + eSp->createSprite(240, 60); + eSp->setTextFont(2); +#endif +} + +void loop() +{ + DW1000Ranging.loop(); +} diff --git a/examples/Shield/DW1000Ranging_TAG/config.h b/examples/Shield/DW1000Ranging_TAG/config.h new file mode 100644 index 0000000..90af1a9 --- /dev/null +++ b/examples/Shield/DW1000Ranging_TAG/config.h @@ -0,0 +1,13 @@ +// => Hardware select +#define LILYGO_WATCH_2019_WITH_TOUCH // To use T-Watch2019 with touchscreen, please uncomment this line +// #define LILYGO_WATCH_2019_NO_TOUCH // To use T-Watch2019 Not touchscreen , please uncomment this line +// #define LILYGO_WATCH_BLOCK + +//No SUPPORT!!!! +//#define LILYGO_LILYPI_V1 +//#define LILYGO_WATCH_2020_V1 +//No SUPPORT!!!! + +#include + + diff --git a/examples/Shield/DW1000_BasicReceiver/DW1000_BasicReceiver.ino b/examples/Shield/DW1000_BasicReceiver/DW1000_BasicReceiver.ino new file mode 100644 index 0000000..3f6cbc0 --- /dev/null +++ b/examples/Shield/DW1000_BasicReceiver/DW1000_BasicReceiver.ino @@ -0,0 +1,143 @@ + +#pragma mark - Depend arduino-dw1000 +/* +cd ~/Arduino/libraries +git clone https://github.com/Xinyuan-LilyGO/arduino-dw1000.git +*/ +#include "config.h" +#include + +#define SPI_SCLK 14 +#define SPI_MISO 33 +#define SPI_MOSI 15 + +#define BU01_CS 13 +#define BU01_RST 26 +#define BU01_WAKEUP 25 +#define BU01_IRQ 4//34 + +TTGOClass *watch; +SPIClass VSPI1(HSPI); +char buff[128]; + +#ifdef LILYGO_WATCH_HAS_DISPLAY +TFT_eSPI *tft; +#endif + +// DEBUG packet sent status and count +volatile boolean received = false; +volatile boolean error = false; +volatile int16_t numReceived = 0; // todo check int type +String message; + + +void handleReceived() +{ + // status change on reception success + received = true; +} + +void handleError() +{ + error = true; +} + +void receiver() +{ + DW1000.newReceive(); + DW1000.setDefaults(); + // so we don't need to restart the receiver manually + DW1000.receivePermanently(true); + DW1000.startReceive(); +} + + +void setup() +{ + Serial.begin(115200); + + watch = TTGOClass::getWatch(); + + watch->begin(); + + watch->enableLDO3(); + + VSPI1.begin(SPI_SCLK, SPI_MISO, SPI_MOSI); + + Serial.println(F("### DW1000-arduino-receiver-test ###")); + // initialize the driver + DW1000.begin(BU01_IRQ, BU01_RST, VSPI1); + DW1000.select(BU01_CS); + Serial.println(F("DW1000 initialized ...")); + // general configuration + DW1000.newConfiguration(); + DW1000.setDefaults(); + DW1000.setDeviceAddress(6); + DW1000.setNetworkId(10); + DW1000.enableMode(DW1000.MODE_LONGDATA_RANGE_LOWPOWER); + DW1000.commitConfiguration(); + Serial.println(F("Committed configuration ...")); + // DEBUG chip info and registers pretty printed + char msg[128]; + DW1000.getPrintableDeviceIdentifier(msg); + Serial.print("Device ID: "); Serial.println(msg); + DW1000.getPrintableExtendedUniqueIdentifier(msg); + Serial.print("Unique ID: "); Serial.println(msg); + DW1000.getPrintableNetworkIdAndShortAddress(msg); + Serial.print("Network ID & Device Address: "); Serial.println(msg); + DW1000.getPrintableDeviceMode(msg); + Serial.print("Device mode: "); Serial.println(msg); + // attach callback for (successfully) received messages + DW1000.attachReceivedHandler(handleReceived); + DW1000.attachReceiveFailedHandler(handleError); + DW1000.attachErrorHandler(handleError); + // start reception + receiver(); + + +#ifdef LILYGO_WATCH_HAS_DISPLAY + tft = watch->tft; + tft->drawCentreString("Start Receiver", 120, 100, 2); + watch->openBL(); +#endif +} + +void loop() +{ + // enter on confirmation of ISR status change (successfully received) + if (received) { + numReceived++; + // get data as string + DW1000.getData(message); + Serial.print("Received message ... #"); + Serial.println(numReceived); + Serial.print("Data is ... "); + Serial.println(message); + Serial.print("FP power is [dBm] ... "); + Serial.println(DW1000.getFirstPathPower()); + Serial.print("RX power is [dBm] ... "); + Serial.println(DW1000.getReceivePower()); + Serial.print("Signal quality is ... "); + Serial.println(DW1000.getReceiveQuality()); +#ifdef LILYGO_WATCH_HAS_DISPLAY + tft->setCursor(0, 0); + tft->print("Received message ... #"); + tft->println(numReceived); + tft->print("Data is ... "); + tft->println(message); + tft->print("FP power is [dBm] ... "); + tft->println(DW1000.getFirstPathPower()); + tft->print("RX power is [dBm] ... "); + tft->println(DW1000.getReceivePower()); + tft->print("Signal quality is ... "); + tft->println(DW1000.getReceiveQuality()); +#endif + received = false; + } + if (error) { + Serial.println("Error receiving a message"); + error = false; + DW1000.getData(message); + Serial.print("Error data is ... "); Serial.println(message); + } +} diff --git a/examples/Shield/DW1000_BasicReceiver/config.h b/examples/Shield/DW1000_BasicReceiver/config.h new file mode 100644 index 0000000..cc8a5cd --- /dev/null +++ b/examples/Shield/DW1000_BasicReceiver/config.h @@ -0,0 +1,14 @@ +// => Hardware select +#define LILYGO_WATCH_2019_WITH_TOUCH // To use T-Watch2019 with touchscreen, please uncomment this line +// #define LILYGO_WATCH_2019_NO_TOUCH // To use T-Watch2019 Not touchscreen , please uncomment this line +// #define LILYGO_WATCH_BLOCK + +//No SUPPORT!!!! +//#define LILYGO_LILYPI_V1 +//#define LILYGO_WATCH_2020_V1 +//No SUPPORT!!!! + + +#include + + diff --git a/examples/Shield/DW1000_BasicSender/DW1000_BasicSender.ino b/examples/Shield/DW1000_BasicSender/DW1000_BasicSender.ino new file mode 100644 index 0000000..ccb70a7 --- /dev/null +++ b/examples/Shield/DW1000_BasicSender/DW1000_BasicSender.ino @@ -0,0 +1,137 @@ + +#pragma mark - Depend arduino-dw1000 +/* +cd ~/Arduino/libraries +git clone https://github.com/Xinyuan-LilyGO/arduino-dw1000.git +*/ +#include "config.h" +#include + +#define SPI_SCLK 14 +#define SPI_MISO 33 +#define SPI_MOSI 15 + +#define BU01_CS 13 +#define BU01_RST 26 +#define BU01_WAKEUP 25 +#define BU01_IRQ 4//34 + +TTGOClass *watch; +SPIClass VSPI1(HSPI); +char buff[128]; + +#ifdef LILYGO_WATCH_HAS_DISPLAY +TFT_eSPI *tft; +#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 +DW1000Time sentTime; + + +void handleSent() +{ + // status change on sent success + sentAck = true; +} + +void transmitter() +{ + // transmit some data + Serial.print("Transmitting packet ... #"); Serial.println(sentNum); + DW1000.newTransmit(); + DW1000.setDefaults(); + String msg = "Hello DW1000, it's #"; msg += sentNum; + DW1000.setData(msg); + // delay sending the message for the given amount + DW1000Time deltaTime = DW1000Time(10, DW1000Time::MILLISECONDS); + DW1000.setDelay(deltaTime); + DW1000.startTransmit(); + delaySent = millis(); +} + + +void setup() +{ + Serial.begin(115200); + + watch = TTGOClass::getWatch(); + + watch->begin(); + + watch->enableLDO3(); + + VSPI1.begin(SPI_SCLK, SPI_MISO, SPI_MOSI); + + Serial.println(F("### DW1000-arduino-receiver-test ###")); + // initialize the driver + DW1000.begin(BU01_IRQ, BU01_RST, VSPI1); + DW1000.select(BU01_CS); + Serial.println(F("DW1000 initialized ...")); + // general configuration + DW1000.newConfiguration(); + DW1000.setDefaults(); + DW1000.setDeviceAddress(5); + DW1000.setNetworkId(10); + DW1000.enableMode(DW1000.MODE_LONGDATA_RANGE_LOWPOWER); + DW1000.commitConfiguration(); + Serial.println(F("Committed configuration ...")); + // DEBUG chip info and registers pretty printed + char msg[128]; + DW1000.getPrintableDeviceIdentifier(msg); + Serial.print("Device ID: "); Serial.println(msg); + DW1000.getPrintableExtendedUniqueIdentifier(msg); + Serial.print("Unique ID: "); Serial.println(msg); + DW1000.getPrintableNetworkIdAndShortAddress(msg); + Serial.print("Network ID & Device Address: "); Serial.println(msg); + DW1000.getPrintableDeviceMode(msg); + Serial.print("Device mode: "); Serial.println(msg); + // attach callback for (successfully) sent messages + DW1000.attachSentHandler(handleSent); + // start a transmission + transmitter(); + + +#ifdef LILYGO_WATCH_HAS_DISPLAY + tft = watch->tft; + watch->openBL(); + tft->drawCentreString("Start BasicSender example", 120, 100, 2); +#endif +} + +void loop() +{ + if (!sentAck) { + return; + } + // continue on success confirmation + // (we are here after the given amount of send delay time has passed) + sentAck = false; + // update and print some information about the sent message + Serial.print("ARDUINO delay sent [ms] ... "); + Serial.println(millis() - delaySent); + DW1000Time newSentTime; + DW1000.getTransmitTimestamp(newSentTime); + Serial.print("Processed packet ... #"); + Serial.println(sentNum); + Serial.print("Sent timestamp ... "); + Serial.println(newSentTime.getAsMicroSeconds()); + +#ifdef LILYGO_WATCH_HAS_DISPLAY + tft->setCursor(0, 0); + tft->print("Processed packet ... #"); + tft->println(sentNum); + tft->print("Sent timestamp ... "); + tft->println(newSentTime.getAsMicroSeconds()); +#endif + // note: delta is just for simple demo as not correct on system time counter wrap-around + Serial.print("DW1000 delta send time [ms] ... "); + Serial.println((newSentTime.getAsMicroSeconds() - sentTime.getAsMicroSeconds()) * 1.0e-3); + sentTime = newSentTime; + sentNum++; + // again, transmit some data + transmitter(); +} diff --git a/examples/Shield/DW1000_BasicSender/config.h b/examples/Shield/DW1000_BasicSender/config.h new file mode 100644 index 0000000..97d8bbe --- /dev/null +++ b/examples/Shield/DW1000_BasicSender/config.h @@ -0,0 +1,16 @@ +// => Hardware select +#define LILYGO_WATCH_2019_WITH_TOUCH // To use T-Watch2019 with touchscreen, please uncomment this line +// #define LILYGO_WATCH_2019_NO_TOUCH // To use T-Watch2019 Not touchscreen , please uncomment this line +// #define LILYGO_WATCH_BLOCK + +//No SUPPORT!!!! +//#define LILYGO_LILYPI_V1 +//#define LILYGO_WATCH_2020_V1 +//No SUPPORT!!!! + +// Function select +// #define LILYGO_DW1000_MODULE + +#include + + diff --git a/examples/Shield/DW1000_RangingAnchor/DW1000_RangingAnchor.ino b/examples/Shield/DW1000_RangingAnchor/DW1000_RangingAnchor.ino new file mode 100644 index 0000000..6cab6ff --- /dev/null +++ b/examples/Shield/DW1000_RangingAnchor/DW1000_RangingAnchor.ino @@ -0,0 +1,288 @@ + +#pragma mark - Depend arduino-dw1000 +/* +cd ~/Arduino/libraries +git clone https://github.com/Xinyuan-LilyGO/arduino-dw1000.git +*/ +#include "config.h" +#include + +#define SPI_SCLK 14 +#define SPI_MISO 33 +#define SPI_MOSI 15 + +#define BU01_CS 13 +#define BU01_RST 26 +#define BU01_WAKEUP 25 +#define BU01_IRQ 4//34 + +TTGOClass *watch; +SPIClass VSPI1(HSPI); +char buff[128]; + +#ifdef LILYGO_WATCH_HAS_DISPLAY +TFT_eSPI *tft; +#endif + + +// messages used in the ranging protocol +// TODO replace by enum +#define POLL 0 +#define POLL_ACK 1 +#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; +DW1000Time timePollReceived; +DW1000Time timePollAckSent; +DW1000Time timePollAckReceived; +DW1000Time timeRangeSent; +DW1000Time timeRangeReceived; +// last computed range/time +DW1000Time timeComputedRange; +// data buffer +#define LEN_DATA 16 +byte data[LEN_DATA]; +// watchdog and reset period +uint32_t lastActivity; +uint32_t resetPeriod = 250; +// reply times (same on both sides for symm. ranging) +uint16_t replyDelayTimeUS = 3000; +// ranging counter (per second) +uint16_t successRangingCount = 0; +uint32_t rangingCountPeriod = 0; +float samplingRate = 0; + + +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() +{ + // status change on sent success + sentAck = true; +} + +void handleReceived() +{ + // status change on received success + receivedAck = true; +} + +void transmitPollAck() +{ + DW1000.newTransmit(); + DW1000.setDefaults(); + data[0] = POLL_ACK; + // delay the same amount as ranging tag + DW1000Time deltaTime = DW1000Time(replyDelayTimeUS, DW1000Time::MICROSECONDS); + DW1000.setDelay(deltaTime); + DW1000.setData(data, LEN_DATA); + DW1000.startTransmit(); +} + +void transmitRangeReport(float curRange) +{ + DW1000.newTransmit(); + DW1000.setDefaults(); + data[0] = RANGE_REPORT; + // write final ranging result + memcpy(data + 1, &curRange, 4); + DW1000.setData(data, LEN_DATA); + DW1000.startTransmit(); +} + +void transmitRangeFailed() +{ + DW1000.newTransmit(); + DW1000.setDefaults(); + data[0] = RANGE_FAILED; + DW1000.setData(data, LEN_DATA); + DW1000.startTransmit(); +} + +void receiver() +{ + DW1000.newReceive(); + DW1000.setDefaults(); + // so we don't need to restart the receiver manually + DW1000.receivePermanently(true); + DW1000.startReceive(); +} + +/* + * 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() +{ + // asymmetric two-way ranging (more computation intense, less error prone) + DW1000Time round1 = (timePollAckReceived - timePollSent).wrap(); + DW1000Time reply1 = (timePollAckSent - timePollReceived).wrap(); + DW1000Time round2 = (timeRangeReceived - timePollAckSent).wrap(); + DW1000Time reply2 = (timeRangeSent - timePollAckReceived).wrap(); + 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; + // set tof timestamp + timeComputedRange.setTimestamp(tof); +} + +/* + * END RANGING ALGORITHMS + * ---------------------- + */ + +void setup() +{ + Serial.begin(115200); + + watch = TTGOClass::getWatch(); + + watch->begin(); + + watch->enableLDO3(); + + VSPI1.begin(SPI_SCLK, SPI_MISO, SPI_MOSI); + + Serial.println(F("### DW1000-arduino-receiver-test ###")); + // initialize the driver + DW1000.begin(BU01_IRQ, BU01_RST, VSPI1); + DW1000.select(BU01_CS); + Serial.println(F("DW1000 initialized ...")); + + // general configuration + DW1000.newConfiguration(); + DW1000.setDefaults(); + DW1000.setDeviceAddress(1); + DW1000.setNetworkId(10); + DW1000.enableMode(DW1000.MODE_LONGDATA_RANGE_LOWPOWER); + DW1000.commitConfiguration(); + Serial.println(F("Committed configuration ...")); + // DEBUG chip info and registers pretty printed + char msg[128]; + DW1000.getPrintableDeviceIdentifier(msg); + Serial.print("Device ID: "); Serial.println(msg); + DW1000.getPrintableExtendedUniqueIdentifier(msg); + Serial.print("Unique ID: "); Serial.println(msg); + DW1000.getPrintableNetworkIdAndShortAddress(msg); + Serial.print("Network ID & Device Address: "); Serial.println(msg); + DW1000.getPrintableDeviceMode(msg); + Serial.print("Device mode: "); Serial.println(msg); + // attach callback for (successfully) sent and received messages + DW1000.attachSentHandler(handleSent); + DW1000.attachReceivedHandler(handleReceived); + // anchor starts in receiving mode, awaiting a ranging poll message + receiver(); + noteActivity(); + // for first time ranging frequency computation + rangingCountPeriod = millis(); + +#ifdef LILYGO_WATCH_HAS_DISPLAY + tft = watch->tft; + tft->drawCentreString("Start RangingAnchor", 120, 100, 2); + watch->openBL(); +#endif +} + +void loop() +{ + int32_t curMillis = millis(); + if (!sentAck && !receivedAck) { + // check if inactive + if (curMillis - lastActivity > resetPeriod) { + resetInactive(); + } + return; + } + // continue on any success confirmation + if (sentAck) { + sentAck = false; + byte msgId = data[0]; + if (msgId == POLL_ACK) { + DW1000.getTransmitTimestamp(timePollAckSent); + noteActivity(); + } + } + if (receivedAck) { + receivedAck = false; + // get message and parse + DW1000.getData(data, LEN_DATA); + byte msgId = data[0]; + if (msgId != expectedMsgId) { + // 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(); + noteActivity(); + } else if (msgId == RANGE) { + DW1000.getReceiveTimestamp(timeRangeReceived); + expectedMsgId = POLL; + if (!protocolFailed) { + timePollSent.setTimestamp(data + 1); + timePollAckReceived.setTimestamp(data + 6); + timeRangeSent.setTimestamp(data + 11); + // (re-)compute range as two-way ranging is done + computeRangeAsymmetric(); // CHOSEN RANGING ALGORITHM + transmitRangeReport(timeComputedRange.getAsMicroSeconds()); + float distance = timeComputedRange.getAsMeters(); + Serial.print("Range: "); Serial.print(distance); Serial.print(" m"); + Serial.print("\t RX power: "); Serial.print(DW1000.getReceivePower()); Serial.print(" dBm"); + Serial.print("\t Sampling: "); Serial.print(samplingRate); Serial.println(" Hz"); + //Serial.print("FP power is [dBm]: "); Serial.print(DW1000.getFirstPathPower()); + //Serial.print("RX power is [dBm]: "); Serial.println(DW1000.getReceivePower()); + //Serial.print("Receive quality: "); Serial.println(DW1000.getReceiveQuality()); + // update sampling rate (each second) + successRangingCount++; + if (curMillis - rangingCountPeriod > 1000) { + samplingRate = (1000.0f * successRangingCount) / (curMillis - rangingCountPeriod); + rangingCountPeriod = curMillis; + successRangingCount = 0; + } + } else { + transmitRangeFailed(); + } + + noteActivity(); + } + } +} diff --git a/examples/Shield/DW1000_RangingAnchor/config.h b/examples/Shield/DW1000_RangingAnchor/config.h new file mode 100644 index 0000000..cc8a5cd --- /dev/null +++ b/examples/Shield/DW1000_RangingAnchor/config.h @@ -0,0 +1,14 @@ +// => Hardware select +#define LILYGO_WATCH_2019_WITH_TOUCH // To use T-Watch2019 with touchscreen, please uncomment this line +// #define LILYGO_WATCH_2019_NO_TOUCH // To use T-Watch2019 Not touchscreen , please uncomment this line +// #define LILYGO_WATCH_BLOCK + +//No SUPPORT!!!! +//#define LILYGO_LILYPI_V1 +//#define LILYGO_WATCH_2020_V1 +//No SUPPORT!!!! + + +#include + + diff --git a/examples/Shield/DW1000_RangingTag/DW1000_RangingTag.ino b/examples/Shield/DW1000_RangingTag/DW1000_RangingTag.ino new file mode 100644 index 0000000..6d5a152 --- /dev/null +++ b/examples/Shield/DW1000_RangingTag/DW1000_RangingTag.ino @@ -0,0 +1,216 @@ + +#pragma mark - Depend arduino-dw1000 +/* +cd ~/Arduino/libraries +git clone https://github.com/Xinyuan-LilyGO/arduino-dw1000.git +*/ +#include "config.h" +#include + +#define SPI_SCLK 14 +#define SPI_MISO 33 +#define SPI_MOSI 15 + +#define BU01_CS 13 +#define BU01_RST 26 +#define BU01_WAKEUP 25 +#define BU01_IRQ 4//34 + +TTGOClass *watch; +SPIClass VSPI1(HSPI); +char buff[128]; + +#ifdef LILYGO_WATCH_HAS_DISPLAY +TFT_eSPI *tft; +#endif + + +// messages used in the ranging protocol +// TODO replace by enum +#define POLL 0 +#define POLL_ACK 1 +#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 +DW1000Time timePollSent; +DW1000Time timePollAckReceived; +DW1000Time timeRangeSent; +// data buffer +#define LEN_DATA 16 +byte data[LEN_DATA]; +// watchdog and reset period +uint32_t lastActivity; +uint32_t resetPeriod = 250; +// reply times (same on both sides for symm. ranging) +uint16_t replyDelayTimeUS = 3000; + + +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() +{ + // status change on sent success + sentAck = true; +} + +void handleReceived() +{ + // status change on received success + receivedAck = true; +} + +void transmitPoll() +{ + DW1000.newTransmit(); + DW1000.setDefaults(); + data[0] = POLL; + DW1000.setData(data, LEN_DATA); + DW1000.startTransmit(); +} + +void transmitRange() +{ + DW1000.newTransmit(); + DW1000.setDefaults(); + data[0] = RANGE; + // delay sending the message and remember expected future sent timestamp + DW1000Time deltaTime = DW1000Time(replyDelayTimeUS, DW1000Time::MICROSECONDS); + timeRangeSent = DW1000.setDelay(deltaTime); + timePollSent.getTimestamp(data + 1); + timePollAckReceived.getTimestamp(data + 6); + timeRangeSent.getTimestamp(data + 11); + DW1000.setData(data, LEN_DATA); + DW1000.startTransmit(); + //Serial.print("Expect RANGE to be sent @ "); Serial.println(timeRangeSent.getAsFloat()); +} + +void receiver() +{ + DW1000.newReceive(); + DW1000.setDefaults(); + // so we don't need to restart the receiver manually + DW1000.receivePermanently(true); + DW1000.startReceive(); +} + + +void setup() +{ + Serial.begin(115200); + + watch = TTGOClass::getWatch(); + + watch->begin(); + + watch->enableLDO3(); + + VSPI1.begin(SPI_SCLK, SPI_MISO, SPI_MOSI); + + Serial.println(F("### DW1000-arduino-receiver-test ###")); + // initialize the driver + DW1000.begin(BU01_IRQ, BU01_RST, VSPI1); + DW1000.select(BU01_CS); + Serial.println(F("DW1000 initialized ...")); + + // general configuration + DW1000.newConfiguration(); + DW1000.setDefaults(); + DW1000.setDeviceAddress(2); + DW1000.setNetworkId(10); + DW1000.enableMode(DW1000.MODE_LONGDATA_RANGE_LOWPOWER); + DW1000.commitConfiguration(); + Serial.println(F("Committed configuration ...")); + // DEBUG chip info and registers pretty printed + char msg[128]; + DW1000.getPrintableDeviceIdentifier(msg); + Serial.print("Device ID: "); Serial.println(msg); + DW1000.getPrintableExtendedUniqueIdentifier(msg); + Serial.print("Unique ID: "); Serial.println(msg); + DW1000.getPrintableNetworkIdAndShortAddress(msg); + Serial.print("Network ID & Device Address: "); Serial.println(msg); + DW1000.getPrintableDeviceMode(msg); + Serial.print("Device mode: "); Serial.println(msg); + // attach callback for (successfully) sent and received messages + DW1000.attachSentHandler(handleSent); + DW1000.attachReceivedHandler(handleReceived); + // anchor starts by transmitting a POLL message + receiver(); + transmitPoll(); + noteActivity(); + +#ifdef LILYGO_WATCH_HAS_DISPLAY + tft = watch->tft; + tft->drawCentreString("Start RangingTag", 120, 100, 2); + watch->openBL(); +#endif +} + +void loop() +{ + if (!sentAck && !receivedAck) { + // check if inactive + if (millis() - lastActivity > resetPeriod) { + resetInactive(); + } + return; + } + // continue on any success confirmation + if (sentAck) { + sentAck = false; + byte msgId = data[0]; + if (msgId == POLL) { + DW1000.getTransmitTimestamp(timePollSent); + //Serial.print("Sent POLL @ "); Serial.println(timePollSent.getAsFloat()); + } else if (msgId == RANGE) { + DW1000.getTransmitTimestamp(timeRangeSent); + noteActivity(); + } + } + if (receivedAck) { + receivedAck = false; + // get message and parse + 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); + expectedMsgId = POLL_ACK; + transmitPoll(); + return; + } + if (msgId == POLL_ACK) { + DW1000.getReceiveTimestamp(timePollAckReceived); + expectedMsgId = RANGE_REPORT; + transmitRange(); + noteActivity(); + } else if (msgId == RANGE_REPORT) { + expectedMsgId = POLL_ACK; + float curRange; + memcpy(&curRange, data + 1, 4); + transmitPoll(); + noteActivity(); + } else if (msgId == RANGE_FAILED) { + expectedMsgId = POLL_ACK; + transmitPoll(); + noteActivity(); + } + } +} diff --git a/examples/Shield/DW1000_RangingTag/config.h b/examples/Shield/DW1000_RangingTag/config.h new file mode 100644 index 0000000..cc8a5cd --- /dev/null +++ b/examples/Shield/DW1000_RangingTag/config.h @@ -0,0 +1,14 @@ +// => Hardware select +#define LILYGO_WATCH_2019_WITH_TOUCH // To use T-Watch2019 with touchscreen, please uncomment this line +// #define LILYGO_WATCH_2019_NO_TOUCH // To use T-Watch2019 Not touchscreen , please uncomment this line +// #define LILYGO_WATCH_BLOCK + +//No SUPPORT!!!! +//#define LILYGO_LILYPI_V1 +//#define LILYGO_WATCH_2020_V1 +//No SUPPORT!!!! + + +#include + +