From 582df6a0f23486fef9392177a52470857204a818 Mon Sep 17 00:00:00 2001 From: nekomona Date: Tue, 27 Feb 2024 17:15:42 +0800 Subject: [PATCH 1/2] Add multithreading --- src/sensors/SensorManager.cpp | 29 ++++++++++++++++++++++++ src/sensors/SensorManager.h | 4 ++++ src/sensors/sensor.cpp | 42 +++++++++++++++++++++++++++++++++++ src/sensors/sensor.h | 3 +++ 4 files changed, 78 insertions(+) diff --git a/src/sensors/SensorManager.cpp b/src/sensors/SensorManager.cpp index 434f8a5f7..94db4b0a1 100644 --- a/src/sensors/SensorManager.cpp +++ b/src/sensors/SensorManager.cpp @@ -186,10 +186,38 @@ namespace SlimeVR sensor->postSetup(); } } +#if ESP32 + for (auto & sensor : m_Sensors) { + sensor->updateMutex = xSemaphoreCreateMutex(); + } + xTaskCreateUniversal(updateSensors, "sensors", 16*1024, this, 10, &sensorTask, ARDUINO_RUNNING_CORE); +#endif + } + +#if ESP32 + void SensorManager::updateSensors(void * pxParameter) { + SensorManager *pthis = (SensorManager *)pxParameter; + for (;;) { + bool allIMUGood = true; + for (auto & sensor : pthis->m_Sensors) { + if (sensor->isWorking()) { + pthis->swapI2C(sensor->sclPin, sensor->sdaPin); + sensor->motionLoop(); + } + if (sensor->getSensorState() == SensorStatus::SENSOR_ERROR) + { + allIMUGood = false; + } + } + + statusManager.setStatus(SlimeVR::Status::IMU_ERROR, !allIMUGood); + } } +#endif void SensorManager::update() { +#if !ESP32 // Gather IMU data bool allIMUGood = true; for (auto sensor : m_Sensors) { @@ -204,6 +232,7 @@ namespace SlimeVR } statusManager.setStatus(SlimeVR::Status::IMU_ERROR, !allIMUGood); +#endif if (!networkConnection.isConnected()) { return; diff --git a/src/sensors/SensorManager.h b/src/sensors/SensorManager.h index e68033ee7..09bafeab0 100644 --- a/src/sensors/SensorManager.h +++ b/src/sensors/SensorManager.h @@ -71,6 +71,10 @@ namespace SlimeVR void swapI2C(uint8_t scl, uint8_t sda); uint32_t m_LastBundleSentAtMicros = micros(); + #if ESP32 + TaskHandle_t sensorTask = NULL; + static void updateSensors(void * pvParameters); + #endif }; } } diff --git a/src/sensors/sensor.cpp b/src/sensors/sensor.cpp index b992b3485..57e82a8d5 100644 --- a/src/sensors/sensor.cpp +++ b/src/sensors/sensor.cpp @@ -30,33 +30,75 @@ SensorStatus Sensor::getSensorState() { } void Sensor::setAcceleration(Vector3 a) { +#if ESP32 + xSemaphoreTake(updateMutex, portMAX_DELAY); +#endif acceleration = a; newAcceleration = true; +#if ESP32 + xSemaphoreGive(updateMutex); +#endif } void Sensor::setFusedRotation(Quat r) { +#if ESP32 + xSemaphoreTake(updateMutex, portMAX_DELAY); +#endif fusedRotation = r * sensorOffset; bool changed = OPTIMIZE_UPDATES ? !lastFusedRotationSent.equalsWithEpsilon(fusedRotation) : true; if (ENABLE_INSPECTION || changed) { newFusedRotation = true; lastFusedRotationSent = fusedRotation; } +#if ESP32 + xSemaphoreGive(updateMutex); +#endif } void Sensor::sendData() { +#if ESP32 + Quat lquat; + xSemaphoreTake(updateMutex, portMAX_DELAY); if (newFusedRotation) { + newFusedRotation = false; + lquat = fusedRotation; + xSemaphoreGive(updateMutex); + } else { + xSemaphoreGive(updateMutex); + return; + } + + { + networkConnection.sendRotationData(sensorId, &fusedRotation, DATA_TYPE_NORMAL, calibrationAccuracy); +#else + if (newFusedRotation) { newFusedRotation = false; networkConnection.sendRotationData(sensorId, &fusedRotation, DATA_TYPE_NORMAL, calibrationAccuracy); +#endif #ifdef DEBUG_SENSOR m_Logger.trace("Quaternion: %f, %f, %f, %f", UNPACK_QUATERNION(fusedRotation)); #endif #if SEND_ACCELERATION + #if ESP32 + Vector3 laccel; + xSemaphoreTake(updateMutex, portMAX_DELAY); + if (newAcceleration) { + newAcceleration = false; + laccel = acceleration; + xSemaphoreGive(updateMutex); + } else { + xSemaphoreGive(updateMutex); + return; + } + networkConnection.sendSensorAcceleration(sensorId, laccel); + #else if (newAcceleration) { newAcceleration = false; networkConnection.sendSensorAcceleration(sensorId, acceleration); } + #endif #endif } } diff --git a/src/sensors/sensor.h b/src/sensors/sensor.h index 6d603a8c8..c620b01af 100644 --- a/src/sensors/sensor.h +++ b/src/sensors/sensor.h @@ -111,6 +111,9 @@ class Sensor uint8_t sclPin = 0; uint8_t sdaPin = 0; + #if ESP32 + SemaphoreHandle_t updateMutex = NULL; + #endif private: void printTemperatureCalibrationUnsupported(); }; From 65e40529013f3b66c59e8ea661905c4ed988963b Mon Sep 17 00:00:00 2001 From: nekomona Date: Thu, 28 Mar 2024 23:05:58 +0800 Subject: [PATCH 2/2] Add define switches for threading --- src/debug.h | 3 +++ src/sensors/SensorManager.cpp | 6 +++--- src/sensors/SensorManager.h | 2 +- src/sensors/sensor.cpp | 12 ++++++------ src/sensors/sensor.h | 2 +- 5 files changed, 14 insertions(+), 11 deletions(-) diff --git a/src/debug.h b/src/debug.h index b2c4968d5..d0ae03b07 100644 --- a/src/debug.h +++ b/src/debug.h @@ -59,6 +59,9 @@ #define TARGET_LOOPTIME_MICROS (samplingRateInMillis * 1000) #endif +// Enable threading for higher performance on ESP32 variants +#define SENSOR_THREADING true + // Packet bundling/aggregation #define PACKET_BUNDLING PACKET_BUNDLING_BUFFERED // Extra tunable for PACKET_BUNDLING_BUFFERED (10000us = 10ms timeout, 100hz target) diff --git a/src/sensors/SensorManager.cpp b/src/sensors/SensorManager.cpp index 94db4b0a1..046636405 100644 --- a/src/sensors/SensorManager.cpp +++ b/src/sensors/SensorManager.cpp @@ -186,7 +186,7 @@ namespace SlimeVR sensor->postSetup(); } } -#if ESP32 +#if ESP32 && SENSOR_THREADING for (auto & sensor : m_Sensors) { sensor->updateMutex = xSemaphoreCreateMutex(); } @@ -194,7 +194,7 @@ namespace SlimeVR #endif } -#if ESP32 +#if ESP32 && SENSOR_THREADING void SensorManager::updateSensors(void * pxParameter) { SensorManager *pthis = (SensorManager *)pxParameter; for (;;) { @@ -217,7 +217,7 @@ namespace SlimeVR void SensorManager::update() { -#if !ESP32 +#if !(ESP32 && SENSOR_THREADING) // Gather IMU data bool allIMUGood = true; for (auto sensor : m_Sensors) { diff --git a/src/sensors/SensorManager.h b/src/sensors/SensorManager.h index 09bafeab0..6d2096add 100644 --- a/src/sensors/SensorManager.h +++ b/src/sensors/SensorManager.h @@ -71,7 +71,7 @@ namespace SlimeVR void swapI2C(uint8_t scl, uint8_t sda); uint32_t m_LastBundleSentAtMicros = micros(); - #if ESP32 + #if ESP32 && SENSOR_THREADING TaskHandle_t sensorTask = NULL; static void updateSensors(void * pvParameters); #endif diff --git a/src/sensors/sensor.cpp b/src/sensors/sensor.cpp index 57e82a8d5..b25a61850 100644 --- a/src/sensors/sensor.cpp +++ b/src/sensors/sensor.cpp @@ -30,18 +30,18 @@ SensorStatus Sensor::getSensorState() { } void Sensor::setAcceleration(Vector3 a) { -#if ESP32 +#if ESP32 && SENSOR_THREADING xSemaphoreTake(updateMutex, portMAX_DELAY); #endif acceleration = a; newAcceleration = true; -#if ESP32 +#if ESP32 && SENSOR_THREADING xSemaphoreGive(updateMutex); #endif } void Sensor::setFusedRotation(Quat r) { -#if ESP32 +#if ESP32 && SENSOR_THREADING xSemaphoreTake(updateMutex, portMAX_DELAY); #endif fusedRotation = r * sensorOffset; @@ -50,13 +50,13 @@ void Sensor::setFusedRotation(Quat r) { newFusedRotation = true; lastFusedRotationSent = fusedRotation; } -#if ESP32 +#if ESP32 && SENSOR_THREADING xSemaphoreGive(updateMutex); #endif } void Sensor::sendData() { -#if ESP32 +#if ESP32 && SENSOR_THREADING Quat lquat; xSemaphoreTake(updateMutex, portMAX_DELAY); if (newFusedRotation) { @@ -81,7 +81,7 @@ void Sensor::sendData() { #endif #if SEND_ACCELERATION - #if ESP32 + #if ESP32 && SENSOR_THREADING Vector3 laccel; xSemaphoreTake(updateMutex, portMAX_DELAY); if (newAcceleration) { diff --git a/src/sensors/sensor.h b/src/sensors/sensor.h index c620b01af..82ec7fd03 100644 --- a/src/sensors/sensor.h +++ b/src/sensors/sensor.h @@ -111,7 +111,7 @@ class Sensor uint8_t sclPin = 0; uint8_t sdaPin = 0; - #if ESP32 + #if ESP32 && SENSOR_THREADING SemaphoreHandle_t updateMutex = NULL; #endif private: