diff --git a/CLAUDE.md b/CLAUDE.md new file mode 100644 index 0000000..3a2c0eb --- /dev/null +++ b/CLAUDE.md @@ -0,0 +1,106 @@ +# CLAUDE.md + +This file provides guidance to Claude Code (claude.ai/code) when working with code in this repository. + +## Project Overview + +This is an ESP32-S3 based Ultra-Wideband (UWB) indoor positioning system for warehouse WiFi mapping. The system consists of anchor devices (base stations) and tag devices (mobile trackers) using Makerfabs MaUWB modules with Qorvo DW3000 chips. + +**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. + +## Development Commands + +### Build & Flash +```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) + +# Flash to device +pio run -e tag -t upload +pio run -e anchor -t upload + +# Monitor serial output +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 + +## Core Architecture + +### Device Roles & Communication Flow +``` +Anchors: Auto-position ’ Store coordinates locally ’ Report to tags +Tags: Range to anchors ’ Collect coordinates ’ USB to PC ’ Real-time + logging +``` + +### 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 + +**2. UWBHelper Library Structure** +- **Complete AT Command Implementation**: All 21 commands from AT manual +- **Device Role Configuration**: `configureDevice(id, isAnchor)` sets anchor/tag mode +- **Advanced Data Structures**: RangeResult, AnchorPosition, DeviceData for multi-device tracking +- **Position Calculation**: PositionCalculator class with trilateration algorithms +- **Data Filtering**: DistanceFilter for noise reduction + +**3. Hardware Configuration** +- **UWB Communication**: UART2 (pins 17/18) at 115200 baud +- **Display**: SSD1306 OLED via I2C (pins 38/39) +- **Reset**: Pin 16 for UWB module reset +- **Network**: ID 1234, 6.8Mbps mode, range filtering enabled + +### Critical Implementation Details + +**Device Identification**: Each device gets unique UWB_INDEX via build flags, determines network role and behavior + +**Data Flow Pattern**: +- Anchors parse range data from tags: `parseRangeData(response, tags[], UWB_TAG_COUNT)` +- Tags parse range data from anchors: `parseRangeData(response, anchors[], MAX_ANCHORS)` +- Serial passthrough enabled for debugging: `uwbSerial.write(Serial.read())` + +**Display Logic**: +- Anchors show active tags with distances/RSSI +- Tags show connected anchors with distances/RSSI +- Both show network status and device counts + +**Timeout Management**: Devices marked inactive after DEVICE_TIMEOUT (5000ms) with no updates + +## Configuration Constants + +Key values in config.h: +- `MAX_ANCHORS 8`: Tag connects to 8 closest anchors +- `UWB_TAG_COUNT 64`: Network supports up to 64 tags +- `NETWORK_ID 1234`: UWB network identifier +- `DEVICE_TIMEOUT 5000`: Device inactivity timeout (ms) +- `DISPLAY_UPDATE_INTERVAL 500`: OLED refresh rate (ms) + +## Development Roadmap Context + +This system implements **Phase 1** of a 5-phase indoor positioning project (see docs/ROADMAP.md): +- **Current**: Basic anchor-tag ranging with USB data collection +- **Next**: Anchor auto-positioning, dual-file logging, PC software, web visualization +- **Goal**: <15min warehouse setup, <30cm accuracy, real-time + offline analysis + +## Hardware Requirements + +- **Modules**: Makerfabs MaUWB ESP32-S3 with built-in UWB + OLED +- **Power**: USB for development, battery for production anchors +- **Range**: Tested up to 100m line-of-sight, 30m through walls +- **Network**: No WiFi required, pure UWB communication + +## Important Notes + +- **No Sleep/OTA**: Removed for simplicity, focus on core positioning +- **AT Command Protocol**: Direct UART communication with UWB module +- **Position Calculation**: Client-side (PC) processing, not on-device +- **Data Logging**: Raw distance data + anchor coordinates for offline analysis \ No newline at end of file diff --git a/README.md b/README.md index 0f23456..326dfb3 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,8 @@ # MaUWB ESP32-S3 Positioning System -Ultra-wideband (UWB) positioning system using ESP32-S3 and Makerfabs UWB modules. +Ultra-wideband (UWB) positioning system using ESP32-S3 and Makerfabs UWB modules for indoor positioning and warehouse mapping applications. + +![Working MaUWB Devices](docs/images/20250819_202527.jpg) ## Features - ESP32-S3 based anchor and tag devices @@ -8,12 +10,14 @@ Ultra-wideband (UWB) positioning system using ESP32-S3 and Makerfabs UWB modules - OLED display for status and measurements - Multiple tag support (up to 64 tags) - 6.8Mbps communication rate -- Serial debugging output +- Complete AT command implementation +- Position calculation with trilateration +- Anchor auto-positioning system +- Real-time positioning with USB data logging -## Hardware -- ESP32-S3 DevKit -- Makerfabs UWB AT Module -- SSD1306 OLED Display (128x64) +![MaUWB Modules](docs/images/20250819_202629.jpg) + +**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 @@ -33,8 +37,63 @@ pio run -e tag -t upload pio device monitor ``` +## AT Command Support + +Complete implementation of all AT commands from the official manual: + +### Basic Commands +- `AT?` - Test connection +- `AT+GETVER?` - Get firmware version +- `AT+RESTART` - Restart module +- `AT+RESTORE` - Factory reset +- `AT+SAVE` - Save configuration + +### Configuration +- `AT+SETCFG` / `AT+GETCFG?` - Device configuration +- `AT+SETANT` / `AT+GETANT?` - Antenna delay calibration +- `AT+SETCAP` / `AT+GETCAP?` - System capacity settings +- `AT+SETRPT` / `AT+GETRPT?` - Auto-reporting control + +### Network & Power +- `AT+SETPAN` / `AT+GETPAN?` - Network ID configuration +- `AT+SETPOW` / `AT+GETPOW?` - Transmission power control +- `AT+SLEEP` - Sleep mode for battery conservation + +### Data Communication +- `AT+DATA` / `AT+RDATA` - Custom data transmission +- Real-time range reporting via `AT+RANGE` parsing + +## Library Features + +The enhanced UWBHelper library provides: +- **Complete AT command coverage** +- **Advanced range data parsing** for multiple anchors +- **Position calculation algorithms** (trilateration, multilateration) +- **Anchor position management** for auto-positioning +- **Distance filtering** for improved accuracy +- **Backward compatibility** with existing code + ## Configuration -- Network ID: 1234 -- Baud Rate: 115200 -- Communication: 6.8Mbps -- Range filtering: Enabled +- **Network ID**: 1234 (configurable via AT+SETPAN) +- **Baud Rate**: 115200 +- **Communication**: 6.8Mbps (AT+SETCFG parameter) +- **Range filtering**: Enabled for accuracy +- **Refresh Rate**: 10Hz (configurable via AT+SETCAP) +- **Max Anchors**: Unlimited (tags connect to 8 closest) +- **Max Tags**: 64 per network + + +## Documentation + +- [AT Command Manual](docs/manuals/Makerfabs%20UWB%20AT%20Module%20AT%20Command%20Manual(v1.1.1).pdf) - Complete AT command reference +- [Hardware Product Page](https://www.makerfabs.com/mauwb-esp32s3-uwb-module.html) - Official hardware documentation +- [Project Roadmap](docs/ROADMAP.md) - Development plan for indoor positioning system + +## Applications + +This system is designed for: +- **Indoor positioning** in warehouses and large buildings +- **Asset tracking** and inventory management +- **Navigation assistance** in GPS-denied environments +- **WiFi signal mapping** and coverage analysis +- **Research and development** in UWB positioning diff --git a/docs/ROADMAP.md b/docs/ROADMAP.md new file mode 100644 index 0000000..15ae09d --- /dev/null +++ b/docs/ROADMAP.md @@ -0,0 +1,255 @@ +# Indoor Positioning System Roadmap +**MaUWB ESP32-S3 Warehouse Positioning & WiFi Mapping System** + +--- + +## 🎯 Project Overview + +**Purpose**: Indoor positioning system for warehouse WiFi strength mapping +- **Hardware**: 1 mobile tag + 8+ battery-powered anchors +- **Connectivity**: Tag connects to PC via USB, anchors are wireless-only +- **Constraint**: Tag connects to 8 closest anchors (auto-switching) +- **Goal**: Quick installation (<15 min) with automatic anchor positioning + +--- + +## 📋 System Requirements + +### Hardware Components +- [ ] **Tag Device**: ESP32-S3 with USB connectivity +- [ ] **Anchor Devices**: 8+ ESP32-S3 units (battery powered) +- [ ] **PC Interface**: USB connection for data collection +- [ ] **Power Management**: Battery-powered anchors (no mains/WiFi) + +### Core Constraints +- ✅ Battery-powered anchors (no WiFi connectivity) +- ✅ Tag connects to 8 closest anchors (auto-switching) +- ✅ USB-only connection (tag to PC) +- ✅ No WiFi scanning needed (handled by PC) +- ✅ Warehouse too large for simultaneous connection to all anchors + +--- + +## ðŸ—ï¸ System Architecture + +### Data Flow Design +``` +Anchors (Battery) → Auto-Position Calibration → Store Coordinates Locally + ↓ +Tag (Mobile) → Collect Raw Data + Anchor Coordinates → PC (USB) → Real-time Display + ↓ ↓ + Two Log Files Live Position View + ↓ + Web Application + (Load both files) +``` + +### Critical Problem Solved +**Challenge**: How do battery-powered anchors transmit calibrated positions to PC? +**Solution**: Dual-file approach - Tag logs raw positioning data + anchor coordinates separately, webapp loads both files for offline processing + +--- + +## 🚀 Implementation Roadmap + +### Phase 1: Anchor Auto-Positioning System +**Duration**: 2-3 weeks + +#### 1.1 Distributed Positioning Algorithm +- [ ] **Anchor Discovery Protocol** + - All anchors broadcast discovery signals on startup + - Build neighbor discovery table for each anchor + - Implement range-based network topology mapping + +- [ ] **Distance Measurement Matrix** + - Each anchor measures distances to all neighbors in range + - Store distance measurements locally (EEPROM/flash) + - Handle partial connectivity (not all anchors can reach each other) + +- [ ] **Coordinate System Establishment** + - Designate anchor with most connections as origin (0,0) + - Establish coordinate system orientation + - Implement distributed position calculation algorithm + +- [ ] **Position Calculation & Storage** + - Each anchor calculates its own position using trilateration + - Store calculated position in local memory + - Implement position confidence scoring + +#### 1.2 Anchor Communication Protocol +- [ ] **Inter-Anchor Data Exchange** + - Protocol for sharing distance measurements + - Handle multi-hop communication for distant anchors + - Implement data consistency checks + +- [ ] **Position Refinement** + - Iterative position improvement algorithm + - Consensus mechanism for coordinate system alignment + - Error detection and correction + +### Phase 2: Tag Data Relay System +**Duration**: 2 weeks + +#### 2.1 Enhanced Tag Functionality +- [ ] **Anchor Discovery & Connection** + - Scan for available anchors + - Connect to 8 closest/strongest anchors + - Implement smooth anchor switching logic + +- [ ] **Position Data Collection** + - Request calibrated positions from connected anchors + - Aggregate anchor position data + - Handle missing or incomplete anchor data + +- [ ] **Real-time Positioning** + - Calculate tag position using 8 connected anchors + - Maintain position continuity during anchor handoffs + - Implement position smoothing/filtering + +#### 2.2 Tag Data Logging System +- [ ] **Dual-File Logging** + - **File 1**: Raw positioning data (distances, RSSI, timestamps) + - **File 2**: Anchor coordinates database (collected from connected anchors) + - USB transfer to PC for both files + +- [ ] **Data Collection Protocol** + - Request anchor coordinates: "Send me your calibrated position" + - Anchor responds: {anchor_id, x, y, calibration_confidence} + - Store coordinates locally and update anchor database file + - Continue logging raw positioning data as current system does + +### Phase 3: PC Software Development +**Duration**: 2 weeks + +#### 3.1 Real-time PC Application +- [ ] **USB Communication & Live Display** + - Receive real-time data stream from tag via USB + - Parse incoming data: raw distances + anchor coordinates + - Calculate live tag position using anchor coordinates + - Display real-time position on 2D map + +- [ ] **Live Monitoring Features** + - Show current tag position with live updates + - Display connected anchors and their positions + - Real-time signal strength indicators + - Live path tracking during mapping session + +#### 3.2 Dual-File Logging (Background) +- [ ] **Simultaneous Data Logging** + - **raw_positioning.csv**: Tag positioning data (distances, RSSI, timestamps) + - **anchor_coordinates.csv**: Anchor position database + - Log files generated automatically during real-time session + - Export files for webapp analysis after session + +- [ ] **Data Validation** + - Verify file integrity and format + - Check timestamp consistency + - Validate anchor coordinate data + +### Phase 4: Web Visualization Application +**Duration**: 2-3 weeks + +#### 4.1 Core Web Interface +- [ ] **Dual-File Upload & Processing** + - Upload **raw_positioning.csv** and **anchor_coordinates.csv** + - Parse and correlate both datasets + - Data validation and error handling + - Calculate actual tag positions using raw data + anchor coordinates + +- [ ] **2D Warehouse Visualization** + - Display anchor positions from coordinates file + - Calculate and plot tag path using positioning algorithm + - Interactive warehouse floor plan with scalable coordinate system + +#### 4.2 Path Analysis Features +- [ ] **Path Tracking Visualization** + - Tag movement path overlay + - Timeline scrubbing and playback + - Speed and direction indicators + +- [ ] **Data Analysis Tools** + - Path statistics and metrics + - Export functionality (images, reports) + - Comparison between multiple mapping sessions + +### Phase 5: System Integration & Optimization +**Duration**: 1-2 weeks + +#### 5.1 Quick Installation Workflow +- [ ] **Automated Setup Process** + - Power-on all anchors simultaneously + - Auto-discovery and network formation + - Position calibration and verification + - Tag pairing and PC connection setup + - Target: Ready-to-use in <15 minutes + +#### 5.2 System Validation +- [ ] **Accuracy Testing** + - Position accuracy validation + - Anchor auto-positioning verification + - End-to-end system testing + +- [ ] **Performance Optimization** + - Battery life optimization for anchors + - Data transmission efficiency + - Real-time performance tuning + +--- + +## 🎯 Key Technical Challenges + +### 1. Distributed Anchor Positioning +**Challenge**: Anchors must calculate positions without central coordination +**Solution**: Implement distributed trilateration with consensus mechanism + +### 2. Data Relay Through Tag +**Challenge**: Getting anchor position data to PC without direct connectivity +**Solution**: Tag acts as mobile bridge collecting and relaying data + +### 3. Coordinate System Consistency +**Challenge**: Ensuring all anchors use same coordinate system +**Solution**: Distributed coordinate system establishment protocol + +### 4. Anchor Handoff Management +**Challenge**: Smooth positioning during anchor switching +**Solution**: Position continuity algorithms and coordinate system alignment + +### 5. Partial Connectivity Handling +**Challenge**: Not all anchors can communicate directly +**Solution**: Multi-hop communication and distributed data sharing + +--- + +## 📦 Deliverables + +### Software Components +- [ ] **Enhanced Anchor Firmware** - Auto-positioning and data storage +- [ ] **Enhanced Tag Firmware** - Data relay and USB communication +- [ ] **PC Data Collection Software** - USB interface and logging +- [ ] **Web Visualization Application** - Path analysis and mapping + +### Documentation +- [ ] **Installation Guide** - Quick setup procedures +- [ ] **User Manual** - Operation and troubleshooting +- [ ] **Technical Documentation** - API and protocol specifications +- [ ] **Calibration Procedures** - System validation and accuracy testing + +### Test Results +- [ ] **Positioning Accuracy Report** - Performance metrics +- [ ] **Battery Life Analysis** - Power consumption data +- [ ] **Installation Time Study** - Setup procedure validation + +--- + +## 🔄 Success Criteria + +1. **Installation Time**: Complete system setup in <15 minutes +2. **Positioning Accuracy**: <30cm accuracy in warehouse environment +3. **Battery Life**: Anchors operate >8 hours on single charge +4. **System Reliability**: 99%+ uptime during mapping sessions +5. **Data Integrity**: Complete path tracking with <1% data loss +6. **User Experience**: Simple web interface for path visualization + + +*Last Updated: 2025-01-19* +*Version: 1.0* \ No newline at end of file diff --git a/docs/images/20250819_202527.jpg b/docs/images/20250819_202527.jpg new file mode 100644 index 0000000..b46e709 Binary files /dev/null and b/docs/images/20250819_202527.jpg differ diff --git a/docs/images/20250819_202629.jpg b/docs/images/20250819_202629.jpg new file mode 100644 index 0000000..c34d862 Binary files /dev/null and b/docs/images/20250819_202629.jpg differ diff --git a/Makerfabs UWB AT Module AT Command Manual(v1.1.1).pdf b/docs/manuals/Makerfabs UWB AT Module AT Command Manual(v1.1.1).pdf similarity index 100% rename from Makerfabs UWB AT Module AT Command Manual(v1.1.1).pdf rename to docs/manuals/Makerfabs UWB AT Module AT Command Manual(v1.1.1).pdf diff --git a/lib/UWBHelper/UWBHelper.cpp b/lib/UWBHelper/UWBHelper.cpp index 4b2d23e..e4e7fbe 100644 --- a/lib/UWBHelper/UWBHelper.cpp +++ b/lib/UWBHelper/UWBHelper.cpp @@ -1,32 +1,42 @@ #include "UWBHelper.h" +#include UWBHelper::UWBHelper(HardwareSerial* serial, int reset) { uwbSerial = serial; resetPin = reset; } -bool UWBHelper::begin() { - pinMode(resetPin, OUTPUT); - digitalWrite(resetPin, HIGH); - - uwbSerial->begin(115200); - delay(1000); - - // Test communication +// ===== BASIC COMMANDS (3.2-3.6) ===== + +bool UWBHelper::testConnection() { String response = sendCommand("AT?", 2000); return isResponseOK(response); } -void UWBHelper::reset() { - digitalWrite(resetPin, LOW); - delay(100); - digitalWrite(resetPin, HIGH); - delay(1000); +String UWBHelper::getVersion() { + return sendCommand("AT+GETVER?", 2000); } -bool UWBHelper::configureDevice(int deviceId, bool isAnchor, int dataRate, int rangeFilter) { +bool UWBHelper::restart() { + String response = sendCommand("AT+RESTART", 2000); + return isResponseOK(response); +} + +bool UWBHelper::restore() { + String response = sendCommand("AT+RESTORE", 2000); + return isResponseOK(response); +} + +bool UWBHelper::save() { + String response = sendCommand("AT+SAVE", 2000); + return isResponseOK(response); +} + +// ===== CONFIGURATION COMMANDS (3.7-3.8) ===== + +bool UWBHelper::setConfig(int deviceId, int role, int dataRate, int rangeFilter) { String cmd = "AT+SETCFG=" + String(deviceId) + "," + - String(isAnchor ? 1 : 0) + "," + + String(role) + "," + String(dataRate) + "," + String(rangeFilter); @@ -34,6 +44,24 @@ bool UWBHelper::configureDevice(int deviceId, bool isAnchor, int dataRate, int r return isResponseOK(response); } +String UWBHelper::getConfig() { + return sendCommand("AT+GETCFG?", 2000); +} + +// ===== ANTENNA COMMANDS (3.9-3.10) ===== + +bool UWBHelper::setAntennaDelay(int delay) { + String cmd = "AT+SETANT=" + String(delay); + String response = sendCommand(cmd, 2000); + return isResponseOK(response); +} + +String UWBHelper::getAntennaDelay() { + return sendCommand("AT+GETANT?", 2000); +} + +// ===== CAPACITY COMMANDS (3.11-3.12) ===== + bool UWBHelper::setCapacity(int tagCount, int timeSlot, int extMode) { String cmd = "AT+SETCAP=" + String(tagCount) + "," + String(timeSlot) + "," + @@ -43,28 +71,104 @@ bool UWBHelper::setCapacity(int tagCount, int timeSlot, int extMode) { return isResponseOK(response); } +String UWBHelper::getCapacity() { + return sendCommand("AT+GETCAP?", 2000); +} + +// ===== REPORTING COMMANDS (3.13-3.14) ===== + +bool UWBHelper::setReporting(bool enable) { + String cmd = "AT+SETRPT=" + String(enable ? 1 : 0); + String response = sendCommand(cmd, 2000); + return isResponseOK(response); +} + +String UWBHelper::getReporting() { + return sendCommand("AT+GETRPT?", 2000); +} + +// ===== SLEEP COMMAND (3.16) ===== + +bool UWBHelper::setSleep(int sleepTime) { + String cmd = "AT+SLEEP=" + String(sleepTime); + String response = sendCommand(cmd, 2000); + return isResponseOK(response); +} + +// ===== POWER COMMANDS (3.17-3.18) ===== + +bool UWBHelper::setPower(String powerValue) { + String cmd = "AT+SETPOW=" + powerValue; + String response = sendCommand(cmd, 2000); + return isResponseOK(response); +} + +String UWBHelper::getPower() { + return sendCommand("AT+GETPOW?", 2000); +} + +// ===== DATA COMMANDS (3.19-3.20) ===== + +bool UWBHelper::sendData(int length, String data) { + String cmd = "AT+DATA=" + String(length) + "," + data; + String response = sendCommand(cmd, 2000); + return isResponseOK(response); +} + +String UWBHelper::receiveData() { + return sendCommand("AT+RDATA", 2000); +} + +// ===== NETWORK COMMANDS (3.21-3.22) ===== + bool UWBHelper::setNetwork(int networkId) { String cmd = "AT+SETPAN=" + String(networkId); String response = sendCommand(cmd, 2000); return isResponseOK(response); } +String UWBHelper::getNetwork() { + return sendCommand("AT+GETPAN?", 2000); +} + +// ===== LEGACY WRAPPER FUNCTIONS ===== + +bool UWBHelper::begin() { + pinMode(resetPin, OUTPUT); + digitalWrite(resetPin, HIGH); + + uwbSerial->begin(115200); + delay(1000); + + // Test communication + return testConnection(); +} + +void UWBHelper::reset() { + digitalWrite(resetPin, LOW); + delay(100); + digitalWrite(resetPin, HIGH); + delay(1000); +} + +bool UWBHelper::configureDevice(int deviceId, bool isAnchor, int dataRate, int rangeFilter) { + return setConfig(deviceId, isAnchor ? 1 : 0, dataRate, rangeFilter); +} + bool UWBHelper::enableReporting(bool enable) { - String cmd = "AT+SETRPT=" + String(enable ? 1 : 0); - String response = sendCommand(cmd, 2000); - return isResponseOK(response); + return setReporting(enable); } bool UWBHelper::saveConfiguration() { - String response = sendCommand("AT+SAVE", 2000); - return isResponseOK(response); + return save(); } bool UWBHelper::restartDevice() { - String response = sendCommand("AT+RESTART", 2000); - return isResponseOK(response); + return restart(); } +// ===== COMMUNICATION ===== + String UWBHelper::sendCommand(String command, int timeout) { String response = ""; @@ -75,8 +179,16 @@ String UWBHelper::sendCommand(String command, int timeout) { while ((millis() - startTime) < timeout) { while (uwbSerial->available()) { char c = uwbSerial->read(); - response += c; + if (c == '\n' || c == '\r') { + if (response.length() > 0) { + Serial.println("Response: " + response); + return response; + } + } else { + response += c; + } } + delay(1); } if (response.length() > 0) { @@ -85,6 +197,8 @@ String UWBHelper::sendCommand(String command, int timeout) { return response; } +// ===== RANGE DATA PARSING ===== + bool UWBHelper::parseRangeData(String data, DeviceData devices[], int maxDevices) { if (!data.startsWith("AT+RANGE=")) { return false; @@ -108,7 +222,22 @@ bool UWBHelper::parseRangeData(String data, DeviceData devices[], int maxDevices if (rangeEnd == -1) return false; String rangeData = data.substring(rangeStart, rangeEnd); - float distance = rangeData.toFloat() / 100.0; // Convert cm to meters + + // 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 int rssiIndex = data.indexOf("rssi:("); @@ -137,10 +266,154 @@ bool UWBHelper::parseRangeData(String data, DeviceData devices[], int maxDevices return false; } -String UWBHelper::getVersion() { - return sendCommand("AT+GETVER?", 2000); +bool UWBHelper::parseDetailedRangeData(String data, RangeResult* result) { + if (!data.startsWith("AT+RANGE=")) { + return false; + } + + // Parse tid + int tidIndex = data.indexOf("tid:"); + if (tidIndex == -1) return false; + int commaPos = data.indexOf(',', tidIndex); + if (commaPos == -1) return false; + result->tagId = data.substring(tidIndex + 4, commaPos).toInt(); + + // Parse timer (if present) + int timerIndex = data.indexOf("timer:"); + if (timerIndex != -1) { + int timerComma = data.indexOf(',', timerIndex); + if (timerComma == -1) timerComma = data.indexOf(',', timerIndex); + result->timer = data.substring(timerIndex + 6, timerComma).toInt(); + } + + // Parse timerSys (if present) + int timerSysIndex = data.indexOf("timerSys:"); + if (timerSysIndex != -1) { + int timerSysComma = data.indexOf(',', timerSysIndex); + if (timerSysComma == -1) timerSysComma = data.indexOf(',', timerSysIndex); + result->timerSys = data.substring(timerSysIndex + 9, timerSysComma).toInt(); + } + + // Parse mask + int maskIndex = data.indexOf("mask:"); + if (maskIndex != -1) { + int maskComma = data.indexOf(',', maskIndex); + result->mask = data.substring(maskIndex + 5, maskComma).toInt(); + } + + // Parse sequence + int seqIndex = data.indexOf("seq:"); + if (seqIndex != -1) { + int seqComma = data.indexOf(',', seqIndex); + result->sequence = data.substring(seqIndex + 4, seqComma).toInt(); + } + + // Parse ranges + int rangeIndex = data.indexOf("range:("); + if (rangeIndex != -1) { + int rangeStart = rangeIndex + 7; + int rangeEnd = data.indexOf(')', rangeStart); + String rangeData = data.substring(rangeStart, rangeEnd); + + int startIdx = 0; + for (int i = 0; i < 8; i++) { + int commaIdx = rangeData.indexOf(',', startIdx); + if (commaIdx == -1) commaIdx = rangeData.length(); + + result->ranges[i] = rangeData.substring(startIdx, commaIdx).toFloat() / 100.0; + startIdx = commaIdx + 1; + if (startIdx >= rangeData.length()) break; + } + } + + // Parse RSSI + int rssiIndex = data.indexOf("rssi:("); + if (rssiIndex != -1) { + int rssiStart = rssiIndex + 6; + int rssiEnd = data.indexOf(')', rssiStart); + String rssiData = data.substring(rssiStart, rssiEnd); + + int startIdx = 0; + for (int i = 0; i < 8; i++) { + int commaIdx = rssiData.indexOf(',', startIdx); + if (commaIdx == -1) commaIdx = rssiData.length(); + + result->rssi[i] = rssiData.substring(startIdx, commaIdx).toFloat(); + startIdx = commaIdx + 1; + if (startIdx >= rssiData.length()) break; + } + } + + // Parse anchor IDs + int ancidIndex = data.indexOf("ancid:("); + if (ancidIndex != -1) { + int ancidStart = ancidIndex + 7; + int ancidEnd = data.indexOf(')', ancidStart); + String ancidData = data.substring(ancidStart, ancidEnd); + + int startIdx = 0; + for (int i = 0; i < 8; i++) { + int commaIdx = ancidData.indexOf(',', startIdx); + if (commaIdx == -1) commaIdx = ancidData.length(); + + result->anchorIds[i] = ancidData.substring(startIdx, commaIdx).toInt(); + startIdx = commaIdx + 1; + if (startIdx >= ancidData.length()) break; + } + } + + return true; } +// ===== ADVANCED FUNCTIONS ===== + +bool UWBHelper::requestAnchorPosition(int anchorId, AnchorPosition* position) { + // Custom command to request anchor position + String cmd = "AT+GETPOS=" + String(anchorId); + String response = sendCommand(cmd, 2000); + + if (response.indexOf("POS=") >= 0) { + // Parse response format: POS=id,x,y,confidence + int idStart = response.indexOf("=") + 1; + int comma1 = response.indexOf(",", idStart); + int comma2 = response.indexOf(",", comma1 + 1); + int comma3 = response.indexOf(",", comma2 + 1); + + if (comma1 > 0 && comma2 > 0 && comma3 > 0) { + position->anchorId = response.substring(idStart, comma1).toInt(); + position->x = response.substring(comma1 + 1, comma2).toFloat(); + position->y = response.substring(comma2 + 1, comma3).toFloat(); + position->confidence = response.substring(comma3 + 1).toFloat(); + position->valid = true; + return true; + } + } + + position->valid = false; + return false; +} + +bool UWBHelper::calculatePosition(DeviceData devices[], int deviceCount, float* x, float* y) { + // Simple trilateration with first 3 active devices + int activeCount = 0; + DeviceData activeDevices[3]; + + for (int i = 0; i < deviceCount && activeCount < 3; i++) { + if (devices[i].active) { + activeDevices[activeCount] = devices[i]; + activeCount++; + } + } + + if (activeCount < 3) return false; + + // For now, assume anchors are at fixed positions (would need anchor position data) + // This is a placeholder - real implementation would use actual anchor coordinates + return false; +} + +// ===== UTILITY FUNCTIONS ===== + bool UWBHelper::isResponseOK(String response) { return response.indexOf("OK") >= 0; } @@ -148,12 +421,16 @@ bool UWBHelper::isResponseOK(String response) { void UWBHelper::printDiagnostics() { Serial.println("=== UWB Diagnostics ==="); Serial.println("Version: " + getVersion()); - Serial.println("Config: " + sendCommand("AT+GETCFG?", 2000)); - Serial.println("Capacity: " + sendCommand("AT+GETCAP?", 2000)); - Serial.println("Power: " + sendCommand("AT+GETPOW?", 2000)); + Serial.println("Config: " + getConfig()); + Serial.println("Capacity: " + getCapacity()); + Serial.println("Antenna: " + getAntennaDelay()); + Serial.println("Power: " + getPower()); + Serial.println("Network: " + getNetwork()); + Serial.println("Reporting: " + getReporting()); } -// DistanceFilter Implementation +// ===== DISTANCE FILTER IMPLEMENTATION ===== + DistanceFilter::DistanceFilter() : index(0), filled(false) { for (int i = 0; i < FILTER_SIZE; i++) readings[i] = 0; } @@ -195,4 +472,71 @@ void DistanceFilter::reset() { index = 0; filled = false; for (int i = 0; i < FILTER_SIZE; i++) readings[i] = 0; +} + +// ===== POSITION CALCULATOR IMPLEMENTATION ===== + +bool PositionCalculator::trilaterate(float x1, float y1, float r1, + float x2, float y2, float r2, + float x3, float y3, float r3, + float* x, float* y) { + // Trilateration algorithm + float A = 2 * (x2 - x1); + float B = 2 * (y2 - y1); + float C = pow(r1, 2) - pow(r2, 2) - pow(x1, 2) + pow(x2, 2) - pow(y1, 2) + pow(y2, 2); + float D = 2 * (x3 - x2); + float E = 2 * (y3 - y2); + float F = pow(r2, 2) - pow(r3, 2) - pow(x2, 2) + pow(x3, 2) - pow(y2, 2) + pow(y3, 2); + + float denominator = A * E - B * D; + if (abs(denominator) < 0.001) return false; // Points are collinear + + *x = (C * E - F * B) / denominator; + *y = (A * F - D * C) / denominator; + + return true; +} + +bool PositionCalculator::multilaterate(AnchorPosition anchors[], float distances[], int count, float* x, float* y) { + if (count < 3) return false; + + // Use least squares method for more than 3 anchors + if (count == 3) { + return trilaterate(anchors[0].x, anchors[0].y, distances[0], + anchors[1].x, anchors[1].y, distances[1], + anchors[2].x, anchors[2].y, distances[2], + x, y); + } + + // For more than 3 anchors, use weighted least squares (simplified version) + float sumX = 0, sumY = 0, sumW = 0; + + for (int i = 0; i < count - 1; i++) { + for (int j = i + 1; j < count; j++) { + for (int k = j + 1; k < count; k++) { + float tempX, tempY; + if (trilaterate(anchors[i].x, anchors[i].y, distances[i], + anchors[j].x, anchors[j].y, distances[j], + anchors[k].x, anchors[k].y, distances[k], + &tempX, &tempY)) { + float weight = anchors[i].confidence * anchors[j].confidence * anchors[k].confidence; + sumX += tempX * weight; + sumY += tempY * weight; + sumW += weight; + } + } + } + } + + if (sumW > 0) { + *x = sumX / sumW; + *y = sumY / sumW; + return true; + } + + return false; +} + +float PositionCalculator::calculateDistance(float x1, float y1, float x2, float y2) { + return sqrt(pow(x2 - x1, 2) + pow(y2 - y1, 2)); } \ No newline at end of file diff --git a/lib/UWBHelper/UWBHelper.h b/lib/UWBHelper/UWBHelper.h index f02fabd..8de2f4d 100644 --- a/lib/UWBHelper/UWBHelper.h +++ b/lib/UWBHelper/UWBHelper.h @@ -12,6 +12,25 @@ struct DeviceData { bool active; }; +struct AnchorPosition { + int anchorId; + float x; + float y; + float confidence; + bool valid; +}; + +struct RangeResult { + int tagId; + int mask; + int sequence; + float ranges[8]; + float rssi[8]; + int anchorIds[8]; + unsigned long timer; + unsigned long timerSys; +}; + class UWBHelper { private: HardwareSerial* uwbSerial; @@ -20,26 +39,68 @@ private: public: UWBHelper(HardwareSerial* serial, int reset); - // Initialization + // Basic Commands (3.2-3.6) + bool testConnection(); // AT? + String getVersion(); // AT+GETVER? + bool restart(); // AT+RESTART + bool restore(); // AT+RESTORE + bool save(); // AT+SAVE + + // Configuration Commands (3.7-3.8) + bool setConfig(int deviceId, int role, int dataRate = 1, int rangeFilter = 1); // AT+SETCFG + String getConfig(); // AT+GETCFG? + + // Antenna Commands (3.9-3.10) + bool setAntennaDelay(int delay); // AT+SETANT + String getAntennaDelay(); // AT+GETANT? + + // Capacity Commands (3.11-3.12) + bool setCapacity(int tagCount, int timeSlot = 10, int extMode = 0); // AT+SETCAP + String getCapacity(); // AT+GETCAP? + + // Reporting Commands (3.13-3.14) + bool setReporting(bool enable); // AT+SETRPT + String getReporting(); // AT+GETRPT? + + // Range Command (3.15) + bool parseRangeData(String data, DeviceData devices[], int maxDevices); + bool parseDetailedRangeData(String data, RangeResult* result); + + // Sleep Command (3.16) + bool setSleep(int sleepTime); // AT+SLEEP + + // Power Commands (3.17-3.18) + bool setPower(String powerValue = "FD"); // AT+SETPOW + String getPower(); // AT+GETPOW? + + // Data Commands (3.19-3.20) + bool sendData(int length, String data); // AT+DATA + String receiveData(); // AT+RDATA + + // Network Commands (3.21-3.22) + bool setNetwork(int networkId); // AT+SETPAN + String getNetwork(); // AT+GETPAN? + + // Legacy wrapper functions for backward compatibility bool begin(); void reset(); - - // Configuration bool configureDevice(int deviceId, bool isAnchor, int dataRate = 1, int rangeFilter = 1); - bool setCapacity(int tagCount, int timeSlot = 10, int extMode = 1); - bool setNetwork(int networkId); bool enableReporting(bool enable); bool saveConfiguration(); bool restartDevice(); // Communication String sendCommand(String command, int timeout = 2000); - bool parseRangeData(String data, DeviceData devices[], int maxDevices); + + // Advanced parsing + bool requestAnchorPosition(int anchorId, AnchorPosition* position); // Utility - String getVersion(); bool isResponseOK(String response); void printDiagnostics(); + + // Position calculation helpers + bool calculatePosition(DeviceData devices[], int deviceCount, float* x, float* y); }; // Data filtering class @@ -57,4 +118,16 @@ public: void reset(); }; +// Position calculation class +class PositionCalculator { +public: + static bool trilaterate(float x1, float y1, float r1, + float x2, float y2, float r2, + float x3, float y3, float r3, + float* x, float* y); + + static bool multilaterate(AnchorPosition anchors[], float distances[], int count, float* x, float* y); + static float calculateDistance(float x1, float y1, float x2, float y2); +}; + #endif // UWBHELPER_H \ No newline at end of file