From 7554b98d4032bb21dc4dc0ffb02213cd07b5fff9 Mon Sep 17 00:00:00 2001 From: martin Date: Thu, 21 Aug 2025 23:22:35 +0200 Subject: [PATCH] Fix unified firmware display and initialization issues - Replace dynamic allocation with compile-time fixed arrays (tags[UWB_TAG_COUNT], anchors[MAX_ANCHORS]) - Restore uwb.begin() instead of testConnection() for proper UWB initialization - Add I2C bus reset (Wire.end()/Wire.begin()) to prevent bus conflicts during multi-device initialization - Use compile-time branching (#if DEVICE_TYPE) instead of runtime checks for better performance - Simplify display logic to directly access device arrays without null pointer complexity - Remove unused dynamic memory management and cleanup functions - Fix device counting to use proper array sizes (UWB_TAG_COUNT, MAX_ANCHORS) Resolves crash during initialization when multiple devices start simultaneously. Display now properly shows found anchors/tags with distances and RSSI values. --- src/main.cpp | 143 ++++++++++++++++++++++++++++++++------------------- 1 file changed, 89 insertions(+), 54 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index 252e965..b41b351 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -24,9 +24,14 @@ UWBHelper uwb(&uwbSerial, RESET_PIN); // Get device configuration DeviceConfig deviceConfig = getDeviceConfig(); -// Dynamic data storage based on device type -DeviceData* peerDevices = nullptr; -DistanceFilter* deviceFilters = nullptr; +// Data storage based on device type +#if DEVICE_TYPE == DEVICE_ANCHOR +DeviceData tags[UWB_TAG_COUNT]; +DistanceFilter tagFilters[UWB_TAG_COUNT]; +#elif DEVICE_TYPE == DEVICE_TAG +DeviceData anchors[MAX_ANCHORS]; +#endif + String response = ""; unsigned long lastDisplayUpdate = 0; unsigned long lastPositioningUpdate = 0; @@ -36,6 +41,8 @@ void setup() { Serial.printf("Starting UWB %s (ID: %d)...\n", deviceConfig.deviceName, deviceConfig.deviceId); // Initialize display + Wire.end(); // Ensure I2C bus is properly reset + delay(100); Wire.begin(I2C_SDA, I2C_SCL); if (!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) { Serial.println(F("SSD1306 allocation failed")); @@ -52,8 +59,8 @@ void setup() { digitalWrite(RESET_PIN, HIGH); delay(1000); - // Test communication - if (!uwb.testConnection()) { + // Initialize UWB + if (!uwb.begin()) { Serial.println("UWB initialization failed!"); return; } @@ -69,20 +76,24 @@ void setup() { uwb.restartDevice(); delay(2000); - // Allocate memory for peer devices based on device type - peerDevices = new DeviceData[deviceConfig.maxPeers]; - if (DEVICE_IS_ANCHOR) { - deviceFilters = new DistanceFilter[deviceConfig.maxPeers]; - } - - // Initialize peer device data - for (int i = 0; i < deviceConfig.maxPeers; i++) { - peerDevices[i].deviceId = -1; - peerDevices[i].active = false; - peerDevices[i].distance = 0.0; - peerDevices[i].rssi = 0.0; - peerDevices[i].lastUpdate = 0; - } + // Initialize device data + #if DEVICE_TYPE == DEVICE_ANCHOR + for (int i = 0; i < UWB_TAG_COUNT; i++) { + tags[i].deviceId = -1; + tags[i].active = false; + tags[i].distance = 0.0; + tags[i].rssi = 0.0; + tags[i].lastUpdate = 0; + } + #elif DEVICE_TYPE == DEVICE_TAG + for (int i = 0; i < MAX_ANCHORS; i++) { + anchors[i].deviceId = -1; + anchors[i].active = false; + anchors[i].distance = 0.0; + anchors[i].rssi = 0.0; + anchors[i].lastUpdate = 0; + } + #endif Serial.printf("%s ready! Network ID: %d\n", deviceConfig.deviceName, NETWORK_ID); @@ -135,16 +146,27 @@ void processUWBData() { if (response.length() > 0) { Serial.println("Received: " + response); - if (uwb.parseRangeData(response, peerDevices, deviceConfig.maxPeers)) { - // Successfully parsed data - log active peers - for (int i = 0; i < deviceConfig.maxPeers; i++) { - if (peerDevices[i].active && (millis() - peerDevices[i].lastUpdate < 1000)) { - Serial.printf("%s %d: %.2fm, RSSI: %.1fdBm\n", - deviceConfig.peerDisplayPrefix, peerDevices[i].deviceId, - peerDevices[i].distance, peerDevices[i].rssi); + #if DEVICE_TYPE == DEVICE_ANCHOR + if (uwb.parseRangeData(response, tags, UWB_TAG_COUNT)) { + // Successfully parsed data - log active tags + for (int i = 0; i < UWB_TAG_COUNT; i++) { + if (tags[i].active && (millis() - tags[i].lastUpdate < 1000)) { + Serial.printf("Tag %d: %.2fm, RSSI: %.1fdBm\n", + tags[i].deviceId, tags[i].distance, tags[i].rssi); + } } } - } + #elif DEVICE_TYPE == DEVICE_TAG + if (uwb.parseRangeData(response, anchors, MAX_ANCHORS)) { + // Successfully parsed data - log active anchors + for (int i = 0; i < MAX_ANCHORS; i++) { + if (anchors[i].active && (millis() - anchors[i].lastUpdate < 1000)) { + Serial.printf("Anchor %d: %.2fm, RSSI: %.1fdBm\n", + anchors[i].deviceId, anchors[i].distance, anchors[i].rssi); + } + } + } + #endif response = ""; } } else { @@ -171,16 +193,29 @@ void updateDisplay() { int activeCount = 0; int yPos = 20; - for (int i = 0; i < deviceConfig.maxPeers && yPos < 55; i++) { - if (peerDevices[i].active) { - display.setCursor(0, yPos); - display.printf("%s%d: %.2fm", deviceConfig.peerDisplayPrefix, peerDevices[i].deviceId, peerDevices[i].distance); - display.setCursor(70, yPos); - display.printf("%.0fdBm", peerDevices[i].rssi); - yPos += 8; - activeCount++; + #if DEVICE_TYPE == DEVICE_ANCHOR + for (int i = 0; i < UWB_TAG_COUNT && yPos < 55; i++) { + if (tags[i].active) { + display.setCursor(0, yPos); + display.printf("T%d: %.2fm", tags[i].deviceId, tags[i].distance); + display.setCursor(70, yPos); + display.printf("%.0fdBm", tags[i].rssi); + yPos += 8; + activeCount++; + } } - } + #elif DEVICE_TYPE == DEVICE_TAG + for (int i = 0; i < MAX_ANCHORS && yPos < 55; i++) { + if (anchors[i].active) { + display.setCursor(0, yPos); + display.printf("A%d: %.2fm", anchors[i].deviceId, anchors[i].distance); + display.setCursor(70, yPos); + display.printf("%.0fdBm", anchors[i].rssi); + yPos += 8; + activeCount++; + } + } + #endif if (activeCount == 0) { display.setCursor(0, 30); @@ -192,9 +227,9 @@ void updateDisplay() { // Status line with device-specific information display.setCursor(0, 56); if (DEVICE_IS_ANCHOR) { - display.printf("Active: %d/%d", activeCount, deviceConfig.maxPeers); + display.printf("Active: %d/%d", activeCount, UWB_TAG_COUNT); } else { - display.printf("Found: %d/%d", activeCount, deviceConfig.maxPeers); + display.printf("Found: %d/%d", activeCount, MAX_ANCHORS); } display.display(); @@ -203,12 +238,22 @@ void updateDisplay() { void checkTimeouts() { unsigned long currentTime = millis(); - for (int i = 0; i < deviceConfig.maxPeers; i++) { - if (peerDevices[i].active && (currentTime - peerDevices[i].lastUpdate > DEVICE_TIMEOUT)) { - peerDevices[i].active = false; - Serial.printf("%s %d timeout\n", deviceConfig.peerDisplayPrefix, peerDevices[i].deviceId); + + #if DEVICE_TYPE == DEVICE_ANCHOR + for (int i = 0; i < UWB_TAG_COUNT; i++) { + if (tags[i].active && (currentTime - tags[i].lastUpdate > DEVICE_TIMEOUT)) { + tags[i].active = false; + Serial.printf("Tag %d timeout\n", tags[i].deviceId); + } } - } + #elif DEVICE_TYPE == DEVICE_TAG + for (int i = 0; i < MAX_ANCHORS; i++) { + if (anchors[i].active && (currentTime - anchors[i].lastUpdate > DEVICE_TIMEOUT)) { + anchors[i].active = false; + Serial.printf("Anchor %d timeout\n", anchors[i].deviceId); + } + } + #endif } void processAnchorBehavior() { @@ -303,14 +348,4 @@ void showStartupScreen() { delay(2000); } -// Cleanup -void cleanup() { - if (peerDevices) { - delete[] peerDevices; - peerDevices = nullptr; - } - if (deviceFilters) { - delete[] deviceFilters; - deviceFilters = nullptr; - } -} \ No newline at end of file +// Cleanup - no dynamic allocation needed \ No newline at end of file