Unify firmware, add config system
This commit is contained in:
martin
parent
8a7936280b
commit
49e3230610
11 changed files with 589 additions and 387 deletions
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;
|
||||
}
|
||||
}
|
||||
Loading…
Add table
Add a link
Reference in a new issue