not include takeoff
Dependencies: HCSR04_2 MPU6050_2 mbed SDFileSystem3
Fork of AutoFlight2017_now2 by
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