Initial commit: ESP32-S3 UWB positioning system
- Added anchor and tag implementations for MaUWB modules - Configured for 6.8Mbps communication with range filtering - Support for multiple tags (tag/tag2 environments) - OLED display integration for real-time measurements - Simplified code without sleep mode and OTA functionality - Complete UWBHelper library for AT command interface
This commit is contained in:
martin
commit
a89215b7ff
10 changed files with 806 additions and 0 deletions
198
lib/UWBHelper/UWBHelper.cpp
Normal file
198
lib/UWBHelper/UWBHelper.cpp
Normal file
|
|
@ -0,0 +1,198 @@
|
|||
#include "UWBHelper.h"
|
||||
|
||||
UWBHelper::UWBHelper(HardwareSerial* serial, int reset) {
|
||||
uwbSerial = serial;
|
||||
resetPin = reset;
|
||||
}
|
||||
|
||||
bool UWBHelper::begin() {
|
||||
pinMode(resetPin, OUTPUT);
|
||||
digitalWrite(resetPin, HIGH);
|
||||
|
||||
uwbSerial->begin(115200);
|
||||
delay(1000);
|
||||
|
||||
// Test communication
|
||||
String response = sendCommand("AT?", 2000);
|
||||
return isResponseOK(response);
|
||||
}
|
||||
|
||||
void UWBHelper::reset() {
|
||||
digitalWrite(resetPin, LOW);
|
||||
delay(100);
|
||||
digitalWrite(resetPin, HIGH);
|
||||
delay(1000);
|
||||
}
|
||||
|
||||
bool UWBHelper::configureDevice(int deviceId, bool isAnchor, int dataRate, int rangeFilter) {
|
||||
String cmd = "AT+SETCFG=" + String(deviceId) + "," +
|
||||
String(isAnchor ? 1 : 0) + "," +
|
||||
String(dataRate) + "," +
|
||||
String(rangeFilter);
|
||||
|
||||
String response = sendCommand(cmd, 2000);
|
||||
return isResponseOK(response);
|
||||
}
|
||||
|
||||
bool UWBHelper::setCapacity(int tagCount, int timeSlot, int extMode) {
|
||||
String cmd = "AT+SETCAP=" + String(tagCount) + "," +
|
||||
String(timeSlot) + "," +
|
||||
String(extMode);
|
||||
|
||||
String response = sendCommand(cmd, 2000);
|
||||
return isResponseOK(response);
|
||||
}
|
||||
|
||||
bool UWBHelper::setNetwork(int networkId) {
|
||||
String cmd = "AT+SETPAN=" + String(networkId);
|
||||
String response = sendCommand(cmd, 2000);
|
||||
return isResponseOK(response);
|
||||
}
|
||||
|
||||
bool UWBHelper::enableReporting(bool enable) {
|
||||
String cmd = "AT+SETRPT=" + String(enable ? 1 : 0);
|
||||
String response = sendCommand(cmd, 2000);
|
||||
return isResponseOK(response);
|
||||
}
|
||||
|
||||
bool UWBHelper::saveConfiguration() {
|
||||
String response = sendCommand("AT+SAVE", 2000);
|
||||
return isResponseOK(response);
|
||||
}
|
||||
|
||||
bool UWBHelper::restartDevice() {
|
||||
String response = sendCommand("AT+RESTART", 2000);
|
||||
return isResponseOK(response);
|
||||
}
|
||||
|
||||
String UWBHelper::sendCommand(String command, int timeout) {
|
||||
String response = "";
|
||||
|
||||
Serial.println("Sending: " + command);
|
||||
uwbSerial->println(command);
|
||||
|
||||
unsigned long startTime = millis();
|
||||
while ((millis() - startTime) < timeout) {
|
||||
while (uwbSerial->available()) {
|
||||
char c = uwbSerial->read();
|
||||
response += c;
|
||||
}
|
||||
}
|
||||
|
||||
if (response.length() > 0) {
|
||||
Serial.println("Response: " + response);
|
||||
}
|
||||
return response;
|
||||
}
|
||||
|
||||
bool UWBHelper::parseRangeData(String data, DeviceData devices[], int maxDevices) {
|
||||
if (!data.startsWith("AT+RANGE=")) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// Parse tid (Tag ID)
|
||||
int tidIndex = data.indexOf("tid:");
|
||||
if (tidIndex == -1) return false;
|
||||
|
||||
int commaPos = data.indexOf(',', tidIndex);
|
||||
if (commaPos == -1) return false;
|
||||
|
||||
int deviceId = data.substring(tidIndex + 4, commaPos).toInt();
|
||||
|
||||
// Parse range data
|
||||
int rangeIndex = data.indexOf("range:(");
|
||||
if (rangeIndex == -1) return false;
|
||||
|
||||
int rangeStart = rangeIndex + 7;
|
||||
int rangeEnd = data.indexOf(')', rangeStart);
|
||||
if (rangeEnd == -1) return false;
|
||||
|
||||
String rangeData = data.substring(rangeStart, rangeEnd);
|
||||
float distance = rangeData.toFloat() / 100.0; // Convert cm to meters
|
||||
|
||||
// Parse RSSI data
|
||||
int rssiIndex = data.indexOf("rssi:(");
|
||||
float rssi = 0.0;
|
||||
if (rssiIndex != -1) {
|
||||
int rssiStart = rssiIndex + 6;
|
||||
int rssiComma = data.indexOf(',', rssiStart);
|
||||
if (rssiComma == -1) rssiComma = data.indexOf(')', rssiStart);
|
||||
if (rssiComma != -1) {
|
||||
rssi = data.substring(rssiStart, rssiComma).toFloat();
|
||||
}
|
||||
}
|
||||
|
||||
// Update device data
|
||||
for (int i = 0; i < maxDevices; i++) {
|
||||
if (devices[i].deviceId == deviceId || !devices[i].active) {
|
||||
devices[i].deviceId = deviceId;
|
||||
devices[i].distance = distance;
|
||||
devices[i].rssi = rssi;
|
||||
devices[i].lastUpdate = millis();
|
||||
devices[i].active = true;
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
String UWBHelper::getVersion() {
|
||||
return sendCommand("AT+GETVER?", 2000);
|
||||
}
|
||||
|
||||
bool UWBHelper::isResponseOK(String response) {
|
||||
return response.indexOf("OK") >= 0;
|
||||
}
|
||||
|
||||
void UWBHelper::printDiagnostics() {
|
||||
Serial.println("=== UWB Diagnostics ===");
|
||||
Serial.println("Version: " + getVersion());
|
||||
Serial.println("Config: " + sendCommand("AT+GETCFG?", 2000));
|
||||
Serial.println("Capacity: " + sendCommand("AT+GETCAP?", 2000));
|
||||
Serial.println("Power: " + sendCommand("AT+GETPOW?", 2000));
|
||||
}
|
||||
|
||||
// DistanceFilter Implementation
|
||||
DistanceFilter::DistanceFilter() : index(0), filled(false) {
|
||||
for (int i = 0; i < FILTER_SIZE; i++) readings[i] = 0;
|
||||
}
|
||||
|
||||
float DistanceFilter::addReading(float distance) {
|
||||
readings[index] = distance;
|
||||
index = (index + 1) % FILTER_SIZE;
|
||||
if (index == 0) filled = true;
|
||||
|
||||
return getFilteredValue();
|
||||
}
|
||||
|
||||
float DistanceFilter::getFilteredValue() {
|
||||
if (!filled && index == 0) return readings[0];
|
||||
|
||||
// Median filter
|
||||
float sorted[FILTER_SIZE];
|
||||
int count = filled ? FILTER_SIZE : index;
|
||||
|
||||
for (int i = 0; i < count; i++) {
|
||||
sorted[i] = readings[i];
|
||||
}
|
||||
|
||||
// Simple bubble sort
|
||||
for (int i = 0; i < count - 1; i++) {
|
||||
for (int j = 0; j < count - i - 1; j++) {
|
||||
if (sorted[j] > sorted[j + 1]) {
|
||||
float temp = sorted[j];
|
||||
sorted[j] = sorted[j + 1];
|
||||
sorted[j + 1] = temp;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return sorted[count / 2]; // Return median
|
||||
}
|
||||
|
||||
void DistanceFilter::reset() {
|
||||
index = 0;
|
||||
filled = false;
|
||||
for (int i = 0; i < FILTER_SIZE; i++) readings[i] = 0;
|
||||
}
|
||||
60
lib/UWBHelper/UWBHelper.h
Normal file
60
lib/UWBHelper/UWBHelper.h
Normal file
|
|
@ -0,0 +1,60 @@
|
|||
#ifndef UWBHELPER_H
|
||||
#define UWBHELPER_H
|
||||
|
||||
#include <Arduino.h>
|
||||
#include <HardwareSerial.h>
|
||||
|
||||
struct DeviceData {
|
||||
int deviceId;
|
||||
float distance;
|
||||
float rssi;
|
||||
unsigned long lastUpdate;
|
||||
bool active;
|
||||
};
|
||||
|
||||
class UWBHelper {
|
||||
private:
|
||||
HardwareSerial* uwbSerial;
|
||||
int resetPin;
|
||||
|
||||
public:
|
||||
UWBHelper(HardwareSerial* serial, int reset);
|
||||
|
||||
// Initialization
|
||||
bool begin();
|
||||
void reset();
|
||||
|
||||
// Configuration
|
||||
bool configureDevice(int deviceId, bool isAnchor, int dataRate = 1, int rangeFilter = 1);
|
||||
bool setCapacity(int tagCount, int timeSlot = 10, int extMode = 1);
|
||||
bool setNetwork(int networkId);
|
||||
bool enableReporting(bool enable);
|
||||
bool saveConfiguration();
|
||||
bool restartDevice();
|
||||
|
||||
// Communication
|
||||
String sendCommand(String command, int timeout = 2000);
|
||||
bool parseRangeData(String data, DeviceData devices[], int maxDevices);
|
||||
|
||||
// Utility
|
||||
String getVersion();
|
||||
bool isResponseOK(String response);
|
||||
void printDiagnostics();
|
||||
};
|
||||
|
||||
// Data filtering class
|
||||
class DistanceFilter {
|
||||
private:
|
||||
static const int FILTER_SIZE = 5;
|
||||
float readings[FILTER_SIZE];
|
||||
int index;
|
||||
bool filled;
|
||||
|
||||
public:
|
||||
DistanceFilter();
|
||||
float addReading(float distance);
|
||||
float getFilteredValue();
|
||||
void reset();
|
||||
};
|
||||
|
||||
#endif // UWBHELPER_H
|
||||
Loading…
Add table
Add a link
Reference in a new issue