Блог

В настоящее время я использую MPU9150Lib в своем проекте Arduino для чтения данных из моего IMU (конечно, MPU9150). Пока все работает отлично. Но иногда я замечал небольшие сбои. Я думаю, что это проблема с блокировкой кардана. Когда я смотрю на библиотеку, кажется, что они объединяют данные, а затем просто преобразуют полученные углы Эйлера в кватернион. Может ли это вызвать блокировку кардана? Если да, не могли бы вы, ребята, помочь мне переписать код? Я не совсем понимаю математику, лежащую в основе объединения данных. Итак, я не знаю, как исправить библиотеку.

Вы можете найти это здесь:https://github.com/zarthcode/MPU9150Lib/tree/master/libraries/MPU9150Lib

Библиотека содержит следующую функцию. Я не уверен, что проблема заключается в функции, но для меня это выглядит странно, поскольку в конце она преобразует координаты Эйлера в кватернион.

 void MPU9150Lib::dataFusion()
{
  float qMag[4];
  float deltaDMPYaw, deltaMagYaw;
  float newMagYaw, newYaw;
  float temp1[4], unFused[4];
  float unFusedConjugate[4];

  // *** NOTE *** pitch direction swapped here

  m_fusedEulerPose[VEC3_X] = m_dmpEulerPose[VEC3_X];
  m_fusedEulerPose[VEC3_Y] = -m_dmpEulerPose[VEC3_Y];
  m_fusedEulerPose[VEC3_Z] = 0; 
  MPUQuaternionEulerToQuaternion(m_fusedEulerPose, unFused);    // create a new quaternion

  deltaDMPYaw = -m_dmpEulerPose[VEC3_Z]   m_lastDMPYaw;         // calculate change in yaw from dmp
  m_lastDMPYaw = m_dmpEulerPose[VEC3_Z];                        // update that

  qMag[QUAT_W] = 0;
  qMag[QUAT_X] = m_calMag[VEC3_X];
  qMag[QUAT_Y] = m_calMag[VEC3_Y];
  qMag[QUAT_Z] = m_calMag[VEC3_Z];

  // 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[QUAT_Y], qMag[QUAT_X]);

  if (newMagYaw != newMagYaw) {                                 // check for nAn
#ifdef MPULIB_DEBUG
    Serial.println("***nAnn");
#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[VEC3_Z] = newYaw;                            // fill in output yaw value

  MPUQuaternionEulerToQuaternion(m_fusedEulerPose, m_fusedQuaternion);
}
  

Я смог устранить проблему. Я нашел другую библиотеку, которая была загружена для платы sparkfun breakout для mpu9150. Я тоже не смог заставить это работать, но по другим причинам. Исходные данные были очень шумными, и я не мог это исправить, изменив параметры. Но в примере, который поставляется с библиотекой, есть функция для объединения данных. Я скопировал его в MPU9150Lib и скорректировал переменные. Итак, в основном я использую MPU9150Lib теперь с алгоритмом объединения данных с платы sparkfun breakout. Теперь все работает как по маслу. Вы можете найти библиотеку sparkfun здесь:https://github.com/sparkfun/MPU-9150_Breakout/tree/master/firmware/MPU6050/Examples

Приведенный там пример — это, наконец, то, что я искал все время. Хорошо документированный способ использования mpu9150. Мне потребовалось некоторое время, чтобы запустить эту штуку так, как я хотел.