Reorganize project structure and create development roadmap
- Move documentation to organized docs/ directory structure - Add dev notes - Create comprehensive 5-phase roadmap for indoor positioning system - Move AT command manual and hardware images to docs/ - Update README with hardware links and project overview - Remove sleep mode and OTA functionality for simplification - Clean up project structure for production development
This commit is contained in:
martin
parent
ea370ac702
commit
e7c8fad272
8 changed files with 884 additions and 47 deletions
106
CLAUDE.md
Normal file
106
CLAUDE.md
Normal file
|
|
@ -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
|
||||
79
README.md
79
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.
|
||||
|
||||
|
||||
|
||||
## 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)
|
||||
|
||||
|
||||
**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
|
||||
|
|
|
|||
255
docs/ROADMAP.md
Normal file
255
docs/ROADMAP.md
Normal file
|
|
@ -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*
|
||||
BIN
docs/images/20250819_202527.jpg
Normal file
BIN
docs/images/20250819_202527.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 622 KiB |
BIN
docs/images/20250819_202629.jpg
Normal file
BIN
docs/images/20250819_202629.jpg
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 951 KiB |
|
|
@ -1,32 +1,42 @@
|
|||
#include "UWBHelper.h"
|
||||
#include <math.h>
|
||||
|
||||
UWBHelper::UWBHelper(HardwareSerial* serial, int reset) {
|
||||
uwbSerial = serial;
|
||||
resetPin = reset;
|
||||
}
|
||||
|
||||
bool UWBHelper::begin() {
|
||||
pinMode(resetPin, OUTPUT);
|
||||
digitalWrite(resetPin, HIGH);
|
||||
// ===== BASIC COMMANDS (3.2-3.6) =====
|
||||
|
||||
uwbSerial->begin(115200);
|
||||
delay(1000);
|
||||
|
||||
// Test communication
|
||||
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,9 +179,17 @@ String UWBHelper::sendCommand(String command, int timeout) {
|
|||
while ((millis() - startTime) < timeout) {
|
||||
while (uwbSerial->available()) {
|
||||
char c = uwbSerial->read();
|
||||
if (c == '\n' || c == '\r') {
|
||||
if (response.length() > 0) {
|
||||
Serial.println("Response: " + response);
|
||||
return response;
|
||||
}
|
||||
} else {
|
||||
response += c;
|
||||
}
|
||||
}
|
||||
delay(1);
|
||||
}
|
||||
|
||||
if (response.length() > 0) {
|
||||
Serial.println("Response: " + response);
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -196,3 +473,70 @@ void DistanceFilter::reset() {
|
|||
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));
|
||||
}
|
||||
|
|
@ -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
|
||||
Loading…
Add table
Add a link
Reference in a new issue