MPU9150 DMP AK8975电子罗盘
本帖最后由 yshihyu 于 2015-12-27 22:54 编辑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 有关系的函数是下面几段程式码 ,
uint8_t dmpInitialize() {
DEBUG_PRINTLN(F("Setting magnetometer mode to power-down..."));
//mag -> setMode(0);
I2Cdev::writeByte(0x0E, 0x0A, 0x00);
DEBUG_PRINTLN(F("Setting magnetometer mode to fuse access..."));
//mag -> setMode(0x0F);
I2Cdev::writeByte(0x0E, 0x0A, 0x0F);
DEBUG_PRINTLN(F("Reading mag magnetometer factory calibration..."));
int8_t asax, asay, asaz;
//mag -> getAdjustment(&asax, &asay, &asaz);
DEBUG_PRINTLN(F("Setting magnetometer mode to power-down..."));
//mag -> setMode(0);
DEBUG_PRINTLN(F("Setting AK8975 to single measurement mode..."));
//mag -> setMode(1);
I2Cdev::writeByte(0x0E, 0x0A, 0x01);
}
但是 MPU9150.cpp 使用 magnetometer 使用getMotion9函数取raw data可以取到 , 而里面的 MPU9150_RA_MAG_ADDRESS是 0x0C 跟DMP 使用的 0x0E 为什么不同？ 我有试过把 0x0E 改成 0x0C 不过DMP 还是得不到 magnetometer 资料
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) {
//get accel and gyro
getMotion6(ax, ay, az, gx, gy, gz);
//read mag
I2Cdev::writeByte(devAddr, MPU9150_RA_INT_PIN_CFG, 0x02); //set i2c bypass enable pin to true to access magnetometer
delay(10);
I2Cdev::writeByte(MPU9150_RA_MAG_ADDRESS, 0x0A, 0x01); //enable the magnetometer
delay(10);
I2Cdev::readBytes(MPU9150_RA_MAG_ADDRESS, MPU9150_RA_MAG_XOUT_L, 6, buffer);
*mx = (((int16_t)buffer) << 8) | buffer;
*my = (((int16_t)buffer) << 8) | buffer;
*mz = (((int16_t)buffer) << 8) | buffer;
}
lz，能否把源码打包一下上传？官方的可以下载到的代码里没有看到过你上面代码的内容 本帖最后由 yshihyu 于 2015-12-27 23:25 编辑
源码打包了by Jeff Rowberg <jeff@rowberg.net>这作者应该不是官方的代码 本帖最后由 yshihyu 于 2015-12-27 23:26 编辑
MPU9150Lib 后来我官方有找到下面代码 ,
void dataFusion(); // fuse mag data with the dmp quaternion
这样代表 dmp 没有把mag data真正作到硬件?
我没权限可以贴网址所以贴关键代码
boolean MPU9150Lib::read()
{
short intStatus;
int result;
short sensors;
unsigned char more;
unsigned long timestamp;
mpu_get_int_status(&intStatus); // get the current MPU state
if ((intStatus & MPU_INT_STATUS_DMP_0) == 0) // return false if definitely not ready yet
return false;
//get the data from the fifo
if ((result = dmp_read_fifo(m_rawGyro, m_rawAccel, m_rawQuaternion, ×tamp, &sensors, &more)) != 0) {
return false;
}
//got the fifo data so now get the mag data
if ((result = mpu_get_compass_reg(m_rawMag, ×tamp)) != 0) {
#ifdef MPULIB_DEBUG
Serial.print("Failed to read compass: ");
Serial.println(result);
return false;
#endif
}
// got the raw data - now process
m_dmpQuaternion = (float)m_rawQuaternion;// get float version of quaternion
m_dmpQuaternion = (float)m_rawQuaternion;
m_dmpQuaternion = (float)m_rawQuaternion;
m_dmpQuaternion = (float)m_rawQuaternion;
MPUQuaternionNormalize(m_dmpQuaternion); // and normalize
MPUQuaternionQuaternionToEuler(m_dmpQuaternion, m_dmpEulerPose);
// *** Note mag axes are changed here to align with gyros: Y = -X, X = Y
if (m_useMagCalibration) {
m_calMag = -(short)(((long)(m_rawMag - m_magXOffset) * (long)SENSOR_RANGE) / (long)m_magXRange);
m_calMag = (short)(((long)(m_rawMag - m_magYOffset) * (long)SENSOR_RANGE) / (long)m_magYRange);
m_calMag = (short)(((long)(m_rawMag - m_magZOffset) * (long)SENSOR_RANGE) / (long)m_magZRange);
} else {
m_calMag = -m_rawMag;
m_calMag = m_rawMag;
m_calMag = m_rawMag;
}
// Scale accel data
if (m_useAccelCalibration) {
m_calAccel = -(short)(((long)m_rawAccel * (long)SENSOR_RANGE) / (long)m_accelXRange);
m_calAccel = (short)(((long)m_rawAccel * (long)SENSOR_RANGE) / (long)m_accelYRange);
m_calAccel = (short)(((long)m_rawAccel * (long)SENSOR_RANGE) / (long)m_accelZRange);
} else {
m_calAccel = -m_rawAccel;
m_calAccel = m_rawAccel;
m_calAccel = m_rawAccel;
}
dataFusion();
return true;
}
void MPU9150Lib::dataFusion()
{
float qMag;
float deltaDMPYaw, deltaMagYaw;
float newMagYaw, newYaw;
float temp1, unFused;
float unFusedConjugate;
// *** NOTE *** pitch direction swapped here
m_fusedEulerPose = m_dmpEulerPose;
m_fusedEulerPose = -m_dmpEulerPose;
m_fusedEulerPose = 0;
MPUQuaternionEulerToQuaternion(m_fusedEulerPose, unFused); // create a new quaternion
deltaDMPYaw = -m_dmpEulerPose + m_lastDMPYaw; // calculate change in yaw from dmp
m_lastDMPYaw = m_dmpEulerPose; // update that
qMag = 0;
qMag = m_calMag;
qMag = m_calMag;
qMag = m_calMag;
// Tilt compensate mag with the unfused data (i.e. just roll and pitch with yaw 0)
MPUQuaternionConjugate(unFused, unFusedConjugate);
MPUQuaternionMultiply(qMag, unFusedConjugate, temp1);
MPUQuaternionMultiply(unFused, temp1, qMag);
// Now fuse this with the dmp yaw gyro information
newMagYaw = -atan2(qMag, qMag);
if (newMagYaw != newMagYaw) { // check for nAn
#ifdef MPULIB_DEBUG
Serial.println("***nAn\n");
#endif
return; // just ignore in this case
}
if (newMagYaw < 0)
newMagYaw = 2.0f * (float)M_PI + newMagYaw; // need 0 <= newMagYaw <= 2*PI
newYaw = m_lastYaw + deltaDMPYaw; // compute new yaw from change
if (newYaw > (2.0f * (float)M_PI)) // need 0 <= newYaw <= 2*PI
newYaw -= 2.0f * (float)M_PI;
if (newYaw < 0)
newYaw += 2.0f * (float)M_PI;
deltaMagYaw = newMagYaw - newYaw; // compute difference
if (deltaMagYaw >= (float)M_PI)
deltaMagYaw = deltaMagYaw - 2.0f * (float)M_PI;
if (deltaMagYaw <= -(float)M_PI)
deltaMagYaw = (2.0f * (float)M_PI + deltaMagYaw);
newYaw += deltaMagYaw/4; // apply some of the correction
if (newYaw > (2.0f * (float)M_PI)) // need 0 <= newYaw <= 2*PI
newYaw -= 2.0f * (float)M_PI;
if (newYaw < 0)
newYaw += 2.0f * (float)M_PI;
m_lastYaw = newYaw;
if (newYaw > (float)M_PI)
newYaw -= 2.0f * (float)M_PI;
m_fusedEulerPose = newYaw; // fill in output yaw value
MPUQuaternionEulerToQuaternion(m_fusedEulerPose, m_fusedQuaternion);
} 我没用DMP，自己从MPU9250里面读取AK8963的数据，因为我是用的SPI接口，所有也不能直接读取AK8963,因为AK8963是通过6500的IIC下挂的，所有要使用MPU9250内部的SLV4（从机配置4）来读取写入AK8963寄存器，用SLV0来循环读取AK8963的数据。先前就要配置好SLV0 和SLV4，，，楼主应该找找他是如何配置的。
页:
[1]