From 94ece7989916c9c025e8d2cc45c82a9afe032ad0 Mon Sep 17 00:00:00 2001 From: Eric Date: Tue, 9 Jul 2024 23:52:50 -0700 Subject: [PATCH] UWB Trilateration with ESP32, DW3000 #LocalPositioning --- .../4_UWB_Trilateration.ino | 264 ++++++++++++++++++ .../4_UWB_Trilateration/Trilateration.hpp | 44 +++ ESP32_UWB/4_UWB_Trilateration/config.h | 19 ++ 3 files changed, 327 insertions(+) create mode 100644 ESP32_UWB/4_UWB_Trilateration/4_UWB_Trilateration.ino create mode 100644 ESP32_UWB/4_UWB_Trilateration/Trilateration.hpp create mode 100644 ESP32_UWB/4_UWB_Trilateration/config.h diff --git a/ESP32_UWB/4_UWB_Trilateration/4_UWB_Trilateration.ino b/ESP32_UWB/4_UWB_Trilateration/4_UWB_Trilateration.ino new file mode 100644 index 0000000..25f4c34 --- /dev/null +++ b/ESP32_UWB/4_UWB_Trilateration/4_UWB_Trilateration.ino @@ -0,0 +1,264 @@ +///////////////////////////////////////////////////////////////// +/* + UWB Trilateration with ESP32, DW3000 #LocalPositioning + For More Information: https://youtu.be/NjHMXEOxNRE + Created by Eric N. (ThatProject) +*/ +///////////////////////////////////////////////////////////////// +// Version Info +// ESP32 v3.0.2 +// Hardware Info +// https://github.com/Makerfabs/MaUWB_DW3000-with-STM32-AT-Command/ + +#include "config.h" +#include "Trilateration.hpp" +#include +#include +#include + +HardwareSerial SERIAL_AT(2); +Adafruit_SSD1306 display(128, 64, &Wire, -1); + +String response = ""; + +Anchor anchor0(-340, 240); +Anchor anchor1(340, 240); +Anchor anchor2(0, -240); + +Trilateration trilateration(&anchor0, &anchor1, &anchor2); + +void setup() { + pinMode(RESET, OUTPUT); + digitalWrite(RESET, HIGH); + + SERIAL_LOG.begin(115200); + + xTaskCreatePinnedToCore(Task_UWB, + "Task_UWB", + 1024 * 20, + NULL, + tskIDLE_PRIORITY, + NULL, + 0); +} + +void loop() {} + +void drawScreen() { + display.clearDisplay(); + display.setTextSize(1); // Normal 1:1 pixel scale + display.setTextColor(SSD1306_WHITE); // Draw white text + display.setCursor(0, 0); // Start at top-left corner + display.println(F("UWB Trilateration")); + display.setCursor(0, 10); + + String freqText = "Freq: "; + +#ifdef FREQ_850K + freqText = freqText + "850k"; +#else + freqText = freqText + "6.8M"; +#endif + display.println(freqText); + display.setTextSize(2); + display.setCursor(0, 20); + + String infoText = ""; + +#ifdef TAG + infoText = infoText + "Tag: " + UWB_INDEX; +#else + infoText = infoText + "Anchor: " + UWB_INDEX; +#endif + display.println(infoText); + +#ifdef TAG + display.setTextSize(1); + display.setCursor(0, 40); + display.println("User Location: "); + +#else + display.setCursor(0, 50); + display.setTextSize(1); + display.println("......Updating......"); +#endif + display.display(); + + delay(1000); +} + +String sendData(String command, const int timeout, boolean debug) { + String response = ""; + + SERIAL_LOG.println(command); + SERIAL_AT.println(command); // send the read character to the SERIAL_LOG + + long int time = millis(); + + while ((time + timeout) > millis()) { + while (SERIAL_AT.available()) { + + // The esp has data so display its output to the serial window + char c = SERIAL_AT.read(); // read the next character. + response += c; + } + } + return response; +} + +String config_cmd() { + String temp = "AT+SETCFG="; + + // Set device id + temp = temp + UWB_INDEX; + + // Set device role +#ifdef TAG + temp = temp + ",0"; +#endif +#ifdef ANCHOR + temp = temp + ",1"; +#endif + + // Set frequence 850k or 6.8M +#ifdef FREQ_850K + temp = temp + ",0"; +#endif +#ifdef FREQ_6800K + temp = temp + ",1"; +#endif + + // Set range filter + temp = temp + ",1"; + + return temp; +} + +String cap_cmd() { + String temp = "AT+SETCAP="; + + // Set Tag capacity + temp = temp + UWB_TAG_COUNT; + + // Time of a single time slot +#ifdef FREQ_850K + temp = temp + ",15"; +#endif +#ifdef FREQ_6800K + temp = temp + ",10"; +#endif + + return temp; +} + + + +void Task_UWB(void *pvParameters) { + + SERIAL_AT.begin(115200, SERIAL_8N1, IO_RXD2, IO_TXD2); + + SERIAL_AT.println("AT"); + Wire.begin(I2C_SDA, I2C_SCL); + delay(1000); + // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally + if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Address 0x3C for 128x32 + SERIAL_LOG.println(F("SSD1306 allocation failed")); + for (;;) + ; // Don't proceed, loop forever + } + display.clearDisplay(); + + drawScreen(); + + sendData("AT?", 2000, 1); + //sendData("AT+RESTORE", 5000, 1); + + sendData(config_cmd(), 2000, 1); + sendData(cap_cmd(), 2000, 1); + + sendData("AT+SETRPT=1", 2000, 1); + //sendData("AT+SAVE", 2000, 1); + sendData("AT+RESTART", 2000, 1); + + + while (1) { + +#ifdef TAG + tag_action(); +#else + vTaskDelay(10); +#endif + } +} + +void tag_action() { + while (SERIAL_LOG.available() > 0) { + SERIAL_AT.write(SERIAL_LOG.read()); + yield(); + } + while (SERIAL_AT.available() > 0) { + char c = SERIAL_AT.read(); + + if (c == '\r') + continue; + else if (c == '\n' || c == '\r') { + SERIAL_LOG.println(response); + + float rangeValues[3]; + // Parse data, update distances, and calculate user location + if (parseData(response, rangeValues)) { + updateAnchorDistances(rangeValues); + calculateUserLocation(); + } + + response = ""; + vTaskDelay(10); + } else + response += c; + } +} + +bool parseData(String dataString, float rangeValues[3]) { + // Find the position of "range:(" + int rangeStart = dataString.indexOf("range:("); + if (rangeStart == -1) return false; // Return false if "range:(" not found + + // Extract the substring containing the range values + int rangeEnd = dataString.indexOf(")", rangeStart); + String rangePart = dataString.substring(rangeStart + 7, rangeEnd); + + // Parse the range values + char charArray[rangePart.length() + 1]; + rangePart.toCharArray(charArray, rangePart.length() + 1); + char *token = strtok(charArray, ","); + int valueIndex = 0; + while (token != NULL && valueIndex < 3) { + rangeValues[valueIndex] = atoi(token); + valueIndex++; + token = strtok(NULL, ","); + } + + return true; +} + +// Function to update the distances for the anchors +void updateAnchorDistances(float rangeValues[3]) { + anchor0.updateDistance(rangeValues[0]); + anchor1.updateDistance(rangeValues[1]); + anchor2.updateDistance(rangeValues[2]); +} + +// Function to perform trilateration and calculate user location +void calculateUserLocation() { + float user_x, user_y; + trilateration.calcUserLocation(user_x, user_y); + + char buffer[100]; + snprintf(buffer, sizeof(buffer), "x = %.2f\ny = %.2f", user_x, user_y); + Serial.println(buffer); + + display.fillRect(0, 48, 128, 16, SSD1306_BLACK); + display.setCursor(0, 48); + display.print(buffer); + display.display(); +} \ No newline at end of file diff --git a/ESP32_UWB/4_UWB_Trilateration/Trilateration.hpp b/ESP32_UWB/4_UWB_Trilateration/Trilateration.hpp new file mode 100644 index 0000000..8ebe754 --- /dev/null +++ b/ESP32_UWB/4_UWB_Trilateration/Trilateration.hpp @@ -0,0 +1,44 @@ +#include + +class Anchor { +public: + float x; + float y; + float distance; + + Anchor(float x, float y) { + this->x = x; + this->y = y; + this->distance = 0.0; + } + + void updateDistance(float distance) { + this->distance = distance; + } +}; + +class Trilateration { +private: + Anchor *anchor0; + Anchor *anchor1; + Anchor *anchor2; + +public: + Trilateration(Anchor *a0, Anchor *a1, Anchor *a2) { + anchor0 = a0; + anchor1 = a1; + anchor2 = a2; + } + + void calcUserLocation(float &user_x, float &user_y) { + float A = 2 * (anchor1->x - anchor0->x); + float B = 2 * (anchor1->y - anchor0->y); + float C = sq(anchor0->distance) - sq(anchor1->distance) - sq(anchor0->x) + sq(anchor1->x) - sq(anchor0->y) + sq(anchor1->y); + float D = 2 * (anchor2->x - anchor1->x); + float E = 2 * (anchor2->y - anchor1->y); + float F = sq(anchor1->distance) - sq(anchor2->distance) - sq(anchor1->x) + sq(anchor2->x) - sq(anchor1->y) + sq(anchor2->y); + + user_x = ((F * B) - (E * C)) / ((B * D) - (E * A)); + user_y = ((F * A) - (D * C)) / ((A * E) - (D * B)); + } +}; diff --git a/ESP32_UWB/4_UWB_Trilateration/config.h b/ESP32_UWB/4_UWB_Trilateration/config.h new file mode 100644 index 0000000..075cce6 --- /dev/null +++ b/ESP32_UWB/4_UWB_Trilateration/config.h @@ -0,0 +1,19 @@ +#include + +// UWB config +#define TAG +//#define ANCHOR +#define UWB_INDEX 1 + +#define FREQ_850K +//#define FREQ_6800K +#define UWB_TAG_COUNT 10 + +#define RESET 32 +#define IO_RXD2 18 +#define IO_TXD2 19 +#define I2C_SDA 4 +#define I2C_SCL 5 + +#define SERIAL_LOG Serial +#define SERIAL_AT mySerial2 \ No newline at end of file