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.
This commit is contained in:
martin
parent
a2e404ec0f
commit
7554b98d40
1 changed files with 89 additions and 54 deletions
125
src/main.cpp
125
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 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;
|
||||
}
|
||||
|
||||
// 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;
|
||||
#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) {
|
||||
#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("%s%d: %.2fm", deviceConfig.peerDisplayPrefix, peerDevices[i].deviceId, peerDevices[i].distance);
|
||||
display.printf("T%d: %.2fm", tags[i].deviceId, tags[i].distance);
|
||||
display.setCursor(70, yPos);
|
||||
display.printf("%.0fdBm", peerDevices[i].rssi);
|
||||
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;
|
||||
}
|
||||
}
|
||||
// Cleanup - no dynamic allocation needed
|
||||
Loading…
Add table
Add a link
Reference in a new issue