Revert "Enable MPU9250 madgwick by default"

This reverts commit 7e270c4.
Timocop · Sep 13, 2023 · bdd171a · bdd171a
1 parent a5a97ef
commit bdd171a
Show file tree

Hide file tree

Showing 2 changed files with 8 additions and 14 deletions.
diff --git a/src/sensors/mpu9250sensor.cpp b/src/sensors/mpu9250sensor.cpp
@@ -28,7 +28,7 @@
 #include "magneto1.4.h"
 #include "GlobalVars.h"
 // #include "mahony.h"
-#include "madgwick.h"
+// #include "madgwick.h"
 #if not (defined(_MAHONY_H_) || defined(_MADGWICK_H_))
 #include "dmpmag.h"
 #endif
@@ -39,8 +39,6 @@ constexpr float gscale = (250. / 32768.0) * (PI / 180.0); //gyro default 250 LSB
 
 #define MAG_CORR_RATIO 0.02
 
-#define MADGWICK_UPDATE_RATE_MS 5.f
-
 #define ACCEL_SENSITIVITY_2G 16384.0f
 
 // Accel scale conversion steps: LSB/G -> G -> m/s^2
@@ -130,14 +128,16 @@ void MPU9250Sensor::motionSetup() {
     imu.setZGyroFIFOEnabled(true);
     imu.setSlave0FIFOEnabled(true);
 
+    // TODO: set a rate we prefer instead of getting the current rate from the device.
+    deltat = 1.0 / 1000.0 * (1 + imu.getRate());
+    //imu.setRate(blah);
+
     imu.resetFIFO();
     imu.setFIFOEnabled(true);
 
     working = true;
     configured = true;
 #endif
-
-    lastMadgwick = millis();
 }
 
 void MPU9250Sensor::motionLoop() {
@@ -222,17 +222,11 @@ void MPU9250Sensor::motionLoop() {
         // TODO: monitor remaining_samples to ensure that the number is going down, not up.
         // remaining_samples
 
-        if ((millis() - lastMadgwick) > MADGWICK_UPDATE_RATE_MS)
-        {
         #if defined(_MAHONY_H_)
-            mahonyQuaternionUpdate(q, Axyz[0], Axyz[1], Axyz[2], Gxyz[0], Gxyz[1], Gxyz[2], Mxyz[0], Mxyz[1], Mxyz[2], (millis() - lastMadgwick) / 1000.f);
+        mahonyQuaternionUpdate(q, Axyz[0], Axyz[1], Axyz[2], Gxyz[0], Gxyz[1], Gxyz[2], Mxyz[0], Mxyz[1], Mxyz[2], deltat * 1.0e-6);
         #elif defined(_MADGWICK_H_)
-
-            madgwickQuaternionUpdateStable(q, Axyz[0], Axyz[1], Axyz[2], Gxyz[0], Gxyz[1], Gxyz[2], Mxyz[0], Mxyz[1], Mxyz[2], (millis() - lastMadgwick) / 1000.f);
+        madgwickQuaternionUpdateStable(q, Axyz[0], Axyz[1], Axyz[2], Gxyz[0], Gxyz[1], Gxyz[2], Mxyz[0], Mxyz[1], Mxyz[2], deltat * 1.0e-6);
         #endif
-
-            lastMadgwick = millis();
-        }
     }
 
     quaternion.set(-q[2], q[1], q[3], q[0]);

diff --git a/src/sensors/mpu9250sensor.h b/src/sensors/mpu9250sensor.h
@@ -54,7 +54,7 @@ class MPU9250Sensor : public Sensor
     VectorInt16 rawAccel{};
     Quat correction{0, 0, 0, 0};
     // Loop timing globals
-    unsigned long lastMadgwick = 0;
+    float deltat = 0;                // sample time in seconds
 
     SlimeVR::Configuration::MPU9250CalibrationConfig m_Calibration;