Implement proper UWB initialization and fix range data parsing

- 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

lib/UWBHelper/UWBHelper.cpp

@@ -199,69 +199,145 @@ 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);
+    // Parse range data - array of distances to each anchor
-    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);
     
-    // Parse first non-zero distance
+    // Parse RSSI data - array of RSSI values from each anchor
-    int commaIdx = 0;
+    int rssiIndex = data.indexOf("rssi:(");
-    float distance = 0.0;
+    if (rssiIndex == -1) return false;
-    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();
+    int rssiStart = rssiIndex + 6;
-        if (dist > 0) {
+    int rssiEnd = data.indexOf(')', rssiStart);
-            distance = dist / 100.0; // Convert cm to meters
+    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;
+    }
+    
+    // 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;
                 }
-        commaIdx = nextComma + 1;
-        if (commaIdx >= rangeData.length()) break;
             }
             
-    // Parse RSSI data
+            if (tagDistance > 0) {
-    int rssiIndex = data.indexOf("rssi:(");
+                // Find or create slot for this tag
-    float rssi = 0.0;
+                int deviceSlot = -1;
-    if (rssiIndex != -1) {
+                for (int j = 0; j < maxDevices; j++) {
-        int rssiStart = rssiIndex + 6;
+                    if (devices[j].deviceId == reportingDeviceId || !devices[j].active) {
-        int rssiComma = data.indexOf(',', rssiStart);
+                        deviceSlot = j;
-        if (rssiComma == -1) rssiComma = data.indexOf(')', rssiStart);
+                        break;
-        if (rssiComma != -1) {
-            rssi = data.substring(rssiStart, rssiComma).toFloat();
                     }
                 }
                 
-    // Update device data
+                if (deviceSlot >= 0) {
-    for (int i = 0; i < maxDevices; i++) {
+                    devices[deviceSlot].deviceId = reportingDeviceId;
-        if (devices[i].deviceId == deviceId || !devices[i].active) {
+                    devices[deviceSlot].distance = tagDistance;
-            devices[i].deviceId = deviceId;
+                    devices[deviceSlot].rssi = tagRssi;
-            devices[i].distance = distance;
+                    devices[deviceSlot].lastUpdate = millis();
-            devices[i].rssi = rssi;
+                    devices[deviceSlot].active = true;
-            devices[i].lastUpdate = millis();
-            devices[i].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;
+        }
+    }
     
     return false;
 }

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)

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
+    Serial.println("Configuring UWB module...");
-    if (!uwb.begin()) {
-        Serial.println("UWB initialization failed!");
-        return;
-    }
     
-    // Configure device based on type
+    // Complete UWB setup sequence following AT command protocol
-    Serial.printf("Configuring as %s...\n", deviceConfig.deviceName);
+    uwb.sendCommand("AT?", 2000);
-    uwb.configureDevice(deviceConfig.deviceId, deviceConfig.isAnchor);
+    delay(500);
-    uwb.setCapacity(deviceConfig.capacityTags, deviceConfig.capacityTimeSlot, deviceConfig.capacityExtMode);
+    
-    uwb.setNetwork(NETWORK_ID);
+    uwb.sendCommand("AT+RESTORE", 5000);
-    uwb.enableReporting(true);
+    delay(2000);  // Wait for factory reset to complete
-    uwb.saveConfiguration();
+    
-    delay(1000);
+    // Configure device role and parameters
-    uwb.restartDevice();
+    String configCmd = "AT+SETCFG=" + String(deviceConfig.deviceId) + ",";
-    delay(2000);
+    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)) {