Skip to content

Commit 94ece79

Browse files
author
Eric
committed
UWB Trilateration with ESP32, DW3000 #LocalPositioning
1 parent 7fa42c9 commit 94ece79

File tree

3 files changed

+327
-0
lines changed

3 files changed

+327
-0
lines changed

‎ESP32_UWB/4_UWB_Trilateration/4_UWB_Trilateration.ino

+264
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,264 @@
1+
/////////////////////////////////////////////////////////////////
2+
/*
3+
UWB Trilateration with ESP32, DW3000 #LocalPositioning
4+
For More Information: https://youtu.be/NjHMXEOxNRE
5+
Created by Eric N. (ThatProject)
6+
*/
7+
/////////////////////////////////////////////////////////////////
8+
// Version Info
9+
// ESP32 v3.0.2
10+
// Hardware Info
11+
// https://github.com/Makerfabs/MaUWB_DW3000-with-STM32-AT-Command/
12+
13+
#include "config.h"
14+
#include "Trilateration.hpp"
15+
#include <Wire.h>
16+
#include <Adafruit_GFX.h>
17+
#include <Adafruit_SSD1306.h>
18+
19+
HardwareSerial SERIAL_AT(2);
20+
Adafruit_SSD1306 display(128, 64, &Wire, -1);
21+
22+
String response = "";
23+
24+
Anchor anchor0(-340, 240);
25+
Anchor anchor1(340, 240);
26+
Anchor anchor2(0, -240);
27+
28+
Trilateration trilateration(&anchor0, &anchor1, &anchor2);
29+
30+
void setup() {
31+
pinMode(RESET, OUTPUT);
32+
digitalWrite(RESET, HIGH);
33+
34+
SERIAL_LOG.begin(115200);
35+
36+
xTaskCreatePinnedToCore(Task_UWB,
37+
"Task_UWB",
38+
1024 * 20,
39+
NULL,
40+
tskIDLE_PRIORITY,
41+
NULL,
42+
0);
43+
}
44+
45+
void loop() {}
46+
47+
void drawScreen() {
48+
display.clearDisplay();
49+
display.setTextSize(1); // Normal 1:1 pixel scale
50+
display.setTextColor(SSD1306_WHITE); // Draw white text
51+
display.setCursor(0, 0); // Start at top-left corner
52+
display.println(F("UWB Trilateration"));
53+
display.setCursor(0, 10);
54+
55+
String freqText = "Freq: ";
56+
57+
#ifdef FREQ_850K
58+
freqText = freqText + "850k";
59+
#else
60+
freqText = freqText + "6.8M";
61+
#endif
62+
display.println(freqText);
63+
display.setTextSize(2);
64+
display.setCursor(0, 20);
65+
66+
String infoText = "";
67+
68+
#ifdef TAG
69+
infoText = infoText + "Tag: " + UWB_INDEX;
70+
#else
71+
infoText = infoText + "Anchor: " + UWB_INDEX;
72+
#endif
73+
display.println(infoText);
74+
75+
#ifdef TAG
76+
display.setTextSize(1);
77+
display.setCursor(0, 40);
78+
display.println("User Location: ");
79+
80+
#else
81+
display.setCursor(0, 50);
82+
display.setTextSize(1);
83+
display.println("......Updating......");
84+
#endif
85+
display.display();
86+
87+
delay(1000);
88+
}
89+
90+
String sendData(String command, const int timeout, boolean debug) {
91+
String response = "";
92+
93+
SERIAL_LOG.println(command);
94+
SERIAL_AT.println(command); // send the read character to the SERIAL_LOG
95+
96+
long int time = millis();
97+
98+
while ((time + timeout) > millis()) {
99+
while (SERIAL_AT.available()) {
100+
101+
// The esp has data so display its output to the serial window
102+
char c = SERIAL_AT.read(); // read the next character.
103+
response += c;
104+
}
105+
}
106+
return response;
107+
}
108+
109+
String config_cmd() {
110+
String temp = "AT+SETCFG=";
111+
112+
// Set device id
113+
temp = temp + UWB_INDEX;
114+
115+
// Set device role
116+
#ifdef TAG
117+
temp = temp + ",0";
118+
#endif
119+
#ifdef ANCHOR
120+
temp = temp + ",1";
121+
#endif
122+
123+
// Set frequence 850k or 6.8M
124+
#ifdef FREQ_850K
125+
temp = temp + ",0";
126+
#endif
127+
#ifdef FREQ_6800K
128+
temp = temp + ",1";
129+
#endif
130+
131+
// Set range filter
132+
temp = temp + ",1";
133+
134+
return temp;
135+
}
136+
137+
String cap_cmd() {
138+
String temp = "AT+SETCAP=";
139+
140+
// Set Tag capacity
141+
temp = temp + UWB_TAG_COUNT;
142+
143+
// Time of a single time slot
144+
#ifdef FREQ_850K
145+
temp = temp + ",15";
146+
#endif
147+
#ifdef FREQ_6800K
148+
temp = temp + ",10";
149+
#endif
150+
151+
return temp;
152+
}
153+
154+
155+
156+
void Task_UWB(void *pvParameters) {
157+
158+
SERIAL_AT.begin(115200, SERIAL_8N1, IO_RXD2, IO_TXD2);
159+
160+
SERIAL_AT.println("AT");
161+
Wire.begin(I2C_SDA, I2C_SCL);
162+
delay(1000);
163+
// SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
164+
if (!display.begin(SSD1306_SWITCHCAPVCC, 0x3C)) { // Address 0x3C for 128x32
165+
SERIAL_LOG.println(F("SSD1306 allocation failed"));
166+
for (;;)
167+
; // Don't proceed, loop forever
168+
}
169+
display.clearDisplay();
170+
171+
drawScreen();
172+
173+
sendData("AT?", 2000, 1);
174+
//sendData("AT+RESTORE", 5000, 1);
175+
176+
sendData(config_cmd(), 2000, 1);
177+
sendData(cap_cmd(), 2000, 1);
178+
179+
sendData("AT+SETRPT=1", 2000, 1);
180+
//sendData("AT+SAVE", 2000, 1);
181+
sendData("AT+RESTART", 2000, 1);
182+
183+
184+
while (1) {
185+
186+
#ifdef TAG
187+
tag_action();
188+
#else
189+
vTaskDelay(10);
190+
#endif
191+
}
192+
}
193+
194+
void tag_action() {
195+
while (SERIAL_LOG.available() > 0) {
196+
SERIAL_AT.write(SERIAL_LOG.read());
197+
yield();
198+
}
199+
while (SERIAL_AT.available() > 0) {
200+
char c = SERIAL_AT.read();
201+
202+
if (c == '\r')
203+
continue;
204+
else if (c == '\n' || c == '\r') {
205+
SERIAL_LOG.println(response);
206+
207+
float rangeValues[3];
208+
// Parse data, update distances, and calculate user location
209+
if (parseData(response, rangeValues)) {
210+
updateAnchorDistances(rangeValues);
211+
calculateUserLocation();
212+
}
213+
214+
response = "";
215+
vTaskDelay(10);
216+
} else
217+
response += c;
218+
}
219+
}
220+
221+
bool parseData(String dataString, float rangeValues[3]) {
222+
// Find the position of "range:("
223+
int rangeStart = dataString.indexOf("range:(");
224+
if (rangeStart == -1) return false; // Return false if "range:(" not found
225+
226+
// Extract the substring containing the range values
227+
int rangeEnd = dataString.indexOf(")", rangeStart);
228+
String rangePart = dataString.substring(rangeStart + 7, rangeEnd);
229+
230+
// Parse the range values
231+
char charArray[rangePart.length() + 1];
232+
rangePart.toCharArray(charArray, rangePart.length() + 1);
233+
char *token = strtok(charArray, ",");
234+
int valueIndex = 0;
235+
while (token != NULL && valueIndex < 3) {
236+
rangeValues[valueIndex] = atoi(token);
237+
valueIndex++;
238+
token = strtok(NULL, ",");
239+
}
240+
241+
return true;
242+
}
243+
244+
// Function to update the distances for the anchors
245+
void updateAnchorDistances(float rangeValues[3]) {
246+
anchor0.updateDistance(rangeValues[0]);
247+
anchor1.updateDistance(rangeValues[1]);
248+
anchor2.updateDistance(rangeValues[2]);
249+
}
250+
251+
// Function to perform trilateration and calculate user location
252+
void calculateUserLocation() {
253+
float user_x, user_y;
254+
trilateration.calcUserLocation(user_x, user_y);
255+
256+
char buffer[100];
257+
snprintf(buffer, sizeof(buffer), "x = %.2f\ny = %.2f", user_x, user_y);
258+
Serial.println(buffer);
259+
260+
display.fillRect(0, 48, 128, 16, SSD1306_BLACK);
261+
display.setCursor(0, 48);
262+
display.print(buffer);
263+
display.display();
264+
}

‎ESP32_UWB/4_UWB_Trilateration/Trilateration.hpp

+44
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,44 @@
1+
#include <stdio.h>
2+
3+
class Anchor {
4+
public:
5+
float x;
6+
float y;
7+
float distance;
8+
9+
Anchor(float x, float y) {
10+
this->x = x;
11+
this->y = y;
12+
this->distance = 0.0;
13+
}
14+
15+
void updateDistance(float distance) {
16+
this->distance = distance;
17+
}
18+
};
19+
20+
class Trilateration {
21+
private:
22+
Anchor *anchor0;
23+
Anchor *anchor1;
24+
Anchor *anchor2;
25+
26+
public:
27+
Trilateration(Anchor *a0, Anchor *a1, Anchor *a2) {
28+
anchor0 = a0;
29+
anchor1 = a1;
30+
anchor2 = a2;
31+
}
32+
33+
void calcUserLocation(float &user_x, float &user_y) {
34+
float A = 2 * (anchor1->x - anchor0->x);
35+
float B = 2 * (anchor1->y - anchor0->y);
36+
float C = sq(anchor0->distance) - sq(anchor1->distance) - sq(anchor0->x) + sq(anchor1->x) - sq(anchor0->y) + sq(anchor1->y);
37+
float D = 2 * (anchor2->x - anchor1->x);
38+
float E = 2 * (anchor2->y - anchor1->y);
39+
float F = sq(anchor1->distance) - sq(anchor2->distance) - sq(anchor1->x) + sq(anchor2->x) - sq(anchor1->y) + sq(anchor2->y);
40+
41+
user_x = ((F * B) - (E * C)) / ((B * D) - (E * A));
42+
user_y = ((F * A) - (D * C)) / ((A * E) - (D * B));
43+
}
44+
};

‎ESP32_UWB/4_UWB_Trilateration/config.h

+19
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,19 @@
1+
#include <Arduino.h>
2+
3+
// UWB config
4+
#define TAG
5+
//#define ANCHOR
6+
#define UWB_INDEX 1
7+
8+
#define FREQ_850K
9+
//#define FREQ_6800K
10+
#define UWB_TAG_COUNT 10
11+
12+
#define RESET 32
13+
#define IO_RXD2 18
14+
#define IO_TXD2 19
15+
#define I2C_SDA 4
16+
#define I2C_SCL 5
17+
18+
#define SERIAL_LOG Serial
19+
#define SERIAL_AT mySerial2

0 commit comments

Comments
 (0)