From 00461f6dc5a9dbca4a70a25b79eabfb5825e2f5e Mon Sep 17 00:00:00 2001
From: LETBBI <ghghdirk12@gmail.com>
Date: Sat, 5 Feb 2022 23:34:22 +0900
Subject: [PATCH 1/3] Delete mahony function and add define check for mpu9250

---
 src/sensors/mpu9250sensor.cpp | 2 +-
 src/sensors/mpu9250sensor.h   | 1 -
 2 files changed, 1 insertion(+), 2 deletions(-)

diff --git a/src/sensors/mpu9250sensor.cpp b/src/sensors/mpu9250sensor.cpp
index 8805bf1..ebf8d99 100644
--- a/src/sensors/mpu9250sensor.cpp
+++ b/src/sensors/mpu9250sensor.cpp
@@ -65,7 +65,7 @@ void MPU9250Sensor::motionSetup() {
         if(az>0 && 10.0*(ax*ax+ay*ay)<az*az) 
             startCalibration(0);
     }
-#ifndef _MAHONY_H_
+#if not (defined(_MAHONY_H_) || defined(_MADGWICK_H_))
     devStatus = imu.dmpInitialize();
     if(devStatus == 0){
         for(int i = 0; i < 5; ++i) {
diff --git a/src/sensors/mpu9250sensor.h b/src/sensors/mpu9250sensor.h
index c0df441..e38b6f1 100644
--- a/src/sensors/mpu9250sensor.h
+++ b/src/sensors/mpu9250sensor.h
@@ -33,7 +33,6 @@ public:
     void motionLoop() override final;
     void startCalibration(int calibrationType) override final;
     void getMPUScaled();
-    void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz, float deltat);
 
 private:
     MPU9250 imu{};

From fbd74e9a97e62d63a4e5525fac2bc4010b09a9ae Mon Sep 17 00:00:00 2001
From: LETBBI <ghghdirk12@gmail.com>
Date: Sat, 5 Feb 2022 23:34:56 +0900
Subject: [PATCH 2/3] Fix dmp sample rate

---
 lib/mpu9250/MPU9250_6Axis_MotionApps_V6_12.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/lib/mpu9250/MPU9250_6Axis_MotionApps_V6_12.cpp b/lib/mpu9250/MPU9250_6Axis_MotionApps_V6_12.cpp
index 2cfdf44..c0dc2a1 100644
--- a/lib/mpu9250/MPU9250_6Axis_MotionApps_V6_12.cpp
+++ b/lib/mpu9250/MPU9250_6Axis_MotionApps_V6_12.cpp
@@ -359,7 +359,7 @@ uint8_t MPU9250::dmpInitialize() { // Lets get it over with fast Write everythin
 	I2Cdev::writeBytes(devAddr,0x1C, 1, &(val = 0x00)); // 0000 0000 ACCEL_CONFIG: 0 =  Accel Full Scale Select: 2g
 	I2Cdev::writeBytes(devAddr,0x37, 1, &(val = 0x80)); // 1001 0000 INT_PIN_CFG: ACTL The logic level for int pin is active low. and interrupt status bits are cleared on any read
 	I2Cdev::writeBytes(devAddr,0x6B, 1, &(val = 0x01)); // 0000 0001 PWR_MGMT_1: Clock Source Select PLL_X_gyro
-	I2Cdev::writeBytes(devAddr,0x19, 1, &(val = 0x04)); // 0000 0100 SMPLRT_DIV: Divides the internal sample rate 400Hz ( Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV))
+	I2Cdev::writeBytes(devAddr,0x19, 1, &(val = 0x02)); // 0000 0100 SMPLRT_DIV: Divides the internal sample rate 400Hz ( Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV))
 	I2Cdev::writeBytes(devAddr,0x1A, 1, &(val = 0x01)); // 0000 0001 CONFIG: Digital Low Pass Filter (DLPF) Configuration 188HZ  //Im betting this will be the beat
 	if (!writeProgMemoryBlock(dmpMemory, MPU9250_DMP_CODE_SIZE)) return 1; // Loads the DMP image into the MPU9250 Memory // Should Never Fail
 	I2Cdev::writeWords(devAddr, 0x70, 1, &(ival = 0x0400)); // DMP Program Start Address

From cc1d6e83fb7eab3a7649f763b6f633d33fafe10e Mon Sep 17 00:00:00 2001
From: LETBBI <ghghdirk12@gmail.com>
Date: Sat, 5 Feb 2022 23:37:42 +0900
Subject: [PATCH 3/3] Move deltaT

this for raw data
---
 src/sensors/mpu9250sensor.cpp | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/sensors/mpu9250sensor.cpp b/src/sensors/mpu9250sensor.cpp
index ebf8d99..1854e6f 100644
--- a/src/sensors/mpu9250sensor.cpp
+++ b/src/sensors/mpu9250sensor.cpp
@@ -106,9 +106,6 @@ void MPU9250Sensor::motionSetup() {
 
 
 void MPU9250Sensor::motionLoop() {
-    unsigned long now = micros();
-    unsigned long deltat = now - last; //seconds since last update
-    last = now;
 #if not (defined(_MAHONY_H_) || defined(_MADGWICK_H_))
     // Update quaternion
     if(!dmpReady)
@@ -135,6 +132,9 @@ void MPU9250Sensor::motionLoop() {
     }else skipCalcMag--;
     quaternion=correction*quat;
 #else
+    unsigned long now = micros();
+    unsigned long deltat = now - last; //seconds since last update
+    last = now;
     getMPUScaled();
     mahonyQuaternionUpdate(q, Axyz[0], Axyz[1], Axyz[2], Gxyz[0], Gxyz[1], Gxyz[2], Mxyz[0], Mxyz[1], Mxyz[2], deltat * 1.0e-6);
     // madgwickQuaternionUpdate(q, Axyz[0], Axyz[1], Axyz[2], Gxyz[0], Gxyz[1], Gxyz[2], Mxyz[0], Mxyz[1], Mxyz[2], deltat * 1.0e-6);