MPU9150 DMP AK8975电子罗盘

yshihyu

MPU9150 DMP , 我在 6050使用DMP 可以取得精确度 pitch and roll angle , 只是 yaw angle 有问题, 所以需要 magnetometer 修正 yaw angle , 所以我想使用 MPU9150 gyroscope accelerometer magnetometer 让 DMP 整合计算出来
mpu9150test.ino 里面 loop 函数里面
mpu.dmpGetMag(mag, fifoBuffer);
取出来 mag都是0

MPU9150_9Axis_MotionApps41.h 跟 dmp 有关
里面的 dmpInitialize 函数跟 magnetometer 有关系的函数是下面几段程式码 ,

1. uint8_t dmpInitialize() {
   
2.     DEBUG_PRINTLN(F("Setting magnetometer mode to power-down..."));
   
3. //mag -> setMode(0);
   
4.     I2Cdev::writeByte(0x0E, 0x0A, 0x00);
   
5.     DEBUG_PRINTLN(F("Setting magnetometer mode to fuse access..."));
   
6. //mag -> setMode(0x0F);
   
7.     I2Cdev::writeByte(0x0E, 0x0A, 0x0F);
   
8.     DEBUG_PRINTLN(F("Reading mag magnetometer factory calibration..."));
   
9.     int8_t asax, asay, asaz;
   
10. //mag -> getAdjustment(&asax, &asay, &asaz);
    
11.     DEBUG_PRINTLN(F("Setting magnetometer mode to power-down..."));
    
12. //mag -> setMode(0);
    
13.     DEBUG_PRINTLN(F("Setting AK8975 to single measurement mode..."));
    
14. //mag -> setMode(1);
    
15.     I2Cdev::writeByte(0x0E, 0x0A, 0x01);
    
16. }

复制代码

但是 MPU9150.cpp 使用 magnetometer 使用getMotion9函数取raw data可以取到 , 而里面的 MPU9150_RA_MAG_ADDRESS是 0x0C 跟DMP 使用的 0x0E 为什么不同？ 我有试过把 0x0E 改成 0x0C 不过DMP 还是得不到 magnetometer 资料

1. void MPU9150::getMotion9(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz, int16_t* mx, int16_t* my, int16_t* mz) {
   
2. //get accel and gyro
   
3.     getMotion6(ax, ay, az, gx, gy, gz);
   
4. //read mag
   
5.     I2Cdev::writeByte(devAddr, MPU9150_RA_INT_PIN_CFG, 0x02); //set i2c bypass enable pin to true to access magnetometer
   
6.     delay(10);
   
7.     I2Cdev::writeByte(MPU9150_RA_MAG_ADDRESS, 0x0A, 0x01); //enable the magnetometer
   
8.     delay(10);
   
9.     I2Cdev::readBytes(MPU9150_RA_MAG_ADDRESS, MPU9150_RA_MAG_XOUT_L, 6, buffer);
   
10.     *mx = (((int16_t)buffer[0]) << 8) | buffer[1];
    
11.     *my = (((int16_t)buffer[2]) << 8) | buffer[3];
    
12.     *mz = (((int16_t)buffer[4]) << 8) | buffer[5];
    
13. }

复制代码

gaojunchen

lz，能否把源码打包一下上传？官方的可以下载到的代码里没有看到过你上面代码的内容

yshihyu

源码打包了by Jeff Rowberg <jeff@rowberg.net>这作者应该不是官方的代码

yshihyu

MPU9150Lib 后来我官方有找到下面代码 ,

void dataFusion();                                        // fuse mag data with the dmp quaternion

这样代表 dmp 没有把mag data真正作到硬件?  


我没权限可以贴网址所以贴关键代码

1. boolean MPU9150Lib::read()
   
2. {
   
3.     short intStatus;
   
4.     int result;
   
5.     short sensors;
   
6.     unsigned char more;
   
7.     unsigned long timestamp;
   
8.    
   
9.     mpu_get_int_status(&intStatus);                       // get the current MPU state
   
10.     if ((intStatus & MPU_INT_STATUS_DMP_0) == 0)          // return false if definitely not ready yet
    
11.         return false;
    
12.         
    
13.     //  get the data from the fifo
    
14.         
    
15.     if ((result = dmp_read_fifo(m_rawGyro, m_rawAccel, m_rawQuaternion, &timestamp, &sensors, &more)) != 0) {
    
16.       return false;
    
17.     }      
    
18.    
    
19.     //  got the fifo data so now get the mag data
    
20.    
    
21.     if ((result = mpu_get_compass_reg(m_rawMag, &timestamp)) != 0) {
    
22. #ifdef MPULIB_DEBUG
    
23.          Serial.print("Failed to read compass: ");
    
24.          Serial.println(result);
    
25.          return false;
    
26. #endif
    
27.     }
    
28.    
    
29.     // got the raw data - now process
    
30.    
    
31.     m_dmpQuaternion[QUAT_W] = (float)m_rawQuaternion[QUAT_W];  // get float version of quaternion
    
32.     m_dmpQuaternion[QUAT_X] = (float)m_rawQuaternion[QUAT_X];
    
33.     m_dmpQuaternion[QUAT_Y] = (float)m_rawQuaternion[QUAT_Y];
    
34.     m_dmpQuaternion[QUAT_Z] = (float)m_rawQuaternion[QUAT_Z];
    
35.     MPUQuaternionNormalize(m_dmpQuaternion);                 // and normalize
    
36.    
    
37.     MPUQuaternionQuaternionToEuler(m_dmpQuaternion, m_dmpEulerPose);
    
38. 
    
39.         //        *** Note mag axes are changed here to align with gyros: Y = -X, X = Y
    
40. 
    
41.     if (m_useMagCalibration) {
    
42.       m_calMag[VEC3_Y] = -(short)(((long)(m_rawMag[VEC3_X] - m_magXOffset) * (long)SENSOR_RANGE) / (long)m_magXRange);
    
43.       m_calMag[VEC3_X] = (short)(((long)(m_rawMag[VEC3_Y] - m_magYOffset) * (long)SENSOR_RANGE) / (long)m_magYRange);
    
44.       m_calMag[VEC3_Z] = (short)(((long)(m_rawMag[VEC3_Z] - m_magZOffset) * (long)SENSOR_RANGE) / (long)m_magZRange);
    
45.     } else {
    
46.       m_calMag[VEC3_Y] = -m_rawMag[VEC3_X];
    
47.       m_calMag[VEC3_X] = m_rawMag[VEC3_Y];
    
48.       m_calMag[VEC3_Z] = m_rawMag[VEC3_Z];
    
49.     }
    
50. 
    
51.     // Scale accel data
    
52. 
    
53.     if (m_useAccelCalibration) {
    
54.       m_calAccel[VEC3_X] = -(short)(((long)m_rawAccel[VEC3_X] * (long)SENSOR_RANGE) / (long)m_accelXRange);
    
55.       m_calAccel[VEC3_Y] = (short)(((long)m_rawAccel[VEC3_Y] * (long)SENSOR_RANGE) / (long)m_accelYRange);
    
56.       m_calAccel[VEC3_Z] = (short)(((long)m_rawAccel[VEC3_Z] * (long)SENSOR_RANGE) / (long)m_accelZRange);
    
57.     } else {
    
58.       m_calAccel[VEC3_X] = -m_rawAccel[VEC3_X];
    
59.       m_calAccel[VEC3_Y] = m_rawAccel[VEC3_Y];
    
60.       m_calAccel[VEC3_Z] = m_rawAccel[VEC3_Z];
    
61.     }
    
62.     dataFusion();
    
63.     return true;
    
64. }
    
65. 
    
66. void MPU9150Lib::dataFusion()
    
67. {
    
68.   float qMag[4];
    
69.   float deltaDMPYaw, deltaMagYaw;
    
70.   float newMagYaw, newYaw;
    
71.   float temp1[4], unFused[4];
    
72.   float unFusedConjugate[4];
    
73. 
    
74.   // *** NOTE *** pitch direction swapped here
    
75. 
    
76.   m_fusedEulerPose[VEC3_X] = m_dmpEulerPose[VEC3_X];
    
77.   m_fusedEulerPose[VEC3_Y] = -m_dmpEulerPose[VEC3_Y];
    
78.   m_fusedEulerPose[VEC3_Z] = 0;       
    
79.   MPUQuaternionEulerToQuaternion(m_fusedEulerPose, unFused);    // create a new quaternion
    
80. 
    
81.   deltaDMPYaw = -m_dmpEulerPose[VEC3_Z] + m_lastDMPYaw;         // calculate change in yaw from dmp
    
82.   m_lastDMPYaw = m_dmpEulerPose[VEC3_Z];                        // update that
    
83. 
    
84.   qMag[QUAT_W] = 0;
    
85.   qMag[QUAT_X] = m_calMag[VEC3_X];
    
86.   qMag[QUAT_Y] = m_calMag[VEC3_Y];
    
87.   qMag[QUAT_Z] = m_calMag[VEC3_Z];
    
88.        
    
89.   // Tilt compensate mag with the unfused data (i.e. just roll and pitch with yaw 0)
    
90.        
    
91.   MPUQuaternionConjugate(unFused, unFusedConjugate);
    
92.   MPUQuaternionMultiply(qMag, unFusedConjugate, temp1);
    
93.   MPUQuaternionMultiply(unFused, temp1, qMag);
    
94. 
    
95.   // Now fuse this with the dmp yaw gyro information
    
96. 
    
97.   newMagYaw = -atan2(qMag[QUAT_Y], qMag[QUAT_X]);
    
98. 
    
99.   if (newMagYaw != newMagYaw) {                                 // check for nAn
    
100. #ifdef MPULIB_DEBUG
     
101.     Serial.println("***nAn\n");
     
102. #endif
     
103.     return;                                                     // just ignore in this case
     
104.   }
     
105.   if (newMagYaw < 0)
     
106.     newMagYaw = 2.0f * (float)M_PI + newMagYaw;                 // need 0 <= newMagYaw <= 2*PI
     
107. 
     
108.   newYaw = m_lastYaw + deltaDMPYaw;                             // compute new yaw from change
     
109.   if (newYaw > (2.0f * (float)M_PI))                            // need 0 <= newYaw <= 2*PI
     
110.     newYaw -= 2.0f * (float)M_PI;
     
111.   if (newYaw < 0)
     
112.     newYaw += 2.0f * (float)M_PI;
     
113. 
     
114.   deltaMagYaw = newMagYaw - newYaw;                             // compute difference
     
115.   if (deltaMagYaw >= (float)M_PI)
     
116.     deltaMagYaw = deltaMagYaw - 2.0f * (float)M_PI;
     
117.   if (deltaMagYaw <= -(float)M_PI)
     
118.     deltaMagYaw = (2.0f * (float)M_PI + deltaMagYaw);
     
119. 
     
120.   newYaw += deltaMagYaw/4;                                      // apply some of the correction
     
121. 
     
122.   if (newYaw > (2.0f * (float)M_PI))                                            // need 0 <= newYaw <= 2*PI
     
123.     newYaw -= 2.0f * (float)M_PI;
     
124.   if (newYaw < 0)
     
125.     newYaw += 2.0f * (float)M_PI;
     
126. 
     
127.   m_lastYaw = newYaw;
     
128. 
     
129.   if (newYaw > (float)M_PI)
     
130.     newYaw -= 2.0f * (float)M_PI;
     
131. 
     
132.   m_fusedEulerPose[VEC3_Z] = newYaw;                            // fill in output yaw value
     
133.        
     
134.   MPUQuaternionEulerToQuaternion(m_fusedEulerPose, m_fusedQuaternion);
     
135. }

复制代码

Qing松

我没用DMP，自己从MPU9250里面读取AK8963的数据，因为我是用的SPI接口，所有也不能直接读取AK8963,因为AK8963是通过6500的IIC下挂的，所有要使用MPU9250内部的SLV4（从机配置4）来读取写入AK8963寄存器，用SLV0来循环读取AK8963的数据。先前就要配置好SLV0 和SLV4，，，楼主应该找找他是如何配置的。