Martin/MAUWB-platformiotest
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

Fix UWB init and range parsing

Browse source
This commit is contained in:
martin 2025-08-25 19:46:51 +02:00
commit 31a75c8783
3 changed files with 164 additions and 61 deletions
Download patch file Download diff file Expand all files Collapse all files

162
lib/UWBHelper/UWBHelper.cpp
View file

@ -199,67 +199,143 @@ String UWBHelper::sendCommand(String command, int timeout) {
// ===== RANGE DATA PARSING =====
bool UWBHelper::parseRangeData(String data, DeviceData devices[], int maxDevices) {
bool UWBHelper::parseRangeData(String data, DeviceData devices[], int maxDevices, bool isAnchor) {
if (!data.startsWith("AT+RANGE=")) {
return false;
}
// Parse tid (Tag ID)
// Parse tid (Tag ID) - this is who's ranging, not who we're ranging to
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
// Parse range data - array of distances to each anchor
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);
// Parse first non-zero distance
int commaIdx = 0;
float distance = 0.0;
for (int i = 0; i < 8; i++) {
int nextComma = rangeData.indexOf(',', commaIdx);
if (nextComma == -1) nextComma = rangeData.length();
float dist = rangeData.substring(commaIdx, nextComma).toFloat();
if (dist > 0) {
distance = dist / 100.0; // Convert cm to meters
break;
}
commaIdx = nextComma + 1;
if (commaIdx >= rangeData.length()) break;
}
// Parse RSSI data
// Parse RSSI data - array of RSSI values from each anchor
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();
}
if (rssiIndex == -1) return false;
int rssiStart = rssiIndex + 6;
int rssiEnd = data.indexOf(')', rssiStart);
if (rssiEnd == -1) return false;
String rssiData = data.substring(rssiStart, rssiEnd);
// Parse anchor IDs - which anchors are active
int ancidIndex = data.indexOf("ancid:(");
if (ancidIndex == -1) return false;
int ancidStart = ancidIndex + 7;
int ancidEnd = data.indexOf(')', ancidStart);
if (ancidEnd == -1) return false;
String ancidData = data.substring(ancidStart, ancidEnd);
// Parse arrays and match anchor IDs with their distances and RSSI
float ranges[8];
float rssiValues[8];
int anchorIds[8];
// Parse range values
int startIdx = 0;
for (int i = 0; i < 8; i++) {
int commaIdx = rangeData.indexOf(',', startIdx);
if (commaIdx == -1) commaIdx = rangeData.length();
ranges[i] = rangeData.substring(startIdx, commaIdx).toFloat() / 100.0; // Convert cm to m
startIdx = commaIdx + 1;
if (startIdx >= rangeData.length()) break;
}
// 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;
// Parse RSSI values
startIdx = 0;
for (int i = 0; i < 8; i++) {
int commaIdx = rssiData.indexOf(',', startIdx);
if (commaIdx == -1) commaIdx = rssiData.length();
rssiValues[i] = rssiData.substring(startIdx, commaIdx).toFloat();
startIdx = commaIdx + 1;
if (startIdx >= rssiData.length()) break;
}
// Parse anchor IDs
startIdx = 0;
for (int i = 0; i < 8; i++) {
int commaIdx = ancidData.indexOf(',', startIdx);
if (commaIdx == -1) commaIdx = ancidData.length();
anchorIds[i] = ancidData.substring(startIdx, commaIdx).toInt();
startIdx = commaIdx + 1;
if (startIdx >= ancidData.length()) break;
}
// Parse tid (Tag ID) to know which device is reporting ranges
int commaPos = data.indexOf(',', tidIndex);
if (commaPos == -1) return false;
int reportingDeviceId = data.substring(tidIndex + 4, commaPos).toInt();
if (isAnchor) {
// ANCHOR BEHAVIOR: When tag reports (tid >= 1), only store the tag itself
if (reportingDeviceId >= 1) {
// Find first non-zero distance and RSSI for this reporting tag
float tagDistance = 0.0;
float tagRssi = 0.0;
for (int i = 0; i < 8; i++) {
if (ranges[i] > 0) {
tagDistance = ranges[i];
tagRssi = rssiValues[i];
break;
}
}
if (tagDistance > 0) {
// Find or create slot for this tag
int deviceSlot = -1;
for (int j = 0; j < maxDevices; j++) {
if (devices[j].deviceId == reportingDeviceId || !devices[j].active) {
deviceSlot = j;
break;
}
}
if (deviceSlot >= 0) {
devices[deviceSlot].deviceId = reportingDeviceId;
devices[deviceSlot].distance = tagDistance;
devices[deviceSlot].rssi = tagRssi;
devices[deviceSlot].lastUpdate = millis();
devices[deviceSlot].active = true;
return true;
}
}
}
} else {
// TAG BEHAVIOR: When tag reports (tid >= 1), store all anchors in the range data
if (reportingDeviceId >= 1) {
bool dataUpdated = false;
for (int i = 0; i < 8; i++) {
if (anchorIds[i] >= 0 && ranges[i] > 0) {
// Find or create slot for this anchor
int deviceSlot = -1;
for (int j = 0; j < maxDevices; j++) {
if (devices[j].deviceId == anchorIds[i] || !devices[j].active) {
deviceSlot = j;
break;
}
}
if (deviceSlot >= 0) {
devices[deviceSlot].deviceId = anchorIds[i];
devices[deviceSlot].distance = ranges[i];
devices[deviceSlot].rssi = rssiValues[i];
devices[deviceSlot].lastUpdate = millis();
devices[deviceSlot].active = true;
dataUpdated = true;
}
}
}
return dataUpdated;
}
}

2
lib/UWBHelper/UWBHelper.h
View file

@ -63,7 +63,7 @@ public:
String getReporting(); // AT+GETRPT?
// Range Command (3.15)
bool parseRangeData(String data, DeviceData devices[], int maxDevices);
bool parseRangeData(String data, DeviceData devices[], int maxDevices, bool isAnchor = true);
bool parseDetailedRangeData(String data, RangeResult* result);
// Sleep Command (3.16)

65
src/main.cpp
View file

@ -51,30 +51,57 @@ void setup() {
showStartupScreen();
// Initialize UWB
// Initialize UWB serial
uwbSerial.begin(115200, SERIAL_8N1, IO_RXD2, IO_TXD2);
// Reset UWB module
// Hardware reset UWB module (critical for reliable operation)
pinMode(RESET_PIN, OUTPUT);
digitalWrite(RESET_PIN, HIGH);
delay(1000);
// Initialize UWB
if (!uwb.begin()) {
Serial.println("UWB initialization failed!");
return;
}
Serial.println("Configuring UWB module...");
// Configure device based on type
Serial.printf("Configuring as %s...\n", deviceConfig.deviceName);
uwb.configureDevice(deviceConfig.deviceId, deviceConfig.isAnchor);
uwb.setCapacity(deviceConfig.capacityTags, deviceConfig.capacityTimeSlot, deviceConfig.capacityExtMode);
uwb.setNetwork(NETWORK_ID);
uwb.enableReporting(true);
uwb.saveConfiguration();
delay(1000);
uwb.restartDevice();
delay(2000);
// Complete UWB setup sequence following AT command protocol
uwb.sendCommand("AT?", 2000);
delay(500);
uwb.sendCommand("AT+RESTORE", 5000);
delay(2000); // Wait for factory reset to complete
// Configure device role and parameters
String configCmd = "AT+SETCFG=" + String(deviceConfig.deviceId) + ",";
configCmd += deviceConfig.isAnchor ? "1" : "0"; // 1=Anchor, 0=Tag
configCmd += ",1,1"; // 6.8Mbps mode, range filtering enabled
uwb.sendCommand(configCmd, 2000);
delay(500);
// Set device capacity
String capacityCmd = "AT+SETCAP=" + String(deviceConfig.capacityTags) + ",";
capacityCmd += String(deviceConfig.capacityTimeSlot) + ",";
capacityCmd += String(deviceConfig.capacityExtMode);
uwb.sendCommand(capacityCmd, 2000);
delay(500);
// Enable automatic reporting and set network
uwb.sendCommand("AT+SETRPT=1", 2000);
delay(500);
uwb.sendCommand("AT+SETPAN=" + String(NETWORK_ID), 2000);
delay(500);
// Save configuration and restart
uwb.sendCommand("AT+SAVE", 2000);
delay(1000); // Wait for save to complete
uwb.sendCommand("AT+RESTART", 2000);
delay(3000); // Wait longer for restart to complete
// Verify configuration
Serial.println("Verifying configuration...");
String configResponse = uwb.sendCommand("AT+GETCFG?", 2000);
Serial.printf("Configuration verification: %s\n", configResponse.c_str());
// Additional verification commands
uwb.sendCommand("AT+GETPAN?", 2000);
uwb.sendCommand("AT+GETRPT?", 2000);
// Initialize device data
#if DEVICE_TYPE == DEVICE_ANCHOR
@ -147,7 +174,7 @@ void processUWBData() {
Serial.println("Received: " + response);
#if DEVICE_TYPE == DEVICE_ANCHOR
if (uwb.parseRangeData(response, tags, UWB_TAG_COUNT)) {
if (uwb.parseRangeData(response, tags, UWB_TAG_COUNT, true)) {
// Successfully parsed data - log active tags
for (int i = 0; i < UWB_TAG_COUNT; i++) {
if (tags[i].active && (millis() - tags[i].lastUpdate < 1000)) {
@ -157,7 +184,7 @@ void processUWBData() {
}
}
#elif DEVICE_TYPE == DEVICE_TAG
if (uwb.parseRangeData(response, anchors, MAX_ANCHORS)) {
if (uwb.parseRangeData(response, anchors, MAX_ANCHORS, false)) {
// Successfully parsed data - log active anchors
for (int i = 0; i < MAX_ANCHORS; i++) {
if (anchors[i].active && (millis() - anchors[i].lastUpdate < 1000)) {