Skip to content

Commit 244fbc3

Browse files
MatteoCnda1MatteoCnda1
committed
feat(ble): Add BLE trilateration indoor positioning example.
1 parent 7baa141 commit 244fbc3

1 file changed

Lines changed: 282 additions & 0 deletions

File tree

‎lib/ble/examples/BleTrilateration/BleTrilateration.ino‎

Lines changed: 282 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,282 @@
1+
// SPDX-License-Identifier: GPL-3.0-or-later
2+
//
3+
// BleTrilateration — estimate 2D position using RSSI trilateration.
4+
//
5+
// 3 STeaMi boards run BleBeacon at known positions (cm).
6+
// This mobile board scans all 3, converts RSSI to distance using the
7+
// log-distance path loss model, then trilaterates a 2D position.
8+
//
9+
// Beacon coordinates (cm, measured on site):
10+
// Beacon_M1 = ( 0, 0)
11+
// Beacon_M2 = (420, 0)
12+
// Beacon_M3 = (330, 278)
13+
//
14+
// Calibration (nRF Connect, iPhone 17 Pro Max):
15+
// RSSI_ref at 1m: M1=-75, M2=-75, M3=-80
16+
// Path loss n: M1=3.99, M2=4.32, M3=2.66
17+
//
18+
// Open the serial monitor at 115200 baud.
19+
20+
#include <Arduino.h>
21+
#include <STM32duinoBLE.h>
22+
#include <math.h>
23+
24+
// === Beacon configuration ===
25+
static const int BEACON_COUNT = 3;
26+
27+
static const char* BEACON_NAMES[BEACON_COUNT] = {
28+
"Beacon_M1",
29+
"Beacon_M2",
30+
"Beacon_M3",
31+
};
32+
33+
// Beacon positions in cm
34+
static const float BEACON_X[BEACON_COUNT] = {0.0f, 420.0f, 330.0f};
35+
static const float BEACON_Y[BEACON_COUNT] = {0.0f, 0.0f, 278.0f};
36+
37+
// Path loss calibration (measured on site)
38+
static const float RSSI_REF[BEACON_COUNT] = {-75.0f, -75.0f, -80.0f};
39+
static const float PATH_LOSS_N[BEACON_COUNT] = {3.99f, 4.32f, 2.66f};
40+
41+
// === Trilateration validity ===
42+
static const float MAX_DIST_CM = 430.0f;
43+
static const float MAX_VALID_CM = 550.0f;
44+
static const float CENTROID_X = (0.0f + 420.0f + 330.0f) / 3.0f;
45+
static const float CENTROID_Y = (0.0f + 0.0f + 278.0f) / 3.0f;
46+
47+
// === RSSI smoothing ===
48+
static const int RSSI_SAMPLES = 8;
49+
static int rssiHistory[BEACON_COUNT][RSSI_SAMPLES] = {{0}};
50+
static int rssiIndex[BEACON_COUNT] = {0};
51+
static int rssiCount[BEACON_COUNT] = {0};
52+
static int currentRssi[BEACON_COUNT];
53+
static bool beaconSeen[BEACON_COUNT] = {false};
54+
55+
// === Position filtering ===
56+
static const float ALPHA = 0.15f;
57+
static const float MIN_MOVE_CM = 15.0f;
58+
static float filteredX = -1.0f;
59+
static float filteredY = -1.0f;
60+
61+
// =============================================================================
62+
// === HELPERS =================================================================
63+
// =============================================================================
64+
65+
int beaconIndex(const String& name) {
66+
for (int i = 0; i < BEACON_COUNT; i++) {
67+
if (name == BEACON_NAMES[i])
68+
return i;
69+
}
70+
return -1;
71+
}
72+
73+
void smoothRssi(int idx, int newRssi) {
74+
rssiHistory[idx][rssiIndex[idx]] = newRssi;
75+
rssiIndex[idx] = (rssiIndex[idx] + 1) % RSSI_SAMPLES;
76+
if (rssiCount[idx] < RSSI_SAMPLES)
77+
rssiCount[idx]++;
78+
int sum = 0;
79+
for (int i = 0; i < rssiCount[idx]; i++)
80+
sum += rssiHistory[idx][i];
81+
currentRssi[idx] = sum / rssiCount[idx];
82+
}
83+
84+
float rssiToDistance(int rssi, int idx) {
85+
float d = pow(10.0f, (RSSI_REF[idx] - rssi) / (10.0f * PATH_LOSS_N[idx]));
86+
d *= 100.0f; // metres -> cm
87+
if (d > MAX_DIST_CM)
88+
d = MAX_DIST_CM;
89+
return d;
90+
}
91+
92+
bool trilaterate(float r0, float r1, float r2, float& outX, float& outY) {
93+
float x1 = BEACON_X[0], y1 = BEACON_Y[0];
94+
float x2 = BEACON_X[1], y2 = BEACON_Y[1];
95+
float x3 = BEACON_X[2], y3 = BEACON_Y[2];
96+
97+
float A = 2.0f * (x2 - x1);
98+
float B = 2.0f * (y2 - y1);
99+
float C = r0 * r0 - r1 * r1 - x1 * x1 + x2 * x2 - y1 * y1 + y2 * y2;
100+
float D = 2.0f * (x3 - x1);
101+
float E = 2.0f * (y3 - y1);
102+
float F = r0 * r0 - r2 * r2 - x1 * x1 + x3 * x3 - y1 * y1 + y3 * y3;
103+
104+
float denom = A * E - B * D;
105+
if (fabsf(denom) < 1e-6f)
106+
return false;
107+
108+
outX = (C * E - F * B) / denom;
109+
outY = (A * F - D * C) / denom;
110+
111+
// Reject if too far from centroid
112+
float dx = outX - CENTROID_X;
113+
float dy = outY - CENTROID_Y;
114+
if (sqrtf(dx * dx + dy * dy) > MAX_VALID_CM)
115+
return false;
116+
117+
return true;
118+
}
119+
120+
void applyFilter(float newX, float newY) {
121+
if (filteredX < 0.0f) {
122+
filteredX = newX;
123+
filteredY = newY;
124+
return;
125+
}
126+
float fx = ALPHA * newX + (1.0f - ALPHA) * filteredX;
127+
float fy = ALPHA * newY + (1.0f - ALPHA) * filteredY;
128+
129+
float dx = fx - filteredX;
130+
float dy = fy - filteredY;
131+
if (sqrtf(dx * dx + dy * dy) < MIN_MOVE_CM)
132+
return;
133+
134+
filteredX = fx;
135+
filteredY = fy;
136+
}
137+
138+
void printMap() {
139+
// ASCII 2D map — 42x28 chars, 1 char = 10cm
140+
const int COLS = 42;
141+
const int ROWS = 28;
142+
char map[ROWS][COLS + 1];
143+
144+
// Fill with spaces
145+
for (int r = 0; r < ROWS; r++) {
146+
for (int c = 0; c < COLS; c++)
147+
map[r][c] = '.';
148+
map[r][COLS] = '\0';
149+
}
150+
151+
// Place beacons
152+
for (int i = 0; i < BEACON_COUNT; i++) {
153+
int c = (int)(BEACON_X[i] / 10.0f);
154+
int r = ROWS - 1 - (int)(BEACON_Y[i] / 10.0f);
155+
if (c >= 0 && c < COLS && r >= 0 && r < ROWS) {
156+
map[r][c] = '0' + i + 1; // '1', '2', '3'
157+
}
158+
}
159+
160+
// Place estimated position
161+
if (filteredX >= 0.0f) {
162+
int c = (int)(filteredX / 10.0f);
163+
int r = ROWS - 1 - (int)(filteredY / 10.0f);
164+
c = max(0, min(COLS - 1, c));
165+
r = max(0, min(ROWS - 1, r));
166+
map[r][c] = 'X';
167+
}
168+
169+
// Print map
170+
Serial.println();
171+
Serial.println("=== Position Map (1 char = 10cm) ===");
172+
for (int r = 0; r < ROWS; r++) {
173+
Serial.println(map[r]);
174+
}
175+
Serial.println("1=M1 2=M2 3=M3 X=You");
176+
}
177+
178+
// =============================================================================
179+
// === SETUP / LOOP ============================================================
180+
// =============================================================================
181+
182+
void setup() {
183+
Serial.begin(115200);
184+
while (!Serial && millis() < 2000)
185+
;
186+
187+
Serial.println("BLE Trilateration Indoor Positioning");
188+
Serial.println("Beacon layout (cm):");
189+
for (int i = 0; i < BEACON_COUNT; i++) {
190+
Serial.print(" ");
191+
Serial.print(BEACON_NAMES[i]);
192+
Serial.print(" = (");
193+
Serial.print((int)BEACON_X[i]);
194+
Serial.print(", ");
195+
Serial.print((int)BEACON_Y[i]);
196+
Serial.println(")");
197+
}
198+
Serial.println();
199+
200+
if (!BLE.begin()) {
201+
Serial.println("BLE init failed!");
202+
while (true)
203+
;
204+
}
205+
206+
BLE.scan(true);
207+
Serial.println("Scanning...");
208+
}
209+
210+
void loop() {
211+
// Update RSSI from scan
212+
BLEDevice device = BLE.available();
213+
if (device) {
214+
int idx = beaconIndex(device.localName());
215+
if (idx >= 0) {
216+
smoothRssi(idx, device.rssi());
217+
beaconSeen[idx] = true;
218+
}
219+
}
220+
221+
BLE.poll();
222+
223+
// Update position every 500ms
224+
static unsigned long lastUpdate = 0;
225+
if (millis() - lastUpdate < 500)
226+
return;
227+
lastUpdate = millis();
228+
229+
// Check all 3 beacons are seen
230+
int seen = 0;
231+
for (int i = 0; i < BEACON_COUNT; i++) {
232+
if (beaconSeen[i])
233+
seen++;
234+
}
235+
236+
if (seen < 3) {
237+
Serial.print("Waiting for beacons (");
238+
Serial.print(seen);
239+
Serial.println("/3 seen)...");
240+
return;
241+
}
242+
243+
// Compute distances
244+
float d[BEACON_COUNT];
245+
for (int i = 0; i < BEACON_COUNT; i++) {
246+
d[i] = rssiToDistance(currentRssi[i], i);
247+
}
248+
249+
// Print distances
250+
Serial.print("Distances: ");
251+
for (int i = 0; i < BEACON_COUNT; i++) {
252+
Serial.print(BEACON_NAMES[i]);
253+
Serial.print("=");
254+
Serial.print((int)d[i]);
255+
Serial.print("cm ");
256+
}
257+
Serial.println();
258+
259+
// Trilaterate
260+
float rawX, rawY;
261+
if (!trilaterate(d[0], d[1], d[2], rawX, rawY)) {
262+
Serial.println("Trilateration failed — aberrant result rejected.");
263+
return;
264+
}
265+
266+
// Filter
267+
applyFilter(rawX, rawY);
268+
269+
// Print position
270+
Serial.print("Position: X=");
271+
Serial.print((int)filteredX);
272+
Serial.print("cm Y=");
273+
Serial.print((int)filteredY);
274+
Serial.println("cm");
275+
276+
// Print ASCII map every 2s
277+
static unsigned long lastMap = 0;
278+
if (millis() - lastMap > 2000) {
279+
lastMap = millis();
280+
printMap();
281+
}
282+
}

0 commit comments

Comments
 (0)