Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add threading support #320

Open
wants to merge 2 commits into
base: main
Choose a base branch
from
Open

Add threading support #320

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
582df6a
Add multithreading
Feb 27, 2024
65e4052
Add define switches for threading
nekomona Mar 28, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 3 additions & 0 deletions src/debug.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
29 changes: 29 additions & 0 deletions src/sensors/SensorManager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -186,10 +186,38 @@ namespace SlimeVR
sensor->postSetup();
}
}
#if ESP32 && SENSOR_THREADING
for (auto & sensor : m_Sensors) {
sensor->updateMutex = xSemaphoreCreateMutex();
}
xTaskCreateUniversal(updateSensors, "sensors", 16*1024, this, 10, &sensorTask, ARDUINO_RUNNING_CORE);
#endif
}

#if ESP32 && SENSOR_THREADING
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 && SENSOR_THREADING)
// 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
View file
Open in desktop
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 && SENSOR_THREADING
TaskHandle_t sensorTask = NULL;
static void updateSensors(void * pvParameters);
#endif
};
}
}
Expand Down
42 changes: 42 additions & 0 deletions src/sensors/sensor.cpp
View file
Open in desktop
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 && SENSOR_THREADING
xSemaphoreTake(updateMutex, portMAX_DELAY);
#endif
acceleration = a;
newAcceleration = true;
#if ESP32 && SENSOR_THREADING
xSemaphoreGive(updateMutex);
#endif
}

void Sensor::setFusedRotation(Quat r) {
#if ESP32 && SENSOR_THREADING
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 && SENSOR_THREADING
xSemaphoreGive(updateMutex);
#endif
}

void Sensor::sendData() {
#if ESP32 && SENSOR_THREADING
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);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
networkConnection.sendRotationData(sensorId, &fusedRotation, DATA_TYPE_NORMAL, calibrationAccuracy);
networkConnection.sendRotationData(sensorId, &lquat, DATA_TYPE_NORMAL, calibrationAccuracy);

Shouldn't it be like that?

Also can be combined my taking lquat out of the define and having the same sendRotationData be used.

#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 && SENSOR_THREADING
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
View file
Open in desktop
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 && SENSOR_THREADING
SemaphoreHandle_t updateMutex = NULL;
#endif
private:
void printTemperatureCalibrationUnsupported();
};
Expand Down
Loading