示例#1
0
void MPUQuaternionEulerToQuaternion(const MPUVector3 v, MPUQuaternion q)
{
  float cosX2 = cos(v[VEC3_X] / 2.0f);
  float sinX2 = sin(v[VEC3_X] / 2.0f);
  float cosY2 = cos(v[VEC3_Y] / 2.0f);
  float sinY2 = sin(v[VEC3_Y] / 2.0f);
  float cosZ2 = cos(v[VEC3_Z] / 2.0f);
  float sinZ2 = sin(v[VEC3_Z] / 2.0f);

  q[QUAT_W] = cosX2 * cosY2 * cosZ2 + sinX2 * sinY2 * sinZ2;
  q[QUAT_X] = sinX2 * cosY2 * cosZ2 - cosX2 * sinY2 * sinZ2;
  q[QUAT_Y] = cosX2 * sinY2 * cosZ2 + sinX2 * cosY2 * sinZ2;
  q[QUAT_Z] = cosX2 * cosY2 * sinZ2 - sinX2 * sinY2 * cosZ2;
  MPUQuaternionNormalize(q);
}
示例#2
0
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, &timestamp, &sensors, &more)) != 0) {
    return false;
  }

  //  got the fifo data so now get the mag data

  if ((result = mpu_get_compass_reg(m_rawMag, &timestamp)) != 0) {
#ifdef MPULIB_DEBUG
    Serial.print("Failed to read compass: ");
    Serial.println(result);
    return false;
#endif
  }

  // got the raw data - now process

  m_dmpQuaternion[QUAT_W] = (float)m_rawQuaternion[QUAT_W];  // get float version of quaternion
  m_dmpQuaternion[QUAT_X] = (float)m_rawQuaternion[QUAT_X];
  m_dmpQuaternion[QUAT_Y] = (float)m_rawQuaternion[QUAT_Y];
  m_dmpQuaternion[QUAT_Z] = (float)m_rawQuaternion[QUAT_Z];
  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[VEC3_Y] = -(short)(((long)(m_rawMag[VEC3_X] - m_magXOffset) * (long)SENSOR_RANGE) / (long)m_magXRange);
    m_calMag[VEC3_X] = (short)(((long)(m_rawMag[VEC3_Y] - m_magYOffset) * (long)SENSOR_RANGE) / (long)m_magYRange);
    m_calMag[VEC3_Z] = (short)(((long)(m_rawMag[VEC3_Z] - m_magZOffset) * (long)SENSOR_RANGE) / (long)m_magZRange);
  }
  else {
    m_calMag[VEC3_Y] = -m_rawMag[VEC3_X];
    m_calMag[VEC3_X] = m_rawMag[VEC3_Y];
    m_calMag[VEC3_Z] = m_rawMag[VEC3_Z];
  }

  // Scale accel data

  if (m_useAccelCalibration) {
    m_calAccel[VEC3_X] = -(short)(((long)m_rawAccel[VEC3_X] * (long)SENSOR_RANGE) / (long)m_accelXRange);
    m_calAccel[VEC3_Y] = (short)(((long)m_rawAccel[VEC3_Y] * (long)SENSOR_RANGE) / (long)m_accelYRange);
    m_calAccel[VEC3_Z] = (short)(((long)m_rawAccel[VEC3_Z] * (long)SENSOR_RANGE) / (long)m_accelZRange);
  }
  else {
    m_calAccel[VEC3_X] = -m_rawAccel[VEC3_X];
    m_calAccel[VEC3_Y] = m_rawAccel[VEC3_Y];
    m_calAccel[VEC3_Z] = m_rawAccel[VEC3_Z];
  }
  dataFusion();
  return true;
}