diff --git a/lib/UWBHelper/UWBHelper.cpp b/lib/UWBHelper/UWBHelper.cpp index e4e7fbe..1bf87d9 100644 --- a/lib/UWBHelper/UWBHelper.cpp +++ b/lib/UWBHelper/UWBHelper.cpp @@ -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; } } diff --git a/lib/UWBHelper/UWBHelper.h b/lib/UWBHelper/UWBHelper.h index 8de2f4d..e5afb3c 100644 --- a/lib/UWBHelper/UWBHelper.h +++ b/lib/UWBHelper/UWBHelper.h @@ -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) diff --git a/src/main.cpp b/src/main.cpp index b41b351..b2e8370 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -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)) {