C++ (Cpp) mpu6050GetMotion6の例

コード例 #1

ファイル: imu.c プロジェクト: cpacejo/crazyflie-firmware

void imu6Read(Axis3f* gyroOut, Axis3f* accOut)
{
  mpu6050GetMotion6(&accelMpu.x, &accelMpu.y, &accelMpu.z, &gyroMpu.x, &gyroMpu.y, &gyroMpu.z);

  imuAddBiasValue(&gyroBias, &gyroMpu);
  if (!accelBias.isBiasValueFound)
  {
    imuAddBiasValue(&accelBias, &accelMpu);
  }
  if (!gyroBias.isBiasValueFound)
  {
    imuFindBiasValue(&gyroBias);
    if (gyroBias.isBiasValueFound)
    {
      ledseqRun(LED_RED, seq_calibrated);
//      uartPrintf("Gyro bias: %i, %i, %i\n",
//                  gyroBias.bias.x, gyroBias.bias.y, gyroBias.bias.z);
    }
  }

#ifdef IMU_TAKE_ACCEL_BIAS
  if (gyroBias.isBiasValueFound &&
      !accelBias.isBiasValueFound)
  {
    Axis3i32 mean;

    imuCalculateBiasMean(&accelBias, &mean);
    accelBias.bias.x = mean.x;
    accelBias.bias.y = mean.y;
    accelBias.bias.z = mean.z - IMU_1G_RAW;
    accelBias.isBiasValueFound = TRUE;
    //uartPrintf("Accel bias: %i, %i, %i\n",
    //            accelBias.bias.x, accelBias.bias.y, accelBias.bias.z);
  }
#endif


  imuAccIIRLPFilter(&accelMpu, &accelLPF, &accelStoredFilterValues,
                    (int32_t)imuAccLpfAttFactor);

  imuAccAlignToGravity(&accelLPF, &accelLPFAligned);

  // Re-map outputs
  gyroOut->x = (gyroMpu.x - gyroBias.bias.x) * IMU_DEG_PER_LSB_CFG;
  gyroOut->y = (gyroMpu.y - gyroBias.bias.y) * IMU_DEG_PER_LSB_CFG;
  gyroOut->z = (gyroMpu.z - gyroBias.bias.z) * IMU_DEG_PER_LSB_CFG;
  accOut->x = (accelLPFAligned.x - accelBias.bias.x) * IMU_G_PER_LSB_CFG;
  accOut->y = (accelLPFAligned.y - accelBias.bias.y) * IMU_G_PER_LSB_CFG;
  accOut->z = (accelLPFAligned.z - accelBias.bias.z) * IMU_G_PER_LSB_CFG;
}

コード例 #2

ファイル: imu.c プロジェクト: windymid/crazyflie_lh

void imu6Read(Axis3f* gyroOut, Axis3f* accOut)
{
  mpu6050GetMotion6(&accelMpu.x, &accelMpu.y, &accelMpu.z, &gyroMpu.x, &gyroMpu.y, &gyroMpu.z);

  imuAddBiasValue(&gyroBias, &gyroMpu);
  if (!accelBias.isBiasValueFound)
  {
    imuAddBiasValue(&accelBias, &accelMpu);
  }
  if (!gyroBias.isBiasValueFound)
  {
    imuFindBiasValue(&gyroBias);
    if (gyroBias.isBiasValueFound)
    {
      ledseqRun(LED_RED, seq_calibrated);
//      uartPrintf("Gyro bias: %i, %i, %i\n",
//                  gyroBias.bias.x, gyroBias.bias.y, gyroBias.bias.z);
    }
  }

#ifdef IMU_TAKE_ACCEL_BIAS
  if (gyroBias.isBiasValueFound &&
      !accelBias.isBiasValueFound)
  {
    Axis3i32 mean;

    imuCalculateBiasMean(&accelBias, &mean);
    accelBias.bias.x = mean.x;
    accelBias.bias.y = mean.y;
    accelBias.bias.z = mean.z - IMU_1G_RAW;
    accelBias.isBiasValueFound = TRUE;
    //uartPrintf("Accel bias: %i, %i, %i\n",
    //            accelBias.bias.x, accelBias.bias.y, accelBias.bias.z);
  }
#endif


  imuAccIIRLPFilter(&accelMpu, &accelLPF, &accelStoredFilterValues,
                    (int32_t)imuAccLpfAttFactor);

  imuAccAlignToGravity(&accelLPF, &accelLPFAligned);

  // Re-map outputs
  #if 1
  gyroOut->y = -((gyroMpu.x - gyroBias.bias.x) * IMU_DEG_PER_LSB_CFG);
  gyroOut->x = (gyroMpu.y - gyroBias.bias.y) * IMU_DEG_PER_LSB_CFG;
  gyroOut->z = (gyroMpu.z - gyroBias.bias.z) * IMU_DEG_PER_LSB_CFG;
  accOut->y = -((accelLPFAligned.x - accelBias.bias.x) * IMU_G_PER_LSB_CFG);
  accOut->x = (accelLPFAligned.y - accelBias.bias.y) * IMU_G_PER_LSB_CFG;
  accOut->z = (accelLPFAligned.z - accelBias.bias.z) * IMU_G_PER_LSB_CFG;
  #else
    gyroOut->x = (gyroMpu.x - gyroBias.bias.x) * IMU_DEG_PER_LSB_CFG;
    gyroOut->y = (gyroMpu.y - gyroBias.bias.y) * IMU_DEG_PER_LSB_CFG;
    gyroOut->z = (gyroMpu.z - gyroBias.bias.z) * IMU_DEG_PER_LSB_CFG;
  accOut->x = (accelLPFAligned.x - accelBias.bias.x) * IMU_G_PER_LSB_CFG;
  accOut->y = (accelLPFAligned.y - accelBias.bias.y) * IMU_G_PER_LSB_CFG;
  accOut->z = (accelLPFAligned.z - accelBias.bias.z) * IMU_G_PER_LSB_CFG;
  #endif
  

//  uartSendData(sizeof(Axis3f), (uint8_t*)gyroOut);
//  uartSendData(sizeof(Axis3f), (uint8_t*)accOut);

#if 0
  static uint32_t count = 0;
  if (++count >= 19)
  {
    count = 0;
    uartPrintf("%d, %d, %d, %d, %d, %d, %d, %d, %d\n",
                (int32_t)(gyroOut->x * 10),
                (int32_t)(gyroOut->y * 10),
                (int32_t)(gyroOut->z * 10),
                (int32_t)(accOut->x * 1000),
                (int32_t)(accOut->y * 1000),
                (int32_t)(accOut->z * 1000),
                mag.x,
                mag.y,
                mag.z);
  }
#endif
}