Merge code for applying sensorOffset and setting data ready flag (S…

…limeVR#314) Merge applying of sensorOffset and data ready flag Co-authored-by: nekomona <[email protected]>
l0ud · Apr 1, 2024 · c2668ad · c2668ad
1 parent 5534a1d
commit c2668ad
Show file tree

Hide file tree

Showing 9 changed files with 33 additions and 70 deletions.
diff --git a/src/sensors/bmi160sensor.cpp b/src/sensors/bmi160sensor.cpp
@@ -361,13 +361,8 @@ void BMI160Sensor::motionLoop() {
         if (elapsed >= sendInterval) {
             lastRotationPacketSent = now - (elapsed - sendInterval);
 
-            fusedRotation = sfusion.getQuaternionQuat();
-            setFusedRotationReady();
-
-            acceleration = sfusion.getLinearAccVec();
-            setAccelerationReady();
-
-            fusedRotation *= sensorOffset;
+            setFusedRotation(sfusion.getQuaternionQuat());
+            setAcceleration(sfusion.getLinearAccVec());
 
             optimistic_yield(100);
         }

diff --git a/src/sensors/bno055sensor.cpp b/src/sensors/bno055sensor.cpp
@@ -60,16 +60,10 @@ void BNO055Sensor::motionLoop() {
 #endif
 
     // TODO Optimize a bit with setting rawQuat directly
-    Quat quat = imu.getQuat();
-    fusedRotation.set(quat.x, quat.y, quat.z, quat.w);
-    fusedRotation *= sensorOffset;
-    setFusedRotationReady();
+    setFusedRotation(imu.getQuat());
 
 #if SEND_ACCELERATION
-    {
-        acceleration = this->imu.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL);
-        setAccelerationReady();
-    }
+    setAcceleration(imu.getVector(Adafruit_BNO055::VECTOR_LINEARACCEL));
 #endif
 }
 

diff --git a/src/sensors/bno080sensor.cpp b/src/sensors/bno080sensor.cpp
@@ -116,29 +116,30 @@ void BNO080Sensor::motionLoop()
 #if USE_6_AXIS
         if (imu.hasNewGameQuat()) // New quaternion if context
         {
-            imu.getGameQuat(fusedRotation.x, fusedRotation.y, fusedRotation.z, fusedRotation.w, calibrationAccuracy);
-            fusedRotation *= sensorOffset;
+            Quat nRotation;
+            imu.getGameQuat(nRotation.x, nRotation.y, nRotation.z, nRotation.w, calibrationAccuracy);
 
-            setFusedRotationReady();
+            setFusedRotation(nRotation);
             // Leave new quaternion if context open, it's closed later
 
 #else // USE_6_AXIS
 
         if (imu.hasNewQuat()) // New quaternion if context
         {
-            imu.getQuat(fusedRotation.x, fusedRotation.y, fusedRotation.z, fusedRotation.w, magneticAccuracyEstimate, calibrationAccuracy);
-            fusedRotation *= sensorOffset;
+            Quat nRotation;
+            imu.getQuat(nRotation.x, nRotation.y, nRotation.z, nRotation.w, magneticAccuracyEstimate, calibrationAccuracy);
 
-            setFusedRotationReady();
+            setFusedRotation(nRotation);
             // Leave new quaternion if context open, it's closed later
 #endif // USE_6_AXIS
 
             // Continuation of the new quaternion if context, used for both 6 and 9 axis
 #if SEND_ACCELERATION
             {
                 uint8_t acc;
-                imu.getLinAccel(acceleration.x, acceleration.y, acceleration.z, acc);
-                setAccelerationReady();
+                Vector3 nAccel;
+                imu.getLinAccel(nAccel.x, nAccel.y, nAccel.z, acc);
+                setAcceleration(nAccel);
             }
 #endif // SEND_ACCELERATION
         } // Closing new quaternion if context

diff --git a/src/sensors/icm20948sensor.cpp b/src/sensors/icm20948sensor.cpp
@@ -347,18 +347,13 @@ void ICM20948Sensor::readRotation()
             double q2 = ((double)dmpData.Quat6.Data.Q2) / DMPNUMBERTODOUBLECONVERTER; // Convert to double. Divide by 2^30
             double q3 = ((double)dmpData.Quat6.Data.Q3) / DMPNUMBERTODOUBLECONVERTER; // Convert to double. Divide by 2^30
             double q0 = sqrt(1.0 - ((q1 * q1) + (q2 * q2) + (q3 * q3)));
-            fusedRotation.w = q0;
-            fusedRotation.x = q1;
-            fusedRotation.y = q2;
-            fusedRotation.z = q3;
+            Quat nRotation(q1, q2, q3, q0); // x, y, z, w
 
             #if SEND_ACCELERATION
-            calculateAccelerationWithoutGravity(&fusedRotation);
+            calculateAccelerationWithoutGravity(&nRotation);
             #endif
 
-            fusedRotation *= sensorOffset; //imu rotation
-
-            newFusedRotation = true;
+            setFusedRotation(nRotation);
             lastData = millis();
         }
     }
@@ -374,18 +369,13 @@ void ICM20948Sensor::readRotation()
             double q2 = ((double)dmpData.Quat9.Data.Q2) / DMPNUMBERTODOUBLECONVERTER; // Convert to double. Divide by 2^30
             double q3 = ((double)dmpData.Quat9.Data.Q3) / DMPNUMBERTODOUBLECONVERTER; // Convert to double. Divide by 2^30
             double q0 = sqrt(1.0 - ((q1 * q1) + (q2 * q2) + (q3 * q3)));
-            fusedRotation.w = q0;
-            fusedRotation.x = q1;
-            fusedRotation.y = q2;
-            fusedRotation.z = q3;
+            Quat nRotation(q1, q2, q3, q0); // x, y, z, w
 
             #if SEND_ACCELERATION
-            calculateAccelerationWithoutGravity(&fusedRotation);
+            calculateAccelerationWithoutGravity(&nRotation);
             #endif
 
-            fusedRotation *= sensorOffset; //imu rotation
-
-            newFusedRotation = true;
+            setFusedRotation(nRotation);
             lastData = millis();
         }
     }
@@ -508,8 +498,7 @@ void ICM20948Sensor::calculateAccelerationWithoutGravity(Quat *quaternion)
                             };
             sfusion.updateAcc(Axyz);
 
-            acceleration = sfusion.getLinearAccVec();
-			this->newAcceleration = true;
+            setAcceleration(sfusion.getLinearAccVec());
         }
     }
     #endif

diff --git a/src/sensors/icm42688sensor.cpp b/src/sensors/icm42688sensor.cpp
@@ -172,11 +172,8 @@ void ICM42688Sensor::motionLoop() {
     if (magExists)
         sfusion.updateMag(Mxyz);
 
-    fusedRotation = sfusion.getQuaternionQuat();
-    fusedRotation *= sensorOffset;
-    acceleration = sfusion.getLinearAccVec();
-    setFusedRotationReady();
-    setAccelerationReady();
+    setFusedRotation(sfusion.getQuaternionQuat());
+    setAcceleration(sfusion.getLinearAccVec());
 }
 
 void ICM42688Sensor::accel_read() {

diff --git a/src/sensors/mpu6050sensor.cpp b/src/sensors/mpu6050sensor.cpp
@@ -139,9 +139,7 @@ void MPU6050Sensor::motionLoop()
 
         sfusion.updateQuaternion(rawQuat);
 
-        fusedRotation = sfusion.getQuaternionQuat();
-        fusedRotation *= sensorOffset;
-        setFusedRotationReady();
+        setFusedRotation(sfusion.getQuaternionQuat());
 
         #if SEND_ACCELERATION
         {
@@ -153,8 +151,7 @@ void MPU6050Sensor::motionLoop()
 
             sfusion.updateAcc(Axyz);
 
-            acceleration = sfusion.getLinearAccVec();
-			setAccelerationReady();
+			setAcceleration(sfusion.getLinearAccVec());
         }
         #endif
     }

diff --git a/src/sensors/mpu9250sensor.cpp b/src/sensors/mpu9250sensor.cpp
@@ -162,22 +162,15 @@ void MPU9250Sensor::motionLoop() {
     parseMagData(temp);
 
     sfusion.updateMag(Mxyz);
-
-    fusedRotation = sfusion.getQuaternionQuat();
-
     #if SEND_ACCELERATION
     {
         int16_t atemp[3];
         this->imu.dmpGetAccel(atemp, dmpPacket);
         parseAccelData(atemp);
 
         sfusion.updateAcc(Axyz);
-
-        acceleration = sfusion.getLinearAccVec();
-		setAccelerationReady();
     }
     #endif
-
 #else
     union fifo_sample_raw buf;
     uint16_t remaining_samples;
@@ -196,16 +189,11 @@ void MPU9250Sensor::motionLoop() {
 
         sfusion.update9D(Axyz, Gxyz, Mxyz);
     }
-
-    fusedRotation = sfusion.getQuaternionQuat();
-
+#endif
+    setFusedRotation(sfusion.getQuaternionQuat());
     #if SEND_ACCELERATION
-    acceleration = sfusion.getLinearAccVec();
-	setAccelerationReady();
+	setAcceleration(sfusion.getLinearAccVec());
     #endif
-#endif
-    fusedRotation *= sensorOffset;
-    setFusedRotationReady();
 }
 
 void MPU9250Sensor::startCalibration(int calibrationType) {

diff --git a/src/sensors/sensor.cpp b/src/sensors/sensor.cpp
@@ -29,11 +29,13 @@ SensorStatus Sensor::getSensorState() {
     return isWorking() ? SensorStatus::SENSOR_OK : SensorStatus::SENSOR_ERROR;
 }
 
-void Sensor::setAccelerationReady() {
+void Sensor::setAcceleration(Vector3 a) {
+    acceleration = a;
     newAcceleration = true;
 }
 
-void Sensor::setFusedRotationReady() {
+void Sensor::setFusedRotation(Quat r) {
+    fusedRotation = r * sensorOffset;
     bool changed = OPTIMIZE_UPDATES ? !lastFusedRotationSent.equalsWithEpsilon(fusedRotation) : true;
     if (ENABLE_INSPECTION || changed) {
         newFusedRotation = true;

diff --git a/src/sensors/sensor.h b/src/sensors/sensor.h
@@ -58,8 +58,8 @@ class Sensor
     virtual void postSetup(){};
     virtual void motionLoop(){};
     virtual void sendData();
-    virtual void setAccelerationReady();
-    virtual void setFusedRotationReady();
+    virtual void setAcceleration(Vector3 a);
+    virtual void setFusedRotation(Quat r);
     virtual void startCalibration(int calibrationType){};
     virtual SensorStatus getSensorState();
     virtual void printTemperatureCalibrationState();