Skip to content

Commit

Permalink
Add multithreading
Browse files Browse the repository at this point in the history
  • Loading branch information
nekomona authored and nekomona committed Mar 28, 2024
1 parent e09ca3c commit 582df6a
Show file tree
Hide file tree
Showing 4 changed files with 78 additions and 0 deletions.
29 changes: 29 additions & 0 deletions src/sensors/SensorManager.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand All @@ -204,6 +232,7 @@ namespace SlimeVR
}

statusManager.setStatus(SlimeVR::Status::IMU_ERROR, !allIMUGood);
#endif

if (!networkConnection.isConnected()) {
return;
Expand Down
4 changes: 4 additions & 0 deletions src/sensors/SensorManager.h
Original file line number Diff line number Diff line change
Expand Up @@ -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
};
}
}
Expand Down
42 changes: 42 additions & 0 deletions src/sensors/sensor.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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
}
}
Expand Down
3 changes: 3 additions & 0 deletions src/sensors/sensor.h
Original file line number Diff line number Diff line change
Expand Up @@ -111,6 +111,9 @@ class Sensor
uint8_t sclPin = 0;
uint8_t sdaPin = 0;

#if ESP32
SemaphoreHandle_t updateMutex = NULL;
#endif
private:
void printTemperatureCalibrationUnsupported();
};
Expand Down

0 comments on commit 582df6a

Please sign in to comment.