From 582df6a0f23486fef9392177a52470857204a818 Mon Sep 17 00:00:00 2001
From: nekomona <nekomona@nekomona.com>
Date: Tue, 27 Feb 2024 17:15:42 +0800
Subject: [PATCH] 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();
 };