diff --git a/CLAUDE.md b/CLAUDE.md index 9cbd472..146a7b5 100644 --- a/CLAUDE.md +++ b/CLAUDE.md @@ -8,14 +8,20 @@ This is an ESP32-S3 based Ultra-Wideband (UWB) indoor positioning system for war **Key Architecture**: Battery-powered anchors auto-position themselves, mobile tag collects positioning data via USB to PC for real-time display and dual-file logging for offline analysis. +**Unified Firmware**: Single codebase with config-driven behavior for all device types, organized in modular configuration structure. + ## Development Commands -### Build & Flash +### Build & Flash (Unified Firmware) ```bash -# Build specific device type -pio run -e anchor # Build anchor firmware -pio run -e tag # Build tag firmware -pio run -e tag2 # Build second tag (ID 2) +# Build default configurations +pio run -e anchor # Build anchor (ID=0, Network=1234) +pio run -e tag # Build tag (ID=1, Network=1234) +pio run -e debug # Build debug anchor (ID=0, Network=1234) + +# Build with custom device ID and network +set PLATFORMIO_BUILD_FLAGS=-DUWB_INDEX=5 -DNETWORK_ID=2000 +pio run -e anchor # Build anchor ID=5, Network=2000 # Flash to device pio run -e tag -t upload @@ -26,10 +32,16 @@ pio device monitor ``` ### Development Environments -- **anchor**: Base station (UWB_INDEX=0, tracks tags) -- **tag**: Mobile tracker (UWB_INDEX=1, reports to anchors) -- **tag2**: Second mobile tracker (UWB_INDEX=2) -- **debug**: Development build with DEBUG_ENABLED=1 +- **anchor**: Base station with distributed positioning (default: ID=0, Network=1234) +- **tag**: Mobile tracker with coordinate collection (default: ID=1, Network=1234) +- **debug**: Development anchor build with DEBUG_ENABLED=1 + +### Unified Architecture +All devices now use the same `main.cpp` with config-driven behavior: +- Device type determined by build flags (`DEVICE_TYPE_ANCHOR`/`DEVICE_TYPE_TAG`) +- Behavior configured via `anchor_config.h` and `tag_config.h` +- Positioning algorithm settings in `positioning_config.h` +- Easy customization via `PLATFORMIO_BUILD_FLAGS` environment variable ## Core Architecture @@ -41,10 +53,11 @@ Tags: Range to anchors � Collect coordinates � USB to PC � Real-time + log ### Key Technical Patterns -**1. Dual Firmware Architecture** -- `main_anchor.cpp`: Tracks tags, manages UWB_TAG_COUNT devices, displays active tags -- `main_tag.cpp`: Connects to MAX_ANCHORS, reports to PC via USB, displays anchor distances -- Shared: config.h for hardware pins, UWBHelper library for AT commands +**1. Unified Firmware Architecture** +- `src/main.cpp`: Single firmware for all devices, behavior determined by build flags +- `src/config/`: Modular configuration system with device-specific settings +- Dynamic behavior: Device type, peer management, and display logic configured at compile time +- Shared: UWBHelper library for AT commands, unified communication and display handling **2. UWBHelper Library Structure** - **Complete AT Command Implementation**: All 21 commands from AT manual @@ -75,6 +88,13 @@ Tags: Range to anchors � Collect coordinates � USB to PC � Real-time + log **Timeout Management**: Devices marked inactive after DEVICE_TIMEOUT (5000ms) with no updates +**4. Modular Configuration System** +- `src/config/config.h`: Hardware pin definitions and basic system settings +- `src/config/device_config.h`: Device role detection and unified behavior configuration +- `src/config/anchor_config.h`: Anchor-specific settings (tag tracking, positioning, inter-anchor communication) +- `src/config/tag_config.h`: Tag-specific settings (anchor tracking, USB streaming, coordinate collection) +- `src/config/positioning_config.h`: Distributed positioning algorithm parameters and validation settings + ## Configuration Constants Key values in config.h: @@ -87,11 +107,18 @@ Key values in config.h: ## Development Roadmap Context This ESP32 firmware implements the hardware foundation for the UWB positioning system (see docs/ROADMAP.md): -- **Current**: Basic anchor-tag ranging with USB data streaming via UWBHelper library -- **Next**: Anchor auto-positioning, coordinate relay through tag, battery optimization +- **Current**: Unified firmware architecture with config-driven behavior, ready for distributed positioning implementation +- **Framework**: Configuration structure established for anchor discovery, position calculation, coordinate storage, and data relay +- **Next**: Implement anchor auto-positioning algorithms, coordinate relay through tag, battery optimization - **Goal**: <15min anchor setup, <30cm ranging accuracy, reliable USB data streaming to webapp - **WebApp Integration**: All data logging, visualization, and analysis handled by separate Next.js webapp +### Distributed Positioning Framework Ready +- **Anchor Behavior**: Framework for inter-anchor communication, discovery protocol, position calculation +- **Tag Behavior**: Framework for coordinate collection, USB data streaming, real-time positioning +- **Algorithm Foundation**: Trilateration, consensus mechanisms, coordinate system establishment +- **Storage System**: EEPROM-based position persistence for battery-powered anchors + ## Hardware Requirements - **Modules**: Makerfabs MaUWB ESP32-S3 with built-in UWB + OLED diff --git a/README.md b/README.md index 326dfb3..527993f 100644 --- a/README.md +++ b/README.md @@ -19,16 +19,22 @@ Ultra-wideband (UWB) positioning system using ESP32-S3 and Makerfabs UWB modules **Hardware:** [Makerfabs MaUWB ESP32-S3 UWB Module](https://www.makerfabs.com/mauwb-esp32s3-uwb-module.html) with SSD1306 OLED displays -## Environments -- `anchor`: Base station for positioning -- `tag`: Mobile device for tracking (ID 1) -- `tag2`: Mobile device for tracking (ID 2) +## Unified Architecture + +**Single Firmware** - All devices use the same `main.cpp` with config-driven behavior: +- `anchor`: Base station with distributed positioning framework (default: ID=0, Network=1234) +- `tag`: Mobile tracker with coordinate collection (default: ID=1, Network=1234) +- `debug`: Development build with debug output enabled ## Build & Upload ```bash -# Build specific environment -pio run -e anchor -pio run -e tag +# Build with default settings +pio run -e anchor # Anchor ID=0, Network=1234 +pio run -e tag # Tag ID=1, Network=1234 + +# Build with custom device ID and network +set PLATFORMIO_BUILD_FLAGS=-DUWB_INDEX=5 -DNETWORK_ID=2000 +pio run -e anchor # Anchor ID=5, Network=2000 # Upload to device pio run -e tag -t upload @@ -37,6 +43,19 @@ pio run -e tag -t upload pio device monitor ``` +## Project Structure + +### Modular Configuration System +- `src/main.cpp` - Single unified firmware for all device types +- `src/config/` - Device behavior configuration + - `config.h` - Hardware pins and system settings + - `device_config.h` - Device role detection and unified behavior + - `anchor_config.h` - Anchor-specific settings (positioning, inter-anchor communication) + - `tag_config.h` - Tag-specific settings (USB streaming, coordinate collection) + - `positioning_config.h` - Distributed positioning algorithm parameters +- `lib/UWBHelper/` - Complete UWB AT command library with positioning support +- `platformio.ini` - Simplified build environments (3 instead of 8+) + ## AT Command Support Complete implementation of all AT commands from the official manual: diff --git a/platformio.ini b/platformio.ini index f2015c0..e4f8a1d 100644 --- a/platformio.ini +++ b/platformio.ini @@ -25,36 +25,27 @@ lib_deps = adafruit/Adafruit BusIO@^1.14.4 bblanchon/ArduinoJson@^6.21.3 -; Anchor device configuration +; Unified Anchor configuration [env:anchor] -build_src_filter = +<*> - +build_src_filter = +<*> - - build_flags = ${env.build_flags} -DDEVICE_TYPE_ANCHOR=1 -DUWB_INDEX=0 -DNETWORK_ID=1234 -; Tag device configuration +; Unified Tag configuration [env:tag] -build_src_filter = +<*> - +build_src_filter = +<*> - - build_flags = ${env.build_flags} -DDEVICE_TYPE_TAG=1 -DUWB_INDEX=1 -DNETWORK_ID=1234 -; Tag device 2 (copy and modify UWB_INDEX for multiple tags) -[env:tag2] -build_src_filter = +<*> - -build_flags = - ${env.build_flags} - -DDEVICE_TYPE_TAG=1 - -DUWB_INDEX=2 - -DNETWORK_ID=1234 - ; Development environment with debugging [env:debug] -build_src_filter = +<*> - +build_src_filter = +<*> - - build_flags = ${env.build_flags} -DDEVICE_TYPE_ANCHOR=1 diff --git a/src/config/anchor_config.h b/src/config/anchor_config.h new file mode 100644 index 0000000..6836b68 --- /dev/null +++ b/src/config/anchor_config.h @@ -0,0 +1,39 @@ +#ifndef ANCHOR_CONFIG_H +#define ANCHOR_CONFIG_H + +// Anchor-specific configurations +#define MAX_PEER_DEVICES UWB_TAG_COUNT +#define PEER_DEVICE_TYPE "Tag" +#define PEER_PREFIX "T" +#define STATUS_PREFIX "Active Tags:" +#define WAITING_MESSAGE "No active tags" +#define SEARCHING_MESSAGE "Waiting..." + +// UWB Capacity Settings for Anchor +#define CAPACITY_TAGS UWB_TAG_COUNT +#define CAPACITY_TIMESLOT 10 +#define CAPACITY_EXT_MODE 1 + +// Anchor-specific behavior flags +#define ENABLE_TAG_TRACKING true +#define ENABLE_INTER_ANCHOR_COMM true +#define ENABLE_POSITION_CALCULATION true +#define ENABLE_COORDINATE_STORAGE true + +// Distributed Positioning Settings +#define ANCHOR_DISCOVERY_INTERVAL 30000 // 30s between discovery broadcasts +#define POSITION_UPDATE_INTERVAL 60000 // 1min position recalculation +#define NEIGHBOR_TIMEOUT 90000 // 90s anchor neighbor timeout +#define MIN_ANCHORS_FOR_POSITIONING 3 // Minimum anchors needed for trilateration + +// Inter-Anchor Communication +#define ANCHOR_BROADCAST_POWER "FD" // Full power for inter-anchor comm +#define DISCOVERY_RETRY_COUNT 3 // Number of discovery attempts +#define POSITION_CONFIDENCE_THRESHOLD 0.8 // Minimum confidence for valid position + +// Storage Settings +#define STORE_POSITION_IN_EEPROM true +#define EEPROM_POSITION_ADDRESS 0 +#define POSITION_VALIDATION_CHECKS true + +#endif // ANCHOR_CONFIG_H \ No newline at end of file diff --git a/src/config.h b/src/config/config.h similarity index 100% rename from src/config.h rename to src/config/config.h diff --git a/src/config/device_config.h b/src/config/device_config.h new file mode 100644 index 0000000..8f437d6 --- /dev/null +++ b/src/config/device_config.h @@ -0,0 +1,62 @@ +#ifndef DEVICE_CONFIG_H +#define DEVICE_CONFIG_H + +// Device Types +#define DEVICE_ANCHOR 0 +#define DEVICE_TAG 1 + +// Determine device type based on build flags +#if defined(DEVICE_TYPE_ANCHOR) + #define DEVICE_TYPE DEVICE_ANCHOR + #define DEVICE_IS_ANCHOR true + #define DEVICE_IS_TAG false + #define DEVICE_NAME "Anchor" +#elif defined(DEVICE_TYPE_TAG) + #define DEVICE_TYPE DEVICE_TAG + #define DEVICE_IS_ANCHOR false + #define DEVICE_IS_TAG true + #define DEVICE_NAME "Tag" +#else + #error "Device type not specified. Use DEVICE_TYPE_ANCHOR or DEVICE_TYPE_TAG" +#endif + +// Device Role Configurations +#if DEVICE_TYPE == DEVICE_ANCHOR + #include "anchor_config.h" +#elif DEVICE_TYPE == DEVICE_TAG + #include "tag_config.h" +#endif + +// Common device behavior settings +struct DeviceConfig { + int deviceId; + bool isAnchor; + const char* deviceName; + int maxPeers; + int capacityTags; + int capacityTimeSlot; + int capacityExtMode; + const char* statusDisplayPrefix; + const char* peerDisplayPrefix; + const char* waitingMessage; + const char* searchingMessage; +}; + +// Get current device configuration +inline DeviceConfig getDeviceConfig() { + return { + .deviceId = UWB_INDEX, + .isAnchor = DEVICE_IS_ANCHOR, + .deviceName = DEVICE_NAME, + .maxPeers = MAX_PEER_DEVICES, + .capacityTags = CAPACITY_TAGS, + .capacityTimeSlot = CAPACITY_TIMESLOT, + .capacityExtMode = CAPACITY_EXT_MODE, + .statusDisplayPrefix = STATUS_PREFIX, + .peerDisplayPrefix = PEER_PREFIX, + .waitingMessage = WAITING_MESSAGE, + .searchingMessage = SEARCHING_MESSAGE + }; +} + +#endif // DEVICE_CONFIG_H \ No newline at end of file diff --git a/src/config/positioning_config.h b/src/config/positioning_config.h new file mode 100644 index 0000000..0f9cbc6 --- /dev/null +++ b/src/config/positioning_config.h @@ -0,0 +1,54 @@ +#ifndef POSITIONING_CONFIG_H +#define POSITIONING_CONFIG_H + +// Distributed Positioning Algorithm Configuration +#define POSITIONING_ENABLED true + +// Algorithm Parameters +#define TRILATERATION_METHOD 1 // 1 = Standard, 2 = Weighted, 3 = Least Squares +#define MIN_DISTANCE_FOR_CALC 0.3 // Minimum distance in meters for calculations +#define MAX_DISTANCE_FOR_CALC 100.0 // Maximum distance in meters for calculations +#define DISTANCE_FILTER_ALPHA 0.3 // Low-pass filter coefficient for distances + +// Coordinate System +#define COORDINATE_SYSTEM_2D true // 2D positioning (true) or 3D (false) +#define ORIGIN_SELECTION_AUTO true // Auto-select origin anchor +#define COORDINATE_PRECISION 2 // Decimal places for coordinates + +// Position Validation +#define ENABLE_POSITION_VALIDATION true +#define MAX_POSITION_ERROR 2.0 // Maximum allowable position error in meters +#define MIN_POSITION_CONFIDENCE 0.5 // Minimum confidence score (0.0-1.0) +#define POSITION_STABILITY_THRESHOLD 0.5 // Position change threshold for stability + +// Network Topology +#define MAX_NETWORK_DIAMETER 200.0 // Maximum network size in meters +#define MIN_ANCHOR_SEPARATION 2.0 // Minimum distance between anchors +#define OPTIMAL_ANCHOR_SEPARATION 10.0 // Optimal distance between anchors + +// Communication Protocol +#define POSITION_DATA_FORMAT_JSON true // Use JSON format for position data +#define COMPRESS_POSITION_DATA false // Compress position data packets +#define POSITION_DATA_CHECKSUM true // Include checksum in position data + +// Performance Optimization +#define CALC_BATCH_SIZE 5 // Number of positions to calculate per batch +#define POSITION_CACHE_SIZE 10 // Number of positions to cache +#define ENABLE_PARALLEL_CALC false // Enable parallel calculations (if supported) + +// Debug and Logging +#define POSITION_DEBUG_OUTPUT false // Enable position calculation debug output +#define LOG_POSITION_HISTORY false // Log position history for analysis +#define POSITION_LOG_SIZE 100 // Number of positions to log + +// Error Handling +#define RETRY_FAILED_CALCULATIONS true // Retry failed position calculations +#define MAX_CALCULATION_RETRIES 3 // Maximum retry attempts +#define FALLBACK_TO_2D_ON_ERROR true // Fallback to 2D if 3D fails + +// Timing Configuration +#define POSITION_UPDATE_RATE_MS 500 // Position update interval in milliseconds +#define DISTANCE_MEASUREMENT_TIMEOUT 2000 // Timeout for distance measurements +#define CALCULATION_TIMEOUT_MS 1000 // Timeout for position calculations + +#endif // POSITIONING_CONFIG_H \ No newline at end of file diff --git a/src/config/tag_config.h b/src/config/tag_config.h new file mode 100644 index 0000000..19e6562 --- /dev/null +++ b/src/config/tag_config.h @@ -0,0 +1,45 @@ +#ifndef TAG_CONFIG_H +#define TAG_CONFIG_H + +// Tag-specific configurations +#define MAX_PEER_DEVICES MAX_ANCHORS +#define PEER_DEVICE_TYPE "Anchor" +#define PEER_PREFIX "A" +#define STATUS_PREFIX "Anchors:" +#define WAITING_MESSAGE "Searching..." +#define SEARCHING_MESSAGE "for anchors..." + +// UWB Capacity Settings for Tag +#define CAPACITY_TAGS 10 +#define CAPACITY_TIMESLOT 10 +#define CAPACITY_EXT_MODE 1 + +// Tag-specific behavior flags +#define ENABLE_ANCHOR_TRACKING true +#define ENABLE_USB_DATA_STREAMING true +#define ENABLE_COORDINATE_COLLECTION true +#define ENABLE_REAL_TIME_POSITIONING true + +// Anchor Connection Management +#define ANCHOR_SWITCH_THRESHOLD -80 // RSSI threshold for anchor switching +#define MIN_ANCHORS_FOR_POSITION 3 // Minimum anchors for position calculation +#define ANCHOR_SELECTION_INTERVAL 5000 // 5s interval for anchor selection review +#define CONNECTION_STABILITY_TIME 10000 // 10s stable connection before switching + +// Data Streaming Settings +#define USB_STREAM_INTERVAL 100 // 100ms USB data streaming interval +#define STREAM_RAW_DISTANCES true // Stream raw distance measurements +#define STREAM_ANCHOR_POSITIONS true // Stream collected anchor coordinates +#define STREAM_CALCULATED_POSITION true // Stream tag's calculated position + +// Coordinate Collection +#define POSITION_REQUEST_INTERVAL 30000 // 30s interval to request anchor positions +#define COORDINATE_CACHE_TIMEOUT 300000 // 5min timeout for cached anchor positions +#define MAX_POSITION_REQUEST_RETRIES 3 // Retry attempts for position requests + +// Position Calculation +#define POSITION_SMOOTHING_FACTOR 0.3 // Low-pass filter for position smoothing +#define ENABLE_POSITION_PREDICTION true // Predict position during anchor handoffs +#define MAX_POSITION_JUMP 5.0 // Max allowed position jump in meters + +#endif // TAG_CONFIG_H \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp new file mode 100644 index 0000000..252e965 --- /dev/null +++ b/src/main.cpp @@ -0,0 +1,316 @@ +#include +#include +#include +#include +#include "config/config.h" +#include "config/device_config.h" +#include "config/positioning_config.h" +#include "UWBHelper.h" + +// Function declarations +void showStartupScreen(); +void processUWBData(); +void updateDisplay(); +void checkTimeouts(); +void processAnchorBehavior(); +void processTagBehavior(); +void handleSerialPassthrough(); + +// Hardware setup +HardwareSerial uwbSerial(2); +Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET); +UWBHelper uwb(&uwbSerial, RESET_PIN); + +// Get device configuration +DeviceConfig deviceConfig = getDeviceConfig(); + +// Dynamic data storage based on device type +DeviceData* peerDevices = nullptr; +DistanceFilter* deviceFilters = nullptr; +String response = ""; +unsigned long lastDisplayUpdate = 0; +unsigned long lastPositioningUpdate = 0; + +void setup() { + Serial.begin(SERIAL_BAUD); + Serial.printf("Starting UWB %s (ID: %d)...\n", deviceConfig.deviceName, deviceConfig.deviceId); + + // Initialize display + Wire.begin(I2C_SDA, I2C_SCL); + if (!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) { + Serial.println(F("SSD1306 allocation failed")); + for (;;); + } + + showStartupScreen(); + + // Initialize UWB + uwbSerial.begin(115200, SERIAL_8N1, IO_RXD2, IO_TXD2); + + // Reset UWB module + pinMode(RESET_PIN, OUTPUT); + digitalWrite(RESET_PIN, HIGH); + delay(1000); + + // Test communication + if (!uwb.testConnection()) { + Serial.println("UWB initialization failed!"); + return; + } + + // 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); + + // 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; + } + + Serial.printf("%s ready! Network ID: %d\n", deviceConfig.deviceName, NETWORK_ID); + + #if DEVICE_TYPE == DEVICE_ANCHOR + Serial.println("Anchor behaviors enabled:"); + #ifdef ENABLE_TAG_TRACKING + Serial.println("- Tag tracking"); + #endif + #ifdef ENABLE_INTER_ANCHOR_COMM + Serial.println("- Inter-anchor communication"); + #endif + #ifdef ENABLE_POSITION_CALCULATION + Serial.println("- Position calculation"); + #endif + #elif DEVICE_TYPE == DEVICE_TAG + Serial.println("Tag behaviors enabled:"); + #ifdef ENABLE_ANCHOR_TRACKING + Serial.println("- Anchor tracking"); + #endif + #ifdef ENABLE_USB_DATA_STREAMING + Serial.println("- USB data streaming"); + #endif + #ifdef ENABLE_COORDINATE_COLLECTION + Serial.println("- Coordinate collection"); + #endif + #endif +} + +void loop() { + processUWBData(); + updateDisplay(); + checkTimeouts(); + handleSerialPassthrough(); + + // Execute device-specific behavior + if (DEVICE_IS_ANCHOR) { + processAnchorBehavior(); + } else { + processTagBehavior(); + } + + delay(10); +} + +void processUWBData() { + while (uwbSerial.available()) { + char c = uwbSerial.read(); + if (c == '\r') continue; + if (c == '\n') { + 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); + } + } + } + response = ""; + } + } else { + response += c; + } + } +} + +void updateDisplay() { + if (millis() - lastDisplayUpdate < DISPLAY_UPDATE_INTERVAL) return; + + display.clearDisplay(); + display.setTextSize(1); + display.setTextColor(SSD1306_WHITE); + + // Header + display.setCursor(0, 0); + display.printf("%s %d (Net:%d)", deviceConfig.deviceName, deviceConfig.deviceId, NETWORK_ID); + + display.setCursor(0, 10); + display.println(deviceConfig.statusDisplayPrefix); + + // Show active peer devices + 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 (activeCount == 0) { + display.setCursor(0, 30); + display.println(deviceConfig.waitingMessage); + display.setCursor(0, 40); + display.println(deviceConfig.searchingMessage); + } + + // Status line with device-specific information + display.setCursor(0, 56); + if (DEVICE_IS_ANCHOR) { + display.printf("Active: %d/%d", activeCount, deviceConfig.maxPeers); + } else { + display.printf("Found: %d/%d", activeCount, deviceConfig.maxPeers); + } + + display.display(); + lastDisplayUpdate = millis(); +} + +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); + } + } +} + +void processAnchorBehavior() { + #if DEVICE_TYPE == DEVICE_ANCHOR + unsigned long currentTime = millis(); + + #ifdef ENABLE_INTER_ANCHOR_COMM + // TODO: Implement inter-anchor discovery and communication + static unsigned long lastDiscovery = 0; + if (currentTime - lastDiscovery > ANCHOR_DISCOVERY_INTERVAL) { + // Broadcast discovery signal to other anchors + DEBUG_PRINTLN("Broadcasting anchor discovery..."); + lastDiscovery = currentTime; + } + #endif + + #ifdef ENABLE_POSITION_CALCULATION + // TODO: Implement distributed position calculation + if (currentTime - lastPositioningUpdate > POSITION_UPDATE_INTERVAL) { + // Calculate and update anchor position + DEBUG_PRINTLN("Updating anchor position..."); + lastPositioningUpdate = currentTime; + } + #endif + + #ifdef ENABLE_COORDINATE_STORAGE + // TODO: Store calculated position in EEPROM + #endif + #endif +} + +void processTagBehavior() { + #if DEVICE_TYPE == DEVICE_TAG + unsigned long currentTime = millis(); + + #ifdef ENABLE_USB_DATA_STREAMING + // Stream positioning data via USB + static unsigned long lastStream = 0; + if (currentTime - lastStream > USB_STREAM_INTERVAL) { + // TODO: Stream formatted positioning data + lastStream = currentTime; + } + #endif + + #ifdef ENABLE_COORDINATE_COLLECTION + // Request anchor positions + static unsigned long lastPositionRequest = 0; + if (currentTime - lastPositionRequest > POSITION_REQUEST_INTERVAL) { + // TODO: Request positions from connected anchors + DEBUG_PRINTLN("Requesting anchor positions..."); + lastPositionRequest = currentTime; + } + #endif + + #ifdef ENABLE_REAL_TIME_POSITIONING + // Calculate tag position + if (currentTime - lastPositioningUpdate > POSITION_UPDATE_RATE_MS) { + // TODO: Calculate real-time tag position + lastPositioningUpdate = currentTime; + } + #endif + #endif +} + +void handleSerialPassthrough() { + // Handle serial passthrough for debugging + while (Serial.available()) { + uwbSerial.write(Serial.read()); + } +} + +void showStartupScreen() { + display.clearDisplay(); + display.setTextSize(1); + display.setTextColor(SSD1306_WHITE); + display.setCursor(0, 0); + display.printf("MaUWB %s", deviceConfig.deviceName); + + display.setCursor(0, 15); + display.printf("ID: %d", deviceConfig.deviceId); + + display.setCursor(0, 25); + display.printf("Network: %d", NETWORK_ID); + + display.setCursor(0, 35); + display.println("6.8Mbps Mode"); + + display.setCursor(0, 45); + display.println("Initializing..."); + + display.display(); + delay(2000); +} + +// Cleanup +void cleanup() { + if (peerDevices) { + delete[] peerDevices; + peerDevices = nullptr; + } + if (deviceFilters) { + delete[] deviceFilters; + deviceFilters = nullptr; + } +} \ No newline at end of file diff --git a/src/main_anchor.cpp b/src/main_anchor.cpp deleted file mode 100644 index 4e79178..0000000 --- a/src/main_anchor.cpp +++ /dev/null @@ -1,183 +0,0 @@ -#include -#include -#include -#include -#include "config.h" -#include "UWBHelper.h" - -// Function declarations -void showStartupScreen(); -void processUWBData(); -void updateDisplay(); -void checkTimeouts(); - -// Hardware setup -HardwareSerial uwbSerial(2); -Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET); -UWBHelper uwb(&uwbSerial, RESET_PIN); - -// Data storage -DeviceData tags[UWB_TAG_COUNT]; -DistanceFilter tagFilters[UWB_TAG_COUNT]; -String response = ""; -unsigned long lastDisplayUpdate = 0; - -void setup() { - Serial.begin(SERIAL_BAUD); - Serial.println("Starting UWB Anchor..."); - - // Initialize I2C and display - Wire.begin(I2C_SDA, I2C_SCL); - if (!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) { - Serial.println(F("SSD1306 allocation failed")); - for (;;); - } - - showStartupScreen(); - - // Initialize UWB - uwbSerial.begin(115200, SERIAL_8N1, IO_RXD2, IO_TXD2); - delay(1000); - - if (!uwb.begin()) { - Serial.println("UWB initialization failed!"); - return; - } - - // Configure as anchor - Serial.println("Configuring as Anchor..."); - uwb.configureDevice(UWB_INDEX, true); // true = anchor - uwb.setCapacity(UWB_TAG_COUNT, 10, 1); - uwb.setNetwork(NETWORK_ID); - uwb.enableReporting(true); - uwb.saveConfiguration(); - delay(1000); - uwb.restartDevice(); - delay(2000); - - // Initialize tag data - 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; - } - - Serial.println("Anchor ready! Waiting for tags..."); -} - -void loop() { - processUWBData(); - updateDisplay(); - checkTimeouts(); - - // Handle serial passthrough for debugging - while (Serial.available()) { - uwbSerial.write(Serial.read()); - } - - delay(10); -} - -void processUWBData() { - while (uwbSerial.available()) { - char c = uwbSerial.read(); - if (c == '\r') continue; - if (c == '\n') { - if (response.length() > 0) { - Serial.println("Received: " + response); - if (uwb.parseRangeData(response, tags, UWB_TAG_COUNT)) { - // Successfully parsed data - 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); - } - } - } - response = ""; - } - } else { - response += c; - } - } -} - -void updateDisplay() { - if (millis() - lastDisplayUpdate < DISPLAY_UPDATE_INTERVAL) return; - - display.clearDisplay(); - display.setTextSize(1); - display.setTextColor(SSD1306_WHITE); - - // Header - display.setCursor(0, 0); - display.printf("Anchor %d (Net:%d)", UWB_INDEX, NETWORK_ID); - - display.setCursor(0, 10); - display.println("Active Tags:"); - - // Show active tags - int activeCount = 0; - int yPos = 20; - - 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++; - } - } - - if (activeCount == 0) { - display.setCursor(0, 30); - display.println("No active tags"); - display.setCursor(0, 40); - display.println("Waiting..."); - } - - // Status line - display.setCursor(0, 56); - display.printf("Active: %d/%d", activeCount, UWB_TAG_COUNT); - - display.display(); - lastDisplayUpdate = millis(); -} - -void checkTimeouts() { - unsigned long currentTime = millis(); - 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); - } - } -} - -void showStartupScreen() { - display.clearDisplay(); - display.setTextSize(1); - display.setTextColor(SSD1306_WHITE); - display.setCursor(0, 0); - display.println("MaUWB Anchor"); - - display.setCursor(0, 15); - display.printf("ID: %d", UWB_INDEX); - - display.setCursor(0, 25); - display.printf("Network: %d", NETWORK_ID); - - display.setCursor(0, 35); - display.println("6.8Mbps Mode"); - - display.setCursor(0, 45); - display.println("Initializing..."); - - display.display(); - delay(2000); -} - diff --git a/src/main_tag.cpp b/src/main_tag.cpp deleted file mode 100644 index 8fe45b0..0000000 --- a/src/main_tag.cpp +++ /dev/null @@ -1,168 +0,0 @@ -#include -#include -#include -#include -#include "config.h" -#include "UWBHelper.h" - -// Function declarations -void showStartupScreen(); -void processUWBData(); -void updateDisplay(); -void checkTimeouts(); - -// Hardware setup -HardwareSerial uwbSerial(2); -Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET); -UWBHelper uwb(&uwbSerial, RESET_PIN); - -// Data storage -DeviceData anchors[MAX_ANCHORS]; -String response = ""; -unsigned long lastDisplayUpdate = 0; - -void setup() { - Serial.begin(SERIAL_BAUD); - - // Initialize display - Wire.begin(I2C_SDA, I2C_SCL); - if (!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) { - Serial.println(F("SSD1306 allocation failed")); - for (;;); - } - - showStartupScreen(); - - // Initialize UWB - uwbSerial.begin(115200, SERIAL_8N1, IO_RXD2, IO_TXD2); - - if (!uwb.begin()) { - Serial.println("UWB initialization failed!"); - return; - } - - // Configure as tag - Serial.println("Configuring as Tag..."); - uwb.configureDevice(UWB_INDEX, false); // false = tag - uwb.setCapacity(10, 10, 1); - uwb.setNetwork(NETWORK_ID); - uwb.enableReporting(true); - uwb.saveConfiguration(); - delay(1000); - uwb.restartDevice(); - delay(2000); - - // Initialize anchor data - for (int i = 0; i < MAX_ANCHORS; i++) { - anchors[i].deviceId = -1; - anchors[i].active = false; - } - - Serial.println("Tag ready!"); -} - -void loop() { - processUWBData(); - updateDisplay(); - checkTimeouts(); - - // Handle serial passthrough for debugging - while (Serial.available()) { - uwbSerial.write(Serial.read()); - } - - delay(10); -} - -void processUWBData() { - while (uwbSerial.available()) { - char c = uwbSerial.read(); - if (c == '\r') continue; - if (c == '\n') { - if (response.length() > 0) { - Serial.println("Received: " + response); - if (uwb.parseRangeData(response, anchors, MAX_ANCHORS)) { - // Successfully parsed data - 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); - } - } - } - response = ""; - } - } else { - response += c; - } - } -} - -void updateDisplay() { - if (millis() - lastDisplayUpdate < DISPLAY_UPDATE_INTERVAL) return; - - display.clearDisplay(); - display.setTextSize(1); - display.setTextColor(SSD1306_WHITE); - - // Header - display.setCursor(0, 0); - display.printf("Tag %d (Net:%d)", UWB_INDEX, NETWORK_ID); - - display.setCursor(0, 10); - display.println("Anchors:"); - - int activeCount = 0; - int yPos = 20; - - 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++; - } - } - - if (activeCount == 0) { - display.setCursor(0, 30); - display.println("Searching..."); - display.setCursor(0, 40); - display.println("for anchors..."); - } - - // Status line - display.setCursor(0, 56); - display.printf("Found: %d/%d", activeCount, MAX_ANCHORS); - - display.display(); - lastDisplayUpdate = millis(); -} - -void checkTimeouts() { - unsigned long currentTime = millis(); - for (int i = 0; i < MAX_ANCHORS; i++) { - if (anchors[i].active && (currentTime - anchors[i].lastUpdate > DEVICE_TIMEOUT)) { - anchors[i].active = false; - } - } -} - - -void showStartupScreen() { - display.clearDisplay(); - display.setTextSize(1); - display.setTextColor(SSD1306_WHITE); - display.setCursor(0, 0); - display.println("MaUWB Tag"); - display.setCursor(0, 15); - display.printf("ID: %d", UWB_INDEX); - display.setCursor(0, 25); - display.printf("Network: %d", NETWORK_ID); - display.setCursor(0, 35); - display.println("Initializing..."); - display.display(); - delay(2000); -} \ No newline at end of file