UWB Trilateration with ESP32, DW3000 #LocalPositioning

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 <Wire.h>
+#include <Adafruit_GFX.h>
+#include <Adafruit_SSD1306.h>
+
+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();
+}

ESP32_UWB/4_UWB_Trilateration/Trilateration.hpp

@@ -0,0 +1,44 @@
+#include <stdio.h>
+
+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));
+  }
+};

ESP32_UWB/4_UWB_Trilateration/config.h

@@ -0,0 +1,19 @@
+#include <Arduino.h>
+
+// 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