Poll sensor at the same time we send data

SlimeVR · Jan 27, 2025 · 935f009 · 935f009
1 parent ffbee37
commit 935f009
Showing 1 changed file with 11 additions and 14 deletions.
diff --git a/src/sensors/softfusion/softfusionsensor.h b/src/sensors/softfusion/softfusionsensor.h
@@ -218,7 +218,6 @@ class SoftFusionSensor : public Sensor {
 	~SoftFusionSensor() override = default;
 
 	void motionLoop() final {
-
 		calibrator.tick();
 
 		// read fifo updating fusion
@@ -245,6 +244,14 @@ class SoftFusionSensor : public Sensor {
 		uint32_t elapsed = now - m_lastPollTime;
 		if (elapsed >= targetPollIntervalMicros) {
 			m_lastPollTime = now - (elapsed - targetPollIntervalMicros);
+		}
+
+		// send new fusion values when time is up
+		now = micros();
+		constexpr float maxSendRateHz = 100.0f;
+		constexpr uint32_t sendInterval = 1.0f / maxSendRateHz * 1e6;
+		elapsed = now - m_lastRotationPacketSent;
+		if (elapsed >= sendInterval) {
 			m_sensor.bulkRead(
 				[&](const RawSensorT xyz[3], const sensor_real_t timeDelta) {
 					processAccelSample(xyz, timeDelta);
@@ -256,20 +263,11 @@ class SoftFusionSensor : public Sensor {
 					processTempSample(rawTemp, timeDelta);
 				}
 			);
-			optimistic_yield(100);
 			if (!m_fusion.isUpdated()) {
-				return;
+				hadData = true;
+				m_fusion.clearUpdated();
 			}
-			hadData = true;
-			m_fusion.clearUpdated();
-		}
 
-		// send new fusion values when time is up
-		now = micros();
-		constexpr float maxSendRateHz = 120.0f;
-		constexpr uint32_t sendInterval = 1.0f / maxSendRateHz * 1e6;
-		elapsed = now - m_lastRotationPacketSent;
-		if (elapsed >= sendInterval) {
 			m_lastRotationPacketSent = now - (elapsed - sendInterval);
 
 			setFusedRotation(m_fusion.getQuaternionQuat());
@@ -376,8 +374,7 @@ class SoftFusionSensor : public Sensor {
 		m_Logger,
 		getDefaultTempTs(),
 		AScale,
-		GScale
-	};
+		GScale};
 
 	SensorStatus m_status = SensorStatus::SENSOR_OFFLINE;
 	uint32_t m_lastPollTime = micros();