Skip to content

Commit 49732f7

Browse files
author
Eric
committed
[ESP32 + UWB + IMU | Indoor Position & Rotation + Unity Visualization]
1 parent e96fc26 commit 49732f7

146 files changed

Lines changed: 40798 additions & 0 deletions

File tree

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

‎README.md‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -8,6 +8,12 @@ Implement all Unity Project Ideas
88

99
* [Youtube Unity Projects](https://www.youtube.com/playlist?list=PLnq7JUnBumAzcQciRTvPZ3Mw_v7j8noOw)
1010

11+
<a href="https://youtu.be/fPuxcjHsfpc">
12+
<img width="600" src="https://github.com/0015/Unity_Projects/blob/master/misc/UWB_IMU_Position_Rotation.gif">
13+
</a>
14+
15+
* [[ESP32 + UWB + IMU | Indoor Position & Rotation + Unity Visualization][[Video]](https://youtu.be/fPuxcjHsfpc)[[Source Code]](https://github.com/0015/Unity_Projects/tree/master/UWB_Indoor_Position_Rotation_Visualization)
16+
---
1117
<a href="https://youtu.be/c8Pn7lS5Ppg">
1218
<img width="600" src="https://github.com/0015/Unity_Projects/blob/master/misc/UWB_Indoor_Position_Visualization.gif">
1319
</a>

‎UWB_Indoor_Position_Rotation_Visualization/ESP32_UWB_TAG_BNO055_ILI9488/ESP32_UWB_TAG_BNO055_ILI9488.ino‎

Lines changed: 242 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,242 @@
1+
/////////////////////////////////////////////////////////////////
2+
/*
3+
ESP32 + UWB + IMU | Indoor Position & Rotation + Unity Visualization
4+
For More Information: https://youtu.be/fPuxcjHsfpc
5+
Created by Eric N. (ThatProject)
6+
*/
7+
/////////////////////////////////////////////////////////////////
8+
///////////////////
9+
//ESP Version 1.0.6
10+
///////////////////
11+
#include <Wire.h>
12+
#include <Adafruit_Sensor.h>
13+
#include <Adafruit_BNO055.h>
14+
#include <SPI.h>
15+
#include <DW1000Ranging.h>
16+
#include <WiFi.h>
17+
#include <WiFiUdp.h>
18+
#include "MyDisplay.h"
19+
20+
#define I2C_SDA 17
21+
#define I2C_SCL 16
22+
23+
const uint8_t MY_PIN_SCK = 18;
24+
const uint8_t MY_PIN_MOSI = 23;
25+
const uint8_t MY_PIN_MISO = 19;
26+
const uint8_t MY_PIN_SS = 15;
27+
const uint8_t MY_PIN_RST = 2;
28+
const uint8_t MY_PIN_IRQ = 22;
29+
30+
const char *ssid = "ThatProject";
31+
const char *password = "California";
32+
const char *udpAddress = "192.168.0.2"; //My UDP Server IP
33+
const int udpPort = 8080;
34+
unsigned long udpTimer = 0;
35+
bool isWiFiConnected = false;
36+
37+
WiFiUDP udp;
38+
39+
TwoWire I2CBNO = TwoWire(0);
40+
Adafruit_BNO055 bno = Adafruit_BNO055(55, 0x29, &I2CBNO);
41+
42+
MyDisplay *display;
43+
44+
static String uwbDevice[2];
45+
static portMUX_TYPE mux = portMUX_INITIALIZER_UNLOCKED;
46+
static const uint16_t timer_divider = 80;
47+
static const uint64_t timer_max_count = 1000;
48+
static hw_timer_t *timer = NULL;
49+
static SemaphoreHandle_t bin_sem = NULL;
50+
uint32_t cp0_regs[18];
51+
void IRAM_ATTR onTimer() {
52+
uint32_t cp_state = xthal_get_cpenable();
53+
if (cp_state) {
54+
xthal_save_cp0(cp0_regs);
55+
} else {
56+
xthal_set_cpenable(1);
57+
}
58+
59+
portENTER_CRITICAL_ISR(&mux);
60+
DW1000Ranging.loop();
61+
portEXIT_CRITICAL_ISR(&mux);
62+
63+
BaseType_t task_woken = pdFALSE;
64+
xSemaphoreGiveFromISR(bin_sem, &task_woken);
65+
if (task_woken == pdTRUE) {
66+
portYIELD_FROM_ISR();
67+
}
68+
69+
if (cp_state) {
70+
xthal_restore_cp0(cp0_regs);
71+
} else {
72+
xthal_set_cpenable(0);
73+
}
74+
}
75+
76+
void setup() {
77+
Serial.begin(115200);
78+
initDisplay();
79+
80+
bin_sem = xSemaphoreCreateBinary();
81+
if (bin_sem == NULL) {
82+
Serial.println("Could not create semaphore");
83+
ESP.restart();
84+
}
85+
86+
udpTimer = 0;
87+
88+
xTaskCreatePinnedToCore(system_task,
89+
"Task System",
90+
2048,
91+
NULL,
92+
1,
93+
NULL,
94+
1);
95+
96+
connectToWiFi(ssid, password);
97+
}
98+
99+
void setTimerInterrupt() {
100+
timer = timerBegin(0, timer_divider, true);
101+
timerAttachInterrupt(timer, &onTimer, true);
102+
timerAlarmWrite(timer, timer_max_count, true);
103+
timerAlarmEnable(timer);
104+
}
105+
106+
void loop() {}
107+
108+
void system_task(void *parameters) {
109+
110+
I2CBNO.begin(I2C_SDA, I2C_SCL);
111+
112+
if (!bno.begin()) {
113+
Serial.println("No BNO055 detected");
114+
while (1) {
115+
vTaskDelay(10);
116+
}
117+
}
118+
119+
vTaskDelay(100);
120+
121+
uint8_t system, gyro, accel, mag = 0;
122+
display->imuLog("Calibration!", false);
123+
while (system != 3) {
124+
bno.getCalibration(&system, &gyro, &accel, &mag);
125+
vTaskDelay(100);
126+
}
127+
128+
display->imuLog("All Set!", true);
129+
display->setIsAllReady(true);
130+
131+
while (1) {
132+
if (xSemaphoreTake(bin_sem, portMAX_DELAY) == pdTRUE) {
133+
portENTER_CRITICAL(&mux);
134+
display->loop();
135+
display->uwbLog(uwbDevice[0] + " m", 0);
136+
display->uwbLog(uwbDevice[1] + " m", 1);
137+
portEXIT_CRITICAL(&mux);
138+
if (millis() - udpTimer >= 100) {
139+
udpTimer = millis();
140+
imu::Quaternion quat = bno.getQuat();
141+
String strData = String(quat.w(), 4) + ", " + String(quat.x(), 4) + ", " + String(quat.y(), 4) + ", " + String(quat.z(), 4);
142+
sendData(strData);
143+
}else{
144+
if(uwbDevice[0].length() > 1 && uwbDevice[1].length() >1){
145+
String strData = uwbDevice[0];
146+
strData += ", ";
147+
strData += uwbDevice[1];
148+
sendData(strData);
149+
}
150+
}
151+
}
152+
vTaskDelay(30);
153+
}
154+
}
155+
156+
void sendData(String strData){
157+
if (!isWiFiConnected) return;
158+
udp.beginPacket(udpAddress, udpPort);
159+
udp.write((uint8_t *)strData.c_str(), strlen(strData.c_str()));
160+
udp.endPacket();
161+
}
162+
163+
void initDisplay() {
164+
display = new MyDisplay();
165+
display->initTFT();
166+
display->systemLog("[System Up]", true);
167+
}
168+
169+
String getQuaternion() {
170+
imu::Quaternion quat = bno.getQuat();
171+
return String(quat.w(), 4) + ", " + String(quat.x(), 4) + ", " + String(quat.y(), 4) + ", " + String(quat.z(), 4);
172+
}
173+
174+
void initUWB() {
175+
SPI.begin(MY_PIN_SCK, MY_PIN_MISO, MY_PIN_MOSI);
176+
177+
DW1000Ranging.initCommunication(MY_PIN_RST, MY_PIN_SS, MY_PIN_IRQ);
178+
DW1000Ranging.attachNewRange(newRange);
179+
DW1000Ranging.attachNewDevice(newDevice);
180+
DW1000Ranging.attachInactiveDevice(inactiveDevice);
181+
182+
DW1000.enableDebounceClock();
183+
DW1000.enableLedBlinking();
184+
DW1000.setGPIOMode(MSGP0, LED_MODE);
185+
DW1000.setGPIOMode(MSGP1, LED_MODE);
186+
DW1000.setGPIOMode(MSGP2, LED_MODE);
187+
DW1000.setGPIOMode(MSGP3, LED_MODE);
188+
189+
DW1000Ranging.startAsTag("7D:00:22:EA:82:60:3B:9C", DW1000.MODE_LONGDATA_RANGE_LOWPOWER);
190+
setTimerInterrupt();
191+
}
192+
193+
void newRange() {
194+
float projectedRange = DW1000Ranging.getDistantDevice()->getRange() * 2 / 5;
195+
String shortName = String(DW1000Ranging.getDistantDevice()->getShortAddress(), HEX);
196+
uwbDevice[shortName == "aabb" ? 0 : 1] = String(projectedRange);
197+
}
198+
199+
void newDevice(DW1000Device *device) {
200+
String shortName = String(device->getShortAddress(), HEX);
201+
display->uwbStatus(shortName == "aabb", true);
202+
}
203+
204+
void inactiveDevice(DW1000Device *device) {
205+
String shortName = String(device->getShortAddress(), HEX);
206+
display->uwbStatus(shortName == "aabb", false);
207+
}
208+
209+
void WiFiEvent(WiFiEvent_t event) {
210+
switch (event) {
211+
case SYSTEM_EVENT_STA_GOT_IP:
212+
display->systemLog(WiFi.localIP().toString());
213+
display->systemLog("IP address:");
214+
display->systemLog("Connected!");
215+
display->systemLog("WiFi", true);
216+
217+
Serial.print("WiFi connected! IP address: ");
218+
Serial.println(WiFi.localIP());
219+
udp.begin(WiFi.localIP(), udpPort);
220+
221+
display->systemLog("UDP begins!", true);
222+
isWiFiConnected = true;
223+
224+
initUWB();
225+
break;
226+
case SYSTEM_EVENT_STA_DISCONNECTED:
227+
228+
isWiFiConnected = false;
229+
display->systemLog("Disconnected!");
230+
display->systemLog("WiFi", true);
231+
display->setIsAllReady(false);
232+
break;
233+
}
234+
}
235+
236+
void connectToWiFi(const char *ssid, const char *pwd) {
237+
WiFi.disconnect(true);
238+
WiFi.onEvent(WiFiEvent);
239+
WiFi.begin(ssid, pwd);
240+
display->systemLog("Connecting!");
241+
display->systemLog("WIFI", true);
242+
}

‎UWB_Indoor_Position_Rotation_Visualization/ESP32_UWB_TAG_BNO055_ILI9488/MyDisplay.cpp‎

Lines changed: 110 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,110 @@
1+
/////////////////////////////////////////////////////////////////
2+
/*
3+
ESP32 + UWB + IMU | Indoor Position & Rotation + Unity Visualization
4+
For More Information: https://youtu.be/fPuxcjHsfpc
5+
Created by Eric N. (ThatProject)
6+
*/
7+
/////////////////////////////////////////////////////////////////
8+
#include "MyDisplay.h"
9+
#include "bg_image.h"
10+
#include "pyramid_image_1.h"
11+
#include "pyramid_image_2.h"
12+
#include "pyramid_image_3.h"
13+
14+
MyDisplay::MyDisplay() {
15+
tft = new TFT_eSPI();
16+
sLog = new TFT_eSprite(tft);
17+
TFT_timer = 0;
18+
isAllReady = false;
19+
pyramidImageIdx = 0;
20+
}
21+
22+
MyDisplay::~MyDisplay() {
23+
delete tft;
24+
}
25+
26+
void MyDisplay::initTFT() {
27+
tft->init();
28+
tft->setRotation(1);
29+
tft->setFreeFont(&FreeSansBold9pt7b);
30+
tft->fillScreen(TFT_WHITE);
31+
tft->setTextColor(TFT_BLACK, TFT_WHITE);
32+
tft->setSwapBytes(true);
33+
tft->pushImage(0, 0, BGImageWidth, BGImageHeight, BGImage);
34+
35+
sLog->setColorDepth(1);
36+
sLog->createSprite(160, 260);
37+
sLog->fillSprite(TFT_WHITE);
38+
sLog->setScrollRect(0, 0, 160, 260, TFT_WHITE);
39+
sLog->setTextColor(TFT_BLACK);
40+
sLog->setTextDatum(TL_DATUM);
41+
}
42+
43+
void MyDisplay::loop() {
44+
if (isAllReady && millis() - TFT_timer >= 200) {
45+
TFT_timer = millis();
46+
playPyramidImages();
47+
}
48+
}
49+
50+
void MyDisplay::setIsAllReady(bool isOn){
51+
isAllReady = isOn;
52+
53+
if(!isOn){
54+
tft->fillRect(320, 60, 160, 220, TFT_WHITE);
55+
}
56+
}
57+
58+
void MyDisplay::playPyramidImages(){
59+
if(pyramidImageIdx > 2) pyramidImageIdx = 0;
60+
switch(pyramidImageIdx){
61+
case 0:
62+
tft->pushImage(340, 130, 120, 120, PyramidImage1);
63+
break;
64+
case 1:
65+
tft->pushImage(340, 130, 120, 120, PyramidImage2);
66+
break;
67+
case 2:
68+
tft->pushImage(340, 130, 120, 120, PyramidImage3);
69+
break;
70+
default:
71+
break;
72+
}
73+
++pyramidImageIdx;
74+
}
75+
76+
void MyDisplay::systemLog(String str, bool moreSpace){
77+
sLog->drawString(str, 2, 2, 4);
78+
sLog->pushSprite(0, 60);
79+
sLog->scroll(0, 20);
80+
81+
if(moreSpace){
82+
sLog->scroll(0, 18);
83+
}
84+
}
85+
86+
void MyDisplay::imuLog(String str, bool isSuccess){
87+
tft->setTextColor(TFT_BLACK, TFT_WHITE);
88+
tft->fillRect(330, 70, 10, 10, isSuccess? TFT_GREEN : TFT_RED);
89+
tft->setTextPadding(160);
90+
tft->drawString(str, 346, 70, 4);
91+
}
92+
93+
void MyDisplay::imuResetCountDown(String str){
94+
tft->setTextColor(TFT_RED, TFT_WHITE);
95+
tft->setTextPadding(20);
96+
tft->drawString(str, 374, 140, 8);
97+
}
98+
99+
void MyDisplay::uwbStatus(bool isRight, bool isOn){
100+
tft->fillRect(170, isRight ? 70 : 160, 10, 10, isOn ? TFT_GREEN : TFT_RED);
101+
tft->setTextColor(TFT_WHITE, TFT_BLACK);
102+
tft->setTextPadding(120);
103+
tft->drawString(isRight? "Right Anc" : "Left Anc", 190, isRight? 70 : 160, 4);
104+
}
105+
106+
void MyDisplay::uwbLog(String str, int anchorNum){
107+
tft->setTextColor(TFT_WHITE, TFT_BLACK);
108+
tft->setTextPadding(120);
109+
tft->drawString(str, 200, anchorNum==0 ? 100 : 190, 4);
110+
}

0 commit comments

Comments
 (0)