fix Imu icm42688 (#290)

SlimeVR · Sep 22, 2023 · 07785c8 · 07785c8
1 parent 1ed8d63
commit 07785c8
Show file tree

Hide file tree

Showing 2 changed files with 22 additions and 24 deletions.
diff --git a/src/sensors/icm42688sensor.cpp b/src/sensors/icm42688sensor.cpp
@@ -20,7 +20,6 @@
 */
 
 #include "icm42688sensor.h"
-#include "network/network.h"
 #include "globals.h"
 #include "helper_3dmath.h"
 #include <i2cscan.h>
@@ -44,7 +43,7 @@ void ICM42688Sensor::motionSetup() {
 
     m_Logger.info("Connected to ICM42688 (reported device ID 0x%02x) at address 0x%02x", temp, addr);
 
-    if (I2CSCAN::isI2CExist(addr_mag)) {
+    if (I2CSCAN::hasDevOnBus(addr_mag)) {
         I2Cdev::readByte(addr_mag, MMC5983MA_PRODUCT_ID, &temp);
         if(!(temp == 0x30)) {
             m_Logger.fatal("Can't connect to MMC5983MA (reported device ID 0x%02x) at address 0x%02x", temp, addr_mag);
@@ -54,10 +53,11 @@ void ICM42688Sensor::motionSetup() {
             m_Logger.info("Connected to MMC5983MA (reported device ID 0x%02x) at address 0x%02x", temp, addr_mag);
             magExists = true;
         }
+    } else {
+        m_Logger.info("Magnetometer unavailable!");
+        magExists = false;
     }
 
-    deltat = 1.0 / 1000.0; // gyro fifo 1khz
-
     if (magExists) {
         I2Cdev::writeByte(addr_mag, MMC5983MA_CONTROL_1, 0x80); // Reset MMC now
     }
@@ -136,7 +136,6 @@ void ICM42688Sensor::motionLoop() {
 	count += 32; // Add a few read buffer packets (4 ms)
 	uint16_t packets = count / 8;								 // Packet size 8 bytes
 	uint8_t rawData[2080];
-	uint16_t stco = 0;
     I2Cdev::readBytes(addr, ICM42688_FIFO_DATA, count, &rawData[0]); // Read buffer
 
     accel_read();
@@ -164,22 +163,20 @@ void ICM42688Sensor::motionLoop() {
 		Gxyz[2] = raw2 * gscale; //gres
         parseGyroData();
 
-        // TODO: mag axes will be different, make sure to change them here
-        #if defined(_MAHONY_H_)
-        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_)
-        madgwickQuaternionUpdate(q, Axyz[0], Axyz[1], Axyz[2], Gxyz[0], Gxyz[1], Gxyz[2], Mxyz[0], Mxyz[1], Mxyz[2], deltat * 1.0e-6);
-        #endif
+        // TODO: mag axes will be different, make sure to change them???
+        sfusion.updateGyro(Gxyz);
     }
-
-    quaternion.set(-q[2], q[1], q[3], q[0]);
 
-    quaternion *= sensorOffset;
+    sfusion.updateAcc(Axyz);
 
-    if(!lastQuatSent.equalsWithEpsilon(quaternion)) {
-        newData = true;
-        lastQuatSent = quaternion;
-    }
+    if (magExists)
+        sfusion.updateMag(Mxyz);
+
+    fusedRotation = sfusion.getQuaternionQuat();
+    fusedRotation *= sensorOffset;
+    acceleration = sfusion.getLinearAccVec();
+    setFusedRotationReady();
+    setAccelerationReady();
 }
 
 void ICM42688Sensor::accel_read() {
@@ -310,7 +307,6 @@ void ICM42688Sensor::startCalibration(int calibrationType) {
     configuration.save();
 
     ledManager.off();
-    Network::sendCalibrationFinished(CALIBRATION_TYPE_EXTERNAL_ALL, 0);
     m_Logger.debug("Saved the calibration data");
 
     m_Logger.info("Calibration data gathered");

diff --git a/src/sensors/icm42688sensor.h b/src/sensors/icm42688sensor.h
@@ -31,10 +31,14 @@
 #include <MMC5983MA.h>
 #include "I2Cdev.h"
 
+#include "SensorFusion.h"
+
 class ICM42688Sensor : public Sensor
 {
 public:
-    ICM42688Sensor(uint8_t id, uint8_t address, float rotation) : Sensor("ICM42688Sensor", IMU_ICM42688, id, address, rotation){};
+    ICM42688Sensor(uint8_t id, uint8_t address, float rotation, uint8_t sclPin, uint8_t sdaPin)
+        : Sensor("ICM42688Sensor", IMU_ICM42688, id, address, rotation, sclPin, sdaPin),
+        sfusion(0.001f, 0.01f, 0.01f){};
     ~ICM42688Sensor(){};
     void motionSetup() override final;
     void motionLoop() override final;
@@ -45,13 +49,11 @@ class ICM42688Sensor : public Sensor
     bool magExists = false;
 
     // raw data and scaled as vector
-    float q[4]{1.0f, 0.0f, 0.0f, 0.0f}; // for raw filter
     float Axyz[3]{};
     float Gxyz[3]{};
     float Mxyz[3]{};
-    Quat correction{0, 0, 0, 0};
-    // Loop timing globals
-    float deltat = 0;                // sample time in seconds
+
+    SlimeVR::Sensors::SensorFusion sfusion;
 
     SlimeVR::Configuration::ICM42688CalibrationConfig m_Calibration;