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

Implement proper UWB initialization and fix range data parsing

Browse source 
- Replace high-level UWBHelper calls with direct AT command sequence
- Add hardware reset on every startup for reliable operation
- Implement proper timing delays between AT commands
- Add configuration verification with AT+GETCFG
- Complete rewrite of parseRangeData to handle multiple devices correctly
- Fix anchor/tag behavior: anchors only show tags, tags show all anchors
- Add isAnchor parameter to parseRangeData for device-specific logic
- Maintain backward compatibility with default parameter values
This commit is contained in:
martin 2025-08-25 19:46:51 +02:00
commit 1cb8968b93
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;
}
}