Diff: MPU9250/MPU9250.cpp

Revision:

1:f31e17341659

Parent:

0:92024886c0be

diff -r 92024886c0be -r f31e17341659 MPU9250/MPU9250.cpp
--- a/MPU9250/MPU9250.cpp	Tue Aug 01 12:27:13 2017 +0000
+++ b/MPU9250/MPU9250.cpp	Tue Aug 28 07:12:16 2018 +0000
@@ -3,7 +3,7 @@
 #include "MPU9250.h"
 
 
-MPU9250::MPU9250(PinName sda, PinName scl, Serial* serial_p)
+MPU9250::MPU9250(PinName sda, PinName scl, RawSerial* serial_p)
     :
     i2c_p(new I2C(sda,scl)),
     i2c(*i2c_p),
@@ -90,6 +90,21 @@
     degree[2] = yaw;
 }
 
+
+float MPU9250::calculateYawByMg(){
+    transformCoordinateFromCompassToMPU();
+    lpmag[0] = LPGAIN_MAG *lpmag[0] + (1 - LPGAIN_MAG)*mx;
+    lpmag[1] = LPGAIN_MAG *lpmag[1] + (1 - LPGAIN_MAG)*my;
+    lpmag[2] = LPGAIN_MAG *lpmag[2] + (1 - LPGAIN_MAG)*mz;
+    
+    float radroll = PI/180.0f * roll;
+    float radpitch = PI/180.0f * pitch;
+
+    return 180.0f/PI * atan2(lpmag[2]*sin(radpitch) - lpmag[1]*cos(radpitch),
+                                         lpmag[0]*cos(radroll) - lpmag[1]*sin(radroll)*sin(radpitch) + lpmag[2]*sin(radroll)*cos(radpitch));
+}
+
+
 // Accelerometer and gyroscope self test; check calibration wrt factory settings
 void MPU9250::MPU9250SelfTest(float * destination) // Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass
 {
@@ -295,6 +310,8 @@
         magbias[i] = 0;
 
         eInt[i] = 0.0f; 
+    
+        lpmag[i] = 0.0f;
     }
 
     q[0] = 1.0f;
@@ -874,4 +891,11 @@
   yaw   *= 180.0f / PI; 
   yaw   -= DECLINATION;
   roll  *= 180.0f / PI; 
+}
+
+void MPU9250::transformCoordinateFromCompassToMPU(){
+    float buf = mx;
+    mx = my;
+    my = buf;
+    mz = -mz;
 }
\ No newline at end of file