Add some ESPNow debug

src/GlobalVars.h

@@ -27,12 +27,14 @@
 #include <arduino-timer.h>
 
 #include "LEDManager.h"
+#include "ResetCounter.h"
 #include "configuration/Configuration.h"
 #include "network/connection.h"
 #include "network/manager.h"
 #include "sensors/SensorManager.h"
 #include "status/StatusManager.h"
 #include "batterymonitor.h"
+#include "network/espnowconnection.h"
 
 extern Timer<> globalTimer;
 extern SlimeVR::LEDManager ledManager;
@@ -41,6 +43,8 @@ extern SlimeVR::Configuration::Configuration configuration;
 extern SlimeVR::Sensors::SensorManager sensorManager;
 extern SlimeVR::Network::Manager networkManager;
 extern SlimeVR::Network::Connection networkConnection;
+extern SlimeVR::Network::ESPNowConnection espnowConnection;
 extern BatteryMonitor battery;
+extern SlimeVR::ResetCounter resetCounter;
 
 #endif

src/ResetCounter.cpp

@@ -0,0 +1,143 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2024 Gorbit99 & SlimeVR Contributors
+
+	Permission is hereby granted, free of charge, to any person obtaining a copy
+	of this software and associated documentation files (the "Software"), to deal
+	in the Software without restriction, including without limitation the rights
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	copies of the Software, and to permit persons to whom the Software is
+	furnished to do so, subject to the following conditions:
+
+	The above copyright notice and this permission notice shall be included in
+	all copies or substantial portions of the Software.
+
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+	THE SOFTWARE.
+*/
+
+#include "ResetCounter.h"
+
+#include "GlobalVars.h"
+
+namespace SlimeVR {
+
+#if ESP32
+void resetDelayTimerCallback(void *) {
+    resetCounter.signalResetDelay();
+}
+
+void resetTimeoutTimerCallback(void *) {
+    resetCounter.signalResetTimeout();
+}
+#endif
+
+void ResetCounter::setup() {
+#if ESP32
+
+    if (esp_timer_early_init() != ESP_OK) {
+        m_Logger.error("Couldn't initialize timer!");
+        return;
+    }
+
+    esp_timer_create_args_t delayArgs{
+        .callback = resetDelayTimerCallback,
+        .arg = nullptr,
+        .dispatch_method = ESP_TIMER_TASK,
+        .name = "RESET COUNTER DELAY",
+        .skip_unhandled_events = true,
+    };
+
+    if (esp_timer_create(&delayArgs, &delayTimerHandle) != ESP_OK) {
+        m_Logger.error("Couldn't create delay timer!");
+        return;
+    }
+    if (esp_timer_start_once(delayTimerHandle, resetDelaySeconds * 1e6) != ESP_OK) {
+        m_Logger.error("Couldn't start delay timer!");
+        return;
+    }
+
+    esp_timer_create_args_t timeoutArgs{
+        .callback = resetTimeoutTimerCallback,
+        .arg = nullptr,
+        .dispatch_method = ESP_TIMER_TASK,
+        .name = "RESET COUNTER TIMEOUT",
+        .skip_unhandled_events = true,
+    };
+
+    if (esp_timer_create(&timeoutArgs, &timeoutTimerHandle) != ESP_OK) {
+        m_Logger.error("Couldn't create timeout timer!");
+        return;
+    }
+    if (esp_timer_start_once(timeoutTimerHandle, resetTimeoutSeconds * 1e6) != ESP_OK) {
+        m_Logger.error("Couldn't start timeout timer!");
+        return;
+    }
+
+#elif ESP8266
+
+	timerStartMillis = millis();
+	signalResetDelay();
+
+#endif
+}
+
+void ResetCounter::update() {
+#if ESP8266
+
+	if (timeoutElapsed) {
+		return;
+	}
+
+	uint32_t elapsedMillis = millis() - timerStartMillis;
+
+	if (elapsedMillis < resetTimeoutSeconds * 1e3) {
+		return;
+	}
+
+	signalResetTimeout();
+	timeoutElapsed = true;
+
+#endif
+}
+
+void ResetCounter::signalResetDelay() {
+    if (!configuration.loadResetCount(&resetCount)) {
+        resetCount = 0;
+    }
+    resetCount++;
+    configuration.saveResetCount(resetCount);
+
+#if ESP32
+    esp_timer_delete(delayTimerHandle);
+#endif
+}
+
+void ResetCounter::signalResetTimeout() {
+    configuration.saveResetCount(0);
+
+	if (resetCount > 1) {
+		invokeResetCountCallbacks();
+	}
+
+#if ESP32
+    esp_timer_delete(timeoutTimerHandle);
+#endif
+}
+
+void ResetCounter::onResetCount(std::function<void(uint32_t)> callback) {
+    resetCountCallbacks.push_back(callback);
+}
+
+void ResetCounter::invokeResetCountCallbacks() {
+    for (auto &callback : resetCountCallbacks) {
+        callback(resetCount);
+    }
+}
+
+} // namespace SlimeVR

src/ResetCounter.h

@@ -0,0 +1,72 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2024 Gorbit99 & SlimeVR Contributors
+
+	Permission is hereby granted, free of charge, to any person obtaining a copy
+	of this software and associated documentation files (the "Software"), to deal
+	in the Software without restriction, including without limitation the rights
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	copies of the Software, and to permit persons to whom the Software is
+	furnished to do so, subject to the following conditions:
+
+	The above copyright notice and this permission notice shall be included in
+	all copies or substantial portions of the Software.
+
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+	THE SOFTWARE.
+*/
+
+#pragma once
+
+#include <cstdint>
+#include <functional>
+#include <vector>
+
+#include "./logging/Logger.h"
+
+#ifdef ESP32
+#include <esp_timer.h>
+#include <esp_system.h>
+#endif
+
+namespace SlimeVR {
+
+class ResetCounter {
+public:
+	void setup();
+	void update();
+	void onResetCount(std::function<void(uint32_t)> callback);
+
+private:
+	void signalResetDelay();
+	void signalResetTimeout();
+	void invokeResetCountCallbacks();
+
+	std::vector<std::function<void(uint32_t)>> resetCountCallbacks;
+	uint32_t resetCount = 0;
+
+#if ESP32
+	esp_timer_handle_t delayTimerHandle;
+	esp_timer_handle_t timeoutTimerHandle;
+#elif ESP8266
+	uint32_t timerStartMillis;
+	bool timeoutElapsed = false;
+#endif
+
+	Logging::Logger m_Logger = Logging::Logger("ESPNowConnection");
+
+	static constexpr float resetDelaySeconds = 0.05f;
+	static constexpr float resetTimeoutSeconds = 3.0f;
+
+#if ESP32
+	friend void resetDelayTimerCallback(void*);
+	friend void resetTimeoutTimerCallback(void*);
+#endif
+};
+
+}  // namespace SlimeVR

src/batterymonitor.cpp

@@ -119,7 +119,9 @@ void BatteryMonitor::Loop()
                     level = 1;
                 else if (level < 0)
                     level = 0;
+                #ifndef USE_ESPNOW_COMMUNICATION
                 networkConnection.sendBatteryLevel(voltage, level);
+                #endif
                 #ifdef BATTERY_LOW_POWER_VOLTAGE
                     if (voltage < BATTERY_LOW_POWER_VOLTAGE)
                     {

src/configuration/Configuration.cpp

@@ -242,6 +242,68 @@ namespace SlimeVR {
             return true;
         }
 
+		bool Configuration::loadDongleConnection(uint8_t outMacAddress[6], uint8_t &outTrackerId) {
+			char path[] = "/dongleMac.bin";
+
+			if (!LittleFS.exists(path)) {
+				return false;
+			}
+
+			auto f = SlimeVR::Utils::openFile(path, "r");
+			if (f.isDirectory()) {
+				return false;
+			}
+
+			f.read(outMacAddress, sizeof(uint8_t[6]));
+			f.read(&outTrackerId, sizeof(uint8_t));
+			return true;
+		}
+
+		bool Configuration::saveDongleConnection(const uint8_t macAddress[6], uint8_t trackerId) {
+            char path[] = "/dongleMac.bin";
+
+            m_Logger.trace("Saving dongle mac address");
+
+            File file = LittleFS.open(path, "w");
+            file.write(macAddress, sizeof(uint8_t[6]));
+            file.write(&trackerId, sizeof(uint8_t));
+            file.close();
+
+            m_Logger.debug("Saved dongle mac address");
+            return true;
+		}
+
+		bool Configuration::loadResetCount(uint32_t *outResetCount) {
+			char path[] = "/resetCount.bin";
+
+			if (!LittleFS.exists(path)) {
+				return false;
+			}
+
+			auto f = SlimeVR::Utils::openFile(path, "r");
+			if (f.isDirectory()) {
+				return false;
+			}
+
+			f.read(reinterpret_cast<uint8_t *>(outResetCount), sizeof(uint32_t));
+			return true;
+		}
+
+		bool Configuration::saveResetCount(uint32_t resetCount) {
+            char path[] = "/resetCount.bin";
+
+			// Probably not a smart idea to log this always
+            // m_Logger.trace("Saving reset count");
+
+            auto file = LittleFS.open(path, "w");
+            file.write(reinterpret_cast<uint8_t *>(&resetCount), sizeof(uint32_t));
+            file.close();
+
+			// Probably not a smart idea to log this always
+            // m_Logger.debug("Saved reset count");
+            return true;
+		}
+
         bool Configuration::runMigrations(int32_t version) {
             return true;
         }

src/configuration/Configuration.h

@@ -29,6 +29,7 @@
 #include "DeviceConfig.h"
 #include "logging/Logger.h"
 #include "../motionprocessing/GyroTemperatureCalibrator.h"
+#include "../globals.h"
 
 namespace SlimeVR {
     namespace Configuration {
@@ -50,6 +51,12 @@ namespace SlimeVR {
             bool loadTemperatureCalibration(uint8_t sensorId, GyroTemperatureCalibrationConfig& config);
             bool saveTemperatureCalibration(uint8_t sensorId, const GyroTemperatureCalibrationConfig& config);
 
+            bool loadDongleConnection(uint8_t outMacAddress[6], uint8_t &outTrackerId);
+            bool saveDongleConnection(const uint8_t macAddress[6], uint8_t trackerId);
+            
+            bool loadResetCount(uint32_t *outResetCount);
+            bool saveResetCount(uint32_t resetCount);
+
         private:
             void loadSensors();
             bool runMigrations(int32_t version);

src/debug.h

@@ -90,6 +90,6 @@
 #define ENABLE_INSPECTION false
 
 #define PROTOCOL_VERSION 18
-#define FIRMWARE_VERSION "0.5.0"
+#define FIRMWARE_VERSION "0.5.0-espnow"
 
 #endif // SLIMEVR_DEBUG_H_

src/defines.h

@@ -25,6 +25,8 @@
 // https://docs.slimevr.dev/firmware/configuring-project.html#2-configuring-definesh
 // ================================================
 
+#define USE_ESPNOW_COMMUNICATION true
+
 // Set parameters of IMU and board used
 #define IMU IMU_BNO085
 #define SECOND_IMU IMU

src/main.cpp

@@ -21,6 +21,7 @@
     THE SOFTWARE.
 */
 
+#include "ResetCounter.h"
 #include "Wire.h"
 #include "ota.h"
 #include "GlobalVars.h"
@@ -31,14 +32,22 @@
 #include "batterymonitor.h"
 #include "logging/Logger.h"
 
+#ifdef USE_ESPNOW_COMMUNICATION
+#include "network/espnowconnection.h"
+#endif
+
 Timer<> globalTimer;
 SlimeVR::Logging::Logger logger("SlimeVR");
 SlimeVR::Sensors::SensorManager sensorManager;
 SlimeVR::LEDManager ledManager(LED_PIN);
 SlimeVR::Status::StatusManager statusManager;
 SlimeVR::Configuration::Configuration configuration;
+#ifndef USE_ESPNOW_COMMUNICATION
 SlimeVR::Network::Manager networkManager;
 SlimeVR::Network::Connection networkConnection;
+#else
+SlimeVR::Network::ESPNowConnection espnowConnection;
+#endif
 
 int sensorToCalibrate = -1;
 bool blinking = false;
@@ -48,17 +57,29 @@ unsigned long lastStatePrint = 0;
 bool secondImuActive = false;
 BatteryMonitor battery;
 
+SlimeVR::ResetCounter resetCounter;
+
 void setup()
 {
     Serial.begin(serialBaudRate);
-    globalTimer = timer_create_default();
+    configuration.setup();
+	resetCounter.setup();
+#ifdef USE_ESPNOW_COMMUNICATION
+	resetCounter.onResetCount([&](uint8_t resetCount){
+		if (resetCount == 3) {
+			espnowConnection.broadcastPairingRequest();
+		}
+	});
+#endif
 
 #ifdef ESP32C3
     // Wait for the Computer to be able to connect.
-    delay(2000);
+    // delay(2000);
 #endif
 
-    Serial.println();
+    globalTimer = timer_create_default();
+
+	Serial.println();
     Serial.println();
     Serial.println();
 
@@ -67,7 +88,6 @@ void setup()
     statusManager.setStatus(SlimeVR::Status::LOADING, true);
 
     ledManager.setup();
-    configuration.setup();
 
     SerialCommands::setUp();
 
@@ -98,8 +118,12 @@ void setup()
 
     sensorManager.setup();
 
+#ifndef USE_ESPNOW_COMMUNICATION
     networkManager.setup();
     OTA::otaSetup(otaPassword);
+#else
+	espnowConnection.setup();
+#endif
     battery.Setup();
 
     statusManager.setStatus(SlimeVR::Status::LOADING, false);
@@ -113,8 +137,11 @@ void loop()
 {
     globalTimer.tick();
     SerialCommands::update();
+	resetCounter.update();
+#ifndef USE_ESPNOW_COMMUNICATION
     OTA::otaUpdate();
     networkManager.update();
+#endif
     sensorManager.update();
     battery.Loop();
     ledManager.update();

src/network/espnowconnection.cpp

@@ -0,0 +1,234 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2024 Gorbit99 & SlimeVR Contributors
+
+	Permission is hereby granted, free of charge, to any person obtaining a copy
+	of this software and associated documentation files (the "Software"), to deal
+	in the Software without restriction, including without limitation the rights
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	copies of the Software, and to permit persons to whom the Software is
+	furnished to do so, subject to the following conditions:
+
+	The above copyright notice and this permission notice shall be included in
+	all copies or substantial portions of the Software.
+
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+	THE SOFTWARE.
+*/
+#include "espnowconnection.h"
+#include <algorithm>
+
+#include "../GlobalVars.h"
+#include "espnowmessages.h"
+#include "espnowpackets.h"
+
+#ifndef ESP_OK
+#define ESP_OK 0
+#endif
+
+#define MACSTR        "%02x:%02x:%02x:%02x:%02x:%02x"
+#define MAC2ARGS(mac) mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]
+
+namespace SlimeVR::Network {
+
+#if ESP8266
+void onReceive(uint8_t *senderMacAddress,
+               uint8_t *data,
+               uint8_t dataLen) {
+    espnowConnection.handleMessage(senderMacAddress, data, dataLen);
+}
+#elif ESP32
+void onReceive(const esp_now_recv_info_t *espnowInfo,
+               const uint8_t *data,
+               int dataLen) {
+    espnowConnection.handleMessage(espnowInfo->src_addr, data, static_cast<uint8_t>(dataLen));
+}
+#endif
+
+void ESPNowConnection::setup() {
+    WiFi.mode(WIFI_STA);
+
+    if (esp_now_init() != ESP_OK) {
+        m_Logger.fatal("Couldn't initialize ESPNow!");
+        return;
+    }
+
+	#ifdef ESP8266
+	esp_now_set_self_role(ESP_NOW_ROLE_COMBO);
+	#endif
+
+    if (!registerPeer(broadcastMacAddress)) {
+        m_Logger.fatal("Couldn't add broadcast mac address as a peer!");
+        return;
+    }
+
+    if (!configuration.loadDongleConnection(dongleMacAddress, trackerId)) {
+        m_Logger.info("The tracker isn't paired to a dongle yet!");
+    } else {
+        m_Logger.info("Dongle mac address loaded as " MACSTR "!", MAC2ARGS(dongleMacAddress));
+
+        if (!registerPeer(dongleMacAddress)) {
+            m_Logger.fatal("Couldn't add mac address " MACSTR " as a peer!", MAC2ARGS(dongleMacAddress));
+            return;
+        }
+        paired = true;
+    }
+
+    if (esp_now_register_recv_cb(onReceive) != ESP_OK) {
+        m_Logger.fatal("Couldn't register message callback!");
+        return;
+    }
+
+    if (paired) {
+        sendConnectionRequest();
+    }
+}
+
+void ESPNowConnection::broadcastPairingRequest() {
+    ESPNow::ESPNowPairingMessage message;
+    if (esp_now_send(
+                broadcastMacAddress,
+                reinterpret_cast<uint8_t *>(&message),
+                sizeof(message)
+            ) != ESP_OK) {
+        m_Logger.fatal("Couldn't send pairing message!");
+        return;
+    }
+}
+
+void ESPNowConnection::sendFusionPacket(uint8_t sensorId, Quat fusedQuat, Vector3 accel) {
+    if (!connected) {
+        return;
+    }
+
+    int16_t endBuffer[7];
+    endBuffer[0] = toFixed<15>(fusedQuat.x);
+    endBuffer[1] = toFixed<15>(fusedQuat.y);
+    endBuffer[2] = toFixed<15>(fusedQuat.z);
+    endBuffer[3] = toFixed<15>(fusedQuat.w);
+    endBuffer[4] = toFixed<7>(accel.x);
+    endBuffer[5] = toFixed<7>(accel.y);
+    endBuffer[6] = toFixed<7>(accel.z);
+
+    ESPNow::ESPNowPacketMessage message = {};
+	message.packet.fullSizeFusion.packetId = ESPNow::ESPNowPacketId::FullSizeFusion;
+    message.packet.fullSizeFusion.sensorId = trackerId << 2 | (sensorId & 0x03);
+    memcpy(&message.packet.fullSizeFusion.quat, endBuffer, sizeof(endBuffer));
+
+    if (esp_now_send(
+                broadcastMacAddress,
+                reinterpret_cast<uint8_t *>(&message),
+                sizeof(message)
+            ) != ESP_OK) {
+        // Logging this constantly would be insane
+        // m_Logger.fatal("Couldn't send packet");
+        return;
+    }
+}
+
+bool ESPNowConnection::sendDeviceInfoPacket(Sensor* sensor) {
+    if (!connected) {
+        return false;
+    }
+
+    ESPNow::ESPNowPacketMessage message = {};
+	message.header = ESPNow::ESPNowMessageHeader::Packet;
+	ESPNow::ESPNowPacketDeviceInfo packet;
+    packet.sensorId = trackerId << 2 | (sensor->getSensorId() & 0x03);
+	packet.battPercentage = 128 | static_cast<uint8_t>(CLAMP(battery.getLevel() * 100, 0, 127));
+	packet.batteryVoltage = static_cast<uint8_t>(CLAMP((battery.getVoltage() - 2.45) * 100, 0, 255));
+	packet.temperature = 128;
+	packet.boardId = BOARD;
+	packet.protocolVersion = PROTOCOL_VERSION;
+	packet.imuId = static_cast<uint8_t>(sensor->getSensorType());
+	packet.magId = 0;
+	packet.firmwareDate = 0;
+	packet.firmwareMajor = 0;
+	packet.firmwareMinor = 0;
+	packet.firmwarePatch = 0;
+	message.packet.deviceInfo = packet;
+
+    if (esp_now_send(
+                broadcastMacAddress,
+                reinterpret_cast<uint8_t *>(&message),
+                sizeof(message)
+            ) != ESP_OK) {
+        // Logging this constantly would be insane
+        // m_Logger.fatal("Couldn't send packet");
+        return false;
+    }
+    m_Logger.info("Device info packet sent...");
+	return true;
+}
+
+bool ESPNowConnection::registerPeer(uint8_t macAddress[6]) {
+#if ESP8266
+	return esp_now_add_peer(macAddress, ESP_NOW_ROLE_COMBO, espnowWifiChannel, NULL, 0) == ESP_OK;
+#elif ESP32
+    esp_now_peer_info_t peer;
+    memcpy(peer.peer_addr, macAddress, sizeof(uint8_t[6]));
+    peer.channel = 0;
+    peer.encrypt = false;
+    peer.ifidx = WIFI_IF_STA;
+    return esp_now_add_peer(&peer) == ESP_OK;
+#endif
+}
+
+void ESPNowConnection::sendConnectionRequest() {
+	if (!paired) {
+		return;
+	}
+    ESPNow::ESPNowConnectionMessage message;
+	message.trackerId = trackerId;
+    if (esp_now_send(
+                dongleMacAddress,
+                reinterpret_cast<uint8_t *>(&message),
+                sizeof(message)
+            ) != ESP_OK) {
+        m_Logger.fatal("Couldn't send connection message!");
+        return;
+    }
+}
+
+void ESPNowConnection::handleMessage(uint8_t *senderMacAddress, const uint8_t *data, uint8_t dataLen) {
+    const auto *message = reinterpret_cast<const ESPNow::ESPNowMessage *>(data);
+    auto header = message->base.header;
+
+    switch (header) {
+		case ESPNow::ESPNowMessageHeader::Pairing:
+            return;
+		case ESPNow::ESPNowMessageHeader::PairingAck: {
+            configuration.saveDongleConnection(senderMacAddress, message->pairingAck.trackerId);
+			trackerId = message->pairingAck.trackerId;
+			memcpy(dongleMacAddress, senderMacAddress, sizeof(uint8_t) * 6);
+            m_Logger.info("Paired to dongle at mac address " MACSTR " as tracker %d!", MAC2ARGS(senderMacAddress), trackerId);
+            registerPeer(senderMacAddress);
+            paired = true;
+            connected = true;
+            sendConnectionRequest();
+            return;
+        }
+		case ESPNow::ESPNowMessageHeader::Connection:
+			if (message->connection.trackerId != trackerId) {
+				return;
+			}
+            ledManager.pattern(100, 100, 2);
+            connected = true;
+			m_Logger.info("Connected to dongle at mac address " MACSTR " as tracker %d!", MAC2ARGS(dongleMacAddress), trackerId);
+            return;
+		case ESPNow::ESPNowMessageHeader::Packet:
+            return;
+    }
+}
+
+uint8_t ESPNowConnection::broadcastMacAddress[6] = {
+	0xff, 0xff, 0xff, 0xff, 0xff, 0xff
+};
+
+
+} // namespace SlimeVR::Network

src/network/espnowconnection.h

@@ -0,0 +1,85 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2024 Gorbit99 & SlimeVR Contributors
+
+	Permission is hereby granted, free of charge, to any person obtaining a copy
+	of this software and associated documentation files (the "Software"), to deal
+	in the Software without restriction, including without limitation the rights
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	copies of the Software, and to permit persons to whom the Software is
+	furnished to do so, subject to the following conditions:
+
+	The above copyright notice and this permission notice shall be included in
+	all copies or substantial portions of the Software.
+
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+	THE SOFTWARE.
+*/
+#pragma once
+
+#include "../logging/Logger.h"
+
+#include <quat.h>
+#include <vector3.h>
+#include <algorithm>
+#include <cstdint>
+#if ESP8266
+#include <ESP8266WiFi.h>
+#include <espnow.h>
+#elif ESP32
+#include <WiFi.h>
+#include <esp_now.h>
+#endif
+#include <cmath>
+#include "sensors/sensor.h"
+
+namespace SlimeVR::Network {
+
+class ESPNowConnection {
+public:
+	void setup();
+	void broadcastPairingRequest();
+	bool sendDeviceInfoPacket(Sensor* sensor);
+	void sendFusionPacket(uint8_t sensorId, Quat fusedQuat, Vector3 accel);
+
+private:
+	bool registerPeer(uint8_t macAddress[6]);
+	void sendConnectionRequest();
+
+	void handleMessage(uint8_t *senderMacAddress, const uint8_t *data, uint8_t dataLen);
+
+	template<unsigned int Q>
+	static constexpr inline int16_t toFixed(float number) {
+		auto unsaturated = static_cast<int32_t>(number * (1 << Q));
+		auto saturated = std::clamp(
+				unsaturated,
+				static_cast<int32_t>(-32768),
+				static_cast<int32_t>(32767)
+		);
+		return static_cast<int16_t>(saturated);
+	}
+
+	bool paired = false;
+	bool connected = false;
+	uint8_t dongleMacAddress[6];
+	uint8_t trackerId;
+
+	Logging::Logger m_Logger = Logging::Logger("ESPNowConnection");
+
+	static uint8_t broadcastMacAddress[6];
+
+	static constexpr uint8_t espnowWifiChannel = 1;
+
+	#if ESP8266
+	friend void onReceive(uint8_t *, uint8_t *, uint8_t);
+	#elif ESP32
+	friend void onReceive(const esp_now_recv_info_t *, const uint8_t *, int);
+	#endif
+};
+
+}  // namespace SlimeVR::Network

src/network/espnowmessages.h

@@ -0,0 +1,72 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2024 Gorbit99 & SlimeVR Contributors
+
+	Permission is hereby granted, free of charge, to any person obtaining a copy
+	of this software and associated documentation files (the "Software"), to deal
+	in the Software without restriction, including without limitation the rights
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	copies of the Software, and to permit persons to whom the Software is
+	furnished to do so, subject to the following conditions:
+
+	The above copyright notice and this permission notice shall be included in
+	all copies or substantial portions of the Software.
+
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+	THE SOFTWARE.
+*/
+
+#pragma once
+
+#include <cstdint>
+
+#include "./espnowpackets.h"
+
+namespace SlimeVR::Network::ESPNow {
+
+enum class ESPNowMessageHeader : uint8_t {
+	Pairing = 0x00,
+	PairingAck = 0x01,
+	Connection = 0x02,
+	Packet = 0x03,
+};
+
+struct ESPNowMessageBase {
+	ESPNowMessageHeader header;
+};
+
+struct ESPNowPairingMessage {
+	ESPNowMessageHeader header = ESPNowMessageHeader::Pairing;
+};
+
+struct ESPNowPairingAckMessage {
+	ESPNowMessageHeader header = ESPNowMessageHeader::PairingAck;
+	uint8_t trackerId;
+};
+
+struct ESPNowConnectionMessage {
+	ESPNowMessageHeader header = ESPNowMessageHeader::Connection;
+	uint8_t trackerId;
+};
+
+#pragma pack(push, 1)
+struct ESPNowPacketMessage {
+	ESPNowMessageHeader header = ESPNowMessageHeader::Packet;
+	ESPNowPacket packet;
+};
+#pragma pack(pop)
+
+union ESPNowMessage {
+	ESPNowMessageBase base;
+	ESPNowPairingMessage pairing;
+	ESPNowPairingAckMessage pairingAck;
+	ESPNowConnectionMessage connection;
+	ESPNowPacketMessage packet;
+};
+
+}  // namespace SlimeVR::Network

src/network/espnowpackets.h

@@ -0,0 +1,67 @@
+/*
+	SlimeVR Code is placed under the MIT license
+	Copyright (c) 2024 Gorbit99 & SlimeVR Contributors
+
+	Permission is hereby granted, free of charge, to any person obtaining a copy
+	of this software and associated documentation files (the "Software"), to deal
+	in the Software without restriction, including without limitation the rights
+	to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+	copies of the Software, and to permit persons to whom the Software is
+	furnished to do so, subject to the following conditions:
+
+	The above copyright notice and this permission notice shall be included in
+	all copies or substantial portions of the Software.
+
+	THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+	IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+	FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+	AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+	LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+	THE SOFTWARE.
+*/
+
+#pragma once
+
+#include <cstdint>
+
+namespace SlimeVR::Network::ESPNow {
+
+enum class ESPNowPacketId : uint8_t {
+	DeviceInfo = 0x00,
+	FullSizeFusion = 0x01,
+};
+
+struct ESPNowPacketDeviceInfo {
+	ESPNowPacketId packetId = ESPNowPacketId::DeviceInfo;
+	uint8_t sensorId;
+	uint8_t battPercentage;
+	uint8_t batteryVoltage;
+	uint8_t temperature;
+	uint8_t boardId;
+	uint8_t protocolVersion;
+	uint8_t reserved0;
+	uint8_t imuId;
+	uint8_t magId;
+	uint16_t firmwareDate;
+	uint8_t firmwareMajor;
+	uint8_t firmwareMinor;
+	uint8_t firmwarePatch;
+	uint8_t rssi;
+};
+
+struct ESPNowPacketFullSizeFusion {
+	ESPNowPacketId packetId = ESPNowPacketId::FullSizeFusion;
+	uint8_t sensorId;
+	int16_t quat[4];
+	int16_t accel[3];
+};
+
+union ESPNowPacket {
+	ESPNowPacketDeviceInfo deviceInfo;
+	ESPNowPacketFullSizeFusion fullSizeFusion;
+};
+
+static_assert(sizeof(ESPNowPacket) == 16);
+
+}

src/network/manager.cpp

@@ -20,14 +20,18 @@
 	OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 	THE SOFTWARE.
 */
-#include "manager.h"
 
 #include "GlobalVars.h"
 
-namespace SlimeVR {
-namespace Network {
+#ifndef USE_ESPNOW_COMMUNICATION
 
-void Manager::setup() { ::WiFiNetwork::setUp(); }
+#include "manager.h"
+
+namespace SlimeVR::Network {
+
+void Manager::setup() {
+	::WiFiNetwork::setUp();
+}
 
 void Manager::update() {
 	WiFiNetwork::upkeep();
@@ -48,5 +52,5 @@ void Manager::update() {
 	networkConnection.update();
 }
 
-}  // namespace Network
-}  // namespace SlimeVR
+}  // namespace SlimeVR::Network
+#endif

src/network/wifihandler.cpp

@@ -20,9 +20,13 @@
     OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
     THE SOFTWARE.
 */
+
+#include "GlobalVars.h"
+
+#ifndef USE_ESPNOW_COMMUNICATION
+
 #include "globals.h"
 #include "logging/Logger.h"
-#include "GlobalVars.h"
 #if !ESP8266
 #include "esp_wifi.h"
 #endif
@@ -246,3 +250,5 @@ void WiFiNetwork::upkeep() {
     }
     return;
 }
+
+#endif

src/sensors/SensorManager.cpp

@@ -145,9 +145,12 @@ namespace SlimeVR
 
             statusManager.setStatus(SlimeVR::Status::IMU_ERROR, !allIMUGood);
 
+            #ifndef USE_ESPNOW_COMMUNICATION
             if (!networkConnection.isConnected()) {
                 return;
             }
+            #endif
+			// TODO: ESPNOW
 
             #ifndef PACKET_BUNDLING
                 static_assert(false, "PACKET_BUNDLING not set");
@@ -172,9 +175,12 @@ namespace SlimeVR
 
                 m_LastBundleSentAtMicros = now;
             #endif
-            
+
             #if PACKET_BUNDLING != PACKET_BUNDLING_DISABLED
+                #ifndef USE_ESPNOW_COMMUNICATION
                 networkConnection.beginBundle();
+                #endif
+				// TODO: ESPNOW
             #endif
 
             for (auto &sensor : m_Sensors) {
@@ -184,7 +190,10 @@ namespace SlimeVR
             }
 
             #if PACKET_BUNDLING != PACKET_BUNDLING_DISABLED
+                #ifndef USE_ESPNOW_COMMUNICATION
                 networkConnection.endBundle();
+                #endif
+				// TODO: ESPNOW
             #endif
         }
 

src/sensors/bmi160sensor.cpp

@@ -243,7 +243,10 @@ void BMI160Sensor::motionLoop() {
         getRemappedRotation(&rX, &rY, &rZ);
         getRemappedAcceleration(&aX, &aY, &aZ);
 
+        #ifndef USE_ESPNOW_COMMUNICATION
         networkConnection.sendInspectionRawIMUData(sensorId, rX, rY, rZ, 255, aX, aY, aZ, 255, 0, 0, 0, 255);
+        #endif
+		// TODO: ESPNOW
     }
     #endif
 
@@ -346,9 +349,15 @@ void BMI160Sensor::motionLoop() {
             lastTemperaturePacketSent = now - (elapsed - sendInterval);
             #if BMI160_TEMPCAL_DEBUG
                 uint32_t isCalibrating = gyroTempCalibrator->isCalibrating() ? 10000 : 0;
+                #ifndef USE_ESPNOW_COMMUNICATION
                 networkConnection.sendTemperature(sensorId, isCalibrating + 10000 + (gyroTempCalibrator->config.samplesTotal * 100) + temperature);
+                #endif
+				// TODO: ESPNOW
             #else
+                #ifndef USE_ESPNOW_COMMUNICATION
                 networkConnection.sendTemperature(sensorId, temperature);
+                #endif
+				// TODO: ESPNOW
             #endif
             optimistic_yield(100);
         }

src/sensors/bno080sensor.cpp

@@ -117,7 +117,10 @@ void BNO080Sensor::motionLoop()
             int16_t mZ = imu.getRawMagZ();
             uint8_t mA = imu.getMagAccuracy();
 
+            #ifndef USE_ESPNOW_COMMUNICATION
             networkConnection.sendInspectionRawIMUData(sensorId, rX, rY, rZ, rA, aX, aY, aZ, aA, mX, mY, mZ, mA);
+            #endif
+			// TODO: ESPNOW
         }
 #endif
 
@@ -180,7 +183,10 @@ void BNO080Sensor::motionLoop()
         if (rr != lastReset)
         {
             lastReset = rr;
+            #ifndef USE_ESPNOW_COMMUNICATION
             networkConnection.sendSensorError(this->sensorId, rr);
+            #endif
+			// TODO: ESPNOW
         }
 
         m_Logger.error("Sensor %d doesn't respond. Last reset reason:", sensorId, lastReset);
@@ -196,27 +202,14 @@ SensorStatus BNO080Sensor::getSensorState()
 
 void BNO080Sensor::sendData()
 {
-    if (newFusedRotation)
-    {
-        newFusedRotation = false;
-        networkConnection.sendRotationData(sensorId, &fusedRotation, DATA_TYPE_NORMAL, calibrationAccuracy);
-
-#ifdef DEBUG_SENSOR
-        m_Logger.trace("Quaternion: %f, %f, %f, %f", UNPACK_QUATERNION(fusedRotation));
-#endif
-
-#if SEND_ACCELERATION
-        if (newAcceleration)
-        {
-            newAcceleration = false;
-            networkConnection.sendSensorAcceleration(this->sensorId, this->acceleration);
-        }
-#endif
-    }
+    Sensor::sendData();
 
     if (tap != 0)
     {
+        #ifndef USE_ESPNOW_COMMUNICATION
         networkConnection.sendSensorTap(sensorId, tap);
+        #endif
+		// TODO: ESPNOW
         tap = 0;
     }
 }

src/sensors/icm20948sensor.cpp

@@ -60,7 +60,10 @@ void ICM20948Sensor::motionLoop()
         float mY = imu.magY();
         float mZ = imu.magZ();
 
+        #ifndef USE_ESPNOW_COMMUNICATION
         networkConnection.sendInspectionRawIMUData(sensorId, rX, rY, rZ, 255, aX, aY, aZ, 255, mX, mY, mZ, 255);
+        #endif
+		// TODO: ESPNOW
     }
 #endif
 
@@ -111,18 +114,27 @@ void ICM20948Sensor::sendData()
 
         #if(USE_6_AXIS)
         {
+            #ifndef USE_ESPNOW_COMMUNICATION
             networkConnection.sendRotationData(sensorId, &fusedRotation, DATA_TYPE_NORMAL, 0);
+            #endif
+			// TODO: ESPNOW
         }
         #else
         {
+            #ifndef USE_ESPNOW_COMMUNICATION
             networkConnection.sendRotationData(sensorId, &fusedRotation, DATA_TYPE_NORMAL, dmpData.Quat9.Data.Accuracy);
+            #endif
+			// TODO: ESPNOW
         }
         #endif
 
 #if SEND_ACCELERATION
         if (newAcceleration) {
             newAcceleration = false;
+            #ifndef USE_ESPNOW_COMMUNICATION
             networkConnection.sendSensorAcceleration(sensorId, acceleration);
+            #endif
+			// TODO: ESPNOW
         }
 #endif
     }
@@ -328,7 +340,10 @@ void ICM20948Sensor::checkSensorTimeout()
     if(lastData + 2000 < currenttime) {
         working = false;
         m_Logger.error("Sensor timeout I2C Address 0x%02x delaytime: %d ms", addr, currenttime-lastData );
+        #ifndef USE_ESPNOW_COMMUNICATION
         networkConnection.sendSensorError(this->sensorId, 1);
+        #endif
+		// TODO: ESPNOW
         lastData = currenttime;
     }
 }

src/sensors/sensor.cpp

@@ -44,6 +44,7 @@ void Sensor::setFusedRotation(Quat r) {
 }
 
 void Sensor::sendData() {
+#ifndef USE_ESPNOW_COMMUNICATION
     if (newFusedRotation) {
         newFusedRotation = false;
         networkConnection.sendRotationData(sensorId, &fusedRotation, DATA_TYPE_NORMAL, calibrationAccuracy);
@@ -59,6 +60,17 @@ void Sensor::sendData() {
         }
 #endif
     }
+#else
+	if (!sentDeviceInfo || lastDeviceInfoPacket + 30000 < millis()) {
+		sentDeviceInfo = espnowConnection.sendDeviceInfoPacket(this);
+        lastDeviceInfoPacket = millis();
+	}
+
+	if (newFusedRotation) {
+        newFusedRotation = false;
+		espnowConnection.sendFusionPacket(sensorId, fusedRotation, acceleration);
+	}
+#endif
 }
 
 void Sensor::printTemperatureCalibrationUnsupported() {

src/sensors/sensor.h

@@ -130,6 +130,11 @@ public:
 
 private:
 	void printTemperatureCalibrationUnsupported();
+
+#ifdef USE_ESPNOW_COMMUNICATION
+	bool sentDeviceInfo = false;
+	long lastDeviceInfoPacket = 0;
+#endif
 };
 
 const char * getIMUNameByType(ImuID imuType);

src/sensors/softfusion/softfusionsensor.h

@@ -28,6 +28,8 @@
 
 #include "GlobalVars.h"
 
+#include <cstring>
+
 namespace SlimeVR::Sensors
 {
 
@@ -80,7 +82,10 @@ class SoftFusionSensor : public Sensor
         if (elapsed >= sendInterval) {
             const float temperature = m_sensor.getDirectTemp();
             m_lastTemperaturePacketSent = now - (elapsed - sendInterval);
+            #ifndef USE_ESPNOW_COMMUNICATION
             networkConnection.sendTemperature(sensorId, temperature);
+            #endif
+			// TODO: ESPNOW
         }
     }
 
@@ -192,7 +197,7 @@ public:
 
         // send new fusion values when time is up
         now = micros();
-        constexpr float maxSendRateHz = 120.0f;
+        constexpr float maxSendRateHz = 100.0f;
         constexpr uint32_t sendInterval = 1.0f/maxSendRateHz * 1e6;
         elapsed = now - m_lastRotationPacketSent;
         if (elapsed >= sendInterval) {

src/serial/serialcommands.cpp

@@ -62,6 +62,7 @@ namespace SerialCommands {
 
 	void cmdSet(CmdParser * parser) {
 		if(parser->getParamCount() != 1) {
+            #ifndef USE_ESPNOW_COMMUNICATION
 			if (parser->equalCmdParam(1, "WIFI")) {
 				if(parser->getParamCount() < 3) {
 					logger.error("CMD SET WIFI ERROR: Too few arguments");
@@ -117,12 +118,14 @@ namespace SerialCommands {
 			} else {
 				logger.error("CMD SET ERROR: Unrecognized variable to set");
 			}
+            #endif
 		} else {
 			logger.error("CMD SET ERROR: No variable to set");
 		}
 	}
 
     void printState() {
+        #ifndef USE_ESPNOW_COMMUNICATION
         logger.info(
             "SlimeVR Tracker, board: %d, hardware: %d, protocol: %d, firmware: %s, address: %s, mac: %s, status: %d, wifi state: %d",
             BOARD,
@@ -134,6 +137,8 @@ namespace SerialCommands {
             statusManager.getStatus(),
             WiFiNetwork::getWiFiState()
         );
+        #endif
+		// TODO: ESPNOW
         for (auto &sensor : sensorManager.getSensors()) {
             logger.info(
                 "Sensor[%d]: %s (%.3f %.3f %.3f %.3f) is working: %s, had data: %s",
@@ -204,6 +209,7 @@ namespace SerialCommands {
         }
 
         if (parser->equalCmdParam(1, "TEST")) {
+            #ifndef USE_ESPNOW_COMMUNICATION
             logger.info(
                 "[TEST] Board: %d, hardware: %d, protocol: %d, firmware: %s, address: %s, mac: %s, status: %d, wifi state: %d",
                 BOARD,
@@ -215,6 +221,8 @@ namespace SerialCommands {
                 statusManager.getStatus(),
                 WiFiNetwork::getWiFiState()
             );
+            #endif
+			// TODO: ESPNOW
             auto& sensor0 = sensorManager.getSensors()[0];
             sensor0->motionLoop();
             logger.info(
@@ -231,6 +239,7 @@ namespace SerialCommands {
             }
         }
 
+        #ifndef USE_ESPNOW_COMMUNICATION
         if (parser->equalCmdParam(1, "WIFISCAN")) {
 			logger.info("[WSCAN] Scanning for WiFi networks...");
 
@@ -263,6 +272,7 @@ namespace SerialCommands {
 				WiFi.begin();
 			}
         }
+        #endif
     }
 
     void cmdReboot(CmdParser * parser) {
@@ -328,12 +338,17 @@ namespace SerialCommands {
         logger.info("  Temperature calibration config saves automatically when calibration percent is at 100%");
     }
 
+    void cmdPair(CmdParser* parser) {
+        espnowConnection.broadcastPairingRequest();
+    }
+
     void setUp() {
         cmdCallbacks.addCmd("SET", &cmdSet);
         cmdCallbacks.addCmd("GET", &cmdGet);
         cmdCallbacks.addCmd("FRST", &cmdFactoryReset);
         cmdCallbacks.addCmd("REBOOT", &cmdReboot);
         cmdCallbacks.addCmd("TCAL", &cmdTemperatureCalibration);
+        cmdCallbacks.addCmd("PAIR", &cmdPair);
     }
 
     void update() {