Refactor to unified firmware architecture with modular configuration system
BREAKING CHANGE: Replace separate anchor/tag firmware with single configurable codebase ## Major Changes - **Unified Firmware**: Single main.cpp replaces main_anchor.cpp and main_tag.cpp - **Modular Config**: Organized configuration system in src/config/ directory - **Dynamic Behavior**: Device behavior determined by build flags at compile time - **Simplified Builds**: Reduced from 8+ environments to 3 core environments ## New Architecture - src/main.cpp - Single firmware with config-driven behavior - src/config/device_config.h - Device role detection and unified behavior - src/config/anchor_config.h - Anchor-specific settings (positioning, communication) - src/config/tag_config.h - Tag-specific settings (USB streaming, coordinate collection) - src/config/positioning_config.h - Distributed positioning algorithm framework ## Build System Improvements - Simplified platformio.ini (anchor/tag/debug environments) - Support for custom device ID and network via PLATFORMIO_BUILD_FLAGS - Same firmware for all devices, behavior configured at compile time - Easy device configuration: set PLATFORMIO_BUILD_FLAGS=-DUWB_INDEX=5 -DNETWORK_ID=2000 ## Technical Benefits - Reduced code duplication from ~350 lines to unified ~200 lines - Consistent behavior across device types - Easier maintenance with single codebase - Framework ready for distributed positioning algorithm implementation - Modular configuration system for future algorithm expansion ## Positioning Framework Ready - Anchor discovery protocol framework established - Inter-anchor communication configuration ready - Position calculation algorithm structure in place - Coordinate storage and validation system configured - Tag coordinate collection and USB streaming framework prepared ## Testing - ✅ Anchor build successful (325KB flash usage) - ✅ Tag build successful (325KB flash usage) - ✅ Custom device ID/network configuration working - ✅ UWB initialization sequence fixed for active networks
This commit is contained in:
martin
parent
fafc2d5830
commit
a2e404ec0f
11 changed files with 589 additions and 387 deletions
57
CLAUDE.md
57
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.
|
**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
|
## Development Commands
|
||||||
|
|
||||||
### Build & Flash
|
### Build & Flash (Unified Firmware)
|
||||||
```bash
|
```bash
|
||||||
# Build specific device type
|
# Build default configurations
|
||||||
pio run -e anchor # Build anchor firmware
|
pio run -e anchor # Build anchor (ID=0, Network=1234)
|
||||||
pio run -e tag # Build tag firmware
|
pio run -e tag # Build tag (ID=1, Network=1234)
|
||||||
pio run -e tag2 # Build second tag (ID 2)
|
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
|
# Flash to device
|
||||||
pio run -e tag -t upload
|
pio run -e tag -t upload
|
||||||
|
|
@ -26,10 +32,16 @@ pio device monitor
|
||||||
```
|
```
|
||||||
|
|
||||||
### Development Environments
|
### Development Environments
|
||||||
- **anchor**: Base station (UWB_INDEX=0, tracks tags)
|
- **anchor**: Base station with distributed positioning (default: ID=0, Network=1234)
|
||||||
- **tag**: Mobile tracker (UWB_INDEX=1, reports to anchors)
|
- **tag**: Mobile tracker with coordinate collection (default: ID=1, Network=1234)
|
||||||
- **tag2**: Second mobile tracker (UWB_INDEX=2)
|
- **debug**: Development anchor build with DEBUG_ENABLED=1
|
||||||
- **debug**: Development 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
|
## Core Architecture
|
||||||
|
|
||||||
|
|
@ -41,10 +53,11 @@ Tags: Range to anchors <20> Collect coordinates <20> USB to PC <20> Real-time + log
|
||||||
|
|
||||||
### Key Technical Patterns
|
### Key Technical Patterns
|
||||||
|
|
||||||
**1. Dual Firmware Architecture**
|
**1. Unified Firmware Architecture**
|
||||||
- `main_anchor.cpp`: Tracks tags, manages UWB_TAG_COUNT devices, displays active tags
|
- `src/main.cpp`: Single firmware for all devices, behavior determined by build flags
|
||||||
- `main_tag.cpp`: Connects to MAX_ANCHORS, reports to PC via USB, displays anchor distances
|
- `src/config/`: Modular configuration system with device-specific settings
|
||||||
- Shared: config.h for hardware pins, UWBHelper library for AT commands
|
- 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**
|
**2. UWBHelper Library Structure**
|
||||||
- **Complete AT Command Implementation**: All 21 commands from AT manual
|
- **Complete AT Command Implementation**: All 21 commands from AT manual
|
||||||
|
|
@ -75,6 +88,13 @@ Tags: Range to anchors <20> Collect coordinates <20> USB to PC <20> Real-time + log
|
||||||
|
|
||||||
**Timeout Management**: Devices marked inactive after DEVICE_TIMEOUT (5000ms) with no updates
|
**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
|
## Configuration Constants
|
||||||
|
|
||||||
Key values in config.h:
|
Key values in config.h:
|
||||||
|
|
@ -87,11 +107,18 @@ Key values in config.h:
|
||||||
## Development Roadmap Context
|
## Development Roadmap Context
|
||||||
|
|
||||||
This ESP32 firmware implements the hardware foundation for the UWB positioning system (see docs/ROADMAP.md):
|
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
|
- **Current**: Unified firmware architecture with config-driven behavior, ready for distributed positioning implementation
|
||||||
- **Next**: Anchor auto-positioning, coordinate relay through tag, battery optimization
|
- **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
|
- **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
|
- **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
|
## Hardware Requirements
|
||||||
|
|
||||||
- **Modules**: Makerfabs MaUWB ESP32-S3 with built-in UWB + OLED
|
- **Modules**: Makerfabs MaUWB ESP32-S3 with built-in UWB + OLED
|
||||||
|
|
|
||||||
33
README.md
33
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
|
**Hardware:** [Makerfabs MaUWB ESP32-S3 UWB Module](https://www.makerfabs.com/mauwb-esp32s3-uwb-module.html) with SSD1306 OLED displays
|
||||||
|
|
||||||
## Environments
|
## Unified Architecture
|
||||||
- `anchor`: Base station for positioning
|
|
||||||
- `tag`: Mobile device for tracking (ID 1)
|
**Single Firmware** - All devices use the same `main.cpp` with config-driven behavior:
|
||||||
- `tag2`: Mobile device for tracking (ID 2)
|
- `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
|
## Build & Upload
|
||||||
```bash
|
```bash
|
||||||
# Build specific environment
|
# Build with default settings
|
||||||
pio run -e anchor
|
pio run -e anchor # Anchor ID=0, Network=1234
|
||||||
pio run -e tag
|
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
|
# Upload to device
|
||||||
pio run -e tag -t upload
|
pio run -e tag -t upload
|
||||||
|
|
@ -37,6 +43,19 @@ pio run -e tag -t upload
|
||||||
pio device monitor
|
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
|
## AT Command Support
|
||||||
|
|
||||||
Complete implementation of all AT commands from the official manual:
|
Complete implementation of all AT commands from the official manual:
|
||||||
|
|
|
||||||
|
|
@ -25,36 +25,27 @@ lib_deps =
|
||||||
adafruit/Adafruit BusIO@^1.14.4
|
adafruit/Adafruit BusIO@^1.14.4
|
||||||
bblanchon/ArduinoJson@^6.21.3
|
bblanchon/ArduinoJson@^6.21.3
|
||||||
|
|
||||||
; Anchor device configuration
|
; Unified Anchor configuration
|
||||||
[env:anchor]
|
[env:anchor]
|
||||||
build_src_filter = +<*> -<main_tag.cpp>
|
build_src_filter = +<*> -<main_anchor.cpp> -<main_tag.cpp>
|
||||||
build_flags =
|
build_flags =
|
||||||
${env.build_flags}
|
${env.build_flags}
|
||||||
-DDEVICE_TYPE_ANCHOR=1
|
-DDEVICE_TYPE_ANCHOR=1
|
||||||
-DUWB_INDEX=0
|
-DUWB_INDEX=0
|
||||||
-DNETWORK_ID=1234
|
-DNETWORK_ID=1234
|
||||||
|
|
||||||
; Tag device configuration
|
; Unified Tag configuration
|
||||||
[env:tag]
|
[env:tag]
|
||||||
build_src_filter = +<*> -<main_anchor.cpp>
|
build_src_filter = +<*> -<main_anchor.cpp> -<main_tag.cpp>
|
||||||
build_flags =
|
build_flags =
|
||||||
${env.build_flags}
|
${env.build_flags}
|
||||||
-DDEVICE_TYPE_TAG=1
|
-DDEVICE_TYPE_TAG=1
|
||||||
-DUWB_INDEX=1
|
-DUWB_INDEX=1
|
||||||
-DNETWORK_ID=1234
|
-DNETWORK_ID=1234
|
||||||
|
|
||||||
; Tag device 2 (copy and modify UWB_INDEX for multiple tags)
|
|
||||||
[env:tag2]
|
|
||||||
build_src_filter = +<*> -<main_anchor.cpp>
|
|
||||||
build_flags =
|
|
||||||
${env.build_flags}
|
|
||||||
-DDEVICE_TYPE_TAG=1
|
|
||||||
-DUWB_INDEX=2
|
|
||||||
-DNETWORK_ID=1234
|
|
||||||
|
|
||||||
; Development environment with debugging
|
; Development environment with debugging
|
||||||
[env:debug]
|
[env:debug]
|
||||||
build_src_filter = +<*> -<main_tag.cpp>
|
build_src_filter = +<*> -<main_anchor.cpp> -<main_tag.cpp>
|
||||||
build_flags =
|
build_flags =
|
||||||
${env.build_flags}
|
${env.build_flags}
|
||||||
-DDEVICE_TYPE_ANCHOR=1
|
-DDEVICE_TYPE_ANCHOR=1
|
||||||
|
|
|
||||||
39
src/config/anchor_config.h
Normal file
39
src/config/anchor_config.h
Normal file
|
|
@ -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
|
||||||
62
src/config/device_config.h
Normal file
62
src/config/device_config.h
Normal file
|
|
@ -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
|
||||||
54
src/config/positioning_config.h
Normal file
54
src/config/positioning_config.h
Normal file
|
|
@ -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
|
||||||
45
src/config/tag_config.h
Normal file
45
src/config/tag_config.h
Normal file
|
|
@ -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
|
||||||
316
src/main.cpp
Normal file
316
src/main.cpp
Normal file
|
|
@ -0,0 +1,316 @@
|
||||||
|
#include <Arduino.h>
|
||||||
|
#include <Wire.h>
|
||||||
|
#include <Adafruit_GFX.h>
|
||||||
|
#include <Adafruit_SSD1306.h>
|
||||||
|
#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;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
@ -1,183 +0,0 @@
|
||||||
#include <Arduino.h>
|
|
||||||
#include <Wire.h>
|
|
||||||
#include <Adafruit_GFX.h>
|
|
||||||
#include <Adafruit_SSD1306.h>
|
|
||||||
#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);
|
|
||||||
}
|
|
||||||
|
|
||||||
168
src/main_tag.cpp
168
src/main_tag.cpp
|
|
@ -1,168 +0,0 @@
|
||||||
#include <Arduino.h>
|
|
||||||
#include <Wire.h>
|
|
||||||
#include <Adafruit_GFX.h>
|
|
||||||
#include <Adafruit_SSD1306.h>
|
|
||||||
#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);
|
|
||||||
}
|
|
||||||
Loading…
Add table
Add a link
Reference in a new issue