C++ (Cpp) imuCalculateBiasMean 예제들

예제 #1

파일: imu_cf2.c 프로젝트: guygrigsby/crazyflie-firmware

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

  imuAddBiasValue(&gyroBias, &gyroMpu);
#ifdef IMU_TAKE_ACCEL_BIAS
  if (!accelBias.isBiasValueFound)
  {
    imuAddBiasValue(&accelBias, &accelMpu);
  }
#endif
  if (!gyroBias.isBiasValueFound)
  {
    imuFindBiasValue(&gyroBias);
    if (gyroBias.isBiasValueFound)
    {
      soundSetEffect(SND_CALIB);
      ledseqRun(SYS_LED, seq_calibrated);
    }
  }

#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;
  }
#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;
#ifdef IMU_TAKE_ACCEL_BIAS
  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;
#else
  accOut->x = -(accelLPFAligned.x) * IMU_G_PER_LSB_CFG;
  accOut->y = (accelLPFAligned.y) * IMU_G_PER_LSB_CFG;
  accOut->z = (accelLPFAligned.z) * IMU_G_PER_LSB_CFG;
#endif

}

예제 #2

파일: 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;
}

예제 #3

파일: 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
}

예제 #4

파일: imu.c 프로젝트: lhcalibur/Copter_CortexM4

void imuRead(Axis3f* gyroOut, Axis3f* accOut, Axis3f* magOut)
{
	float G_sensitivity = 0.0f;
	float X_sensitivity = 0.0f;

	BSP_IMU_6AXES_X_GetAxesRaw((AxesRaw_TypeDef *)&accelMpu);
	BSP_IMU_6AXES_G_GetAxesRaw((AxesRaw_TypeDef *)&gyroMpu);
	BSP_MAGNETO_M_GetAxes((Axes_TypeDef *)&mag);
	BSP_IMU_6AXES_G_GetSensitivity(&G_sensitivity);	
	BSP_IMU_6AXES_X_GetSensitivity(&X_sensitivity);


	imuAddBiasValue(&gyroBias, &gyroMpu);
#ifdef IMU_TAKE_ACCEL_BIAS
	if (!accelBias.isBiasValueFound)
	{
		imuAddBiasValue(&accelBias, &accelMpu);
	}
#endif
	if (!gyroBias.isBiasValueFound)
	{
		imuFindBiasValue(&gyroBias);
		if (gyroBias.isBiasValueFound)
		{
			//ledseqRun(SYS_LED, seq_calibrated);
#ifndef IMU_TAKE_ACCEL_BIAS
			BSP_LED_On(LED2);
#endif
		}
	}

#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 - (1 / X_sensitivity * 1000.0f);
		accelBias.isBiasValueFound = true;
		BSP_LED_On(LED2);
	}
#endif
	imuAccIIRLPFilter(&accelMpu, &accelLPF, &accelStoredFilterValues, (int32_t)imuAccLpfAttFactor);

	//imuAccAlignToGravity(&accelLPF, &accelLPFAligned);





	/*
	accOut->x = ACC_Value.AXIS_X / 1000.0;
	accOut->y = ACC_Value.AXIS_Y / 1000.0;
	accOut->z = ACC_Value.AXIS_Z / 1000.0;
	gyroOut->x = GYR_Value.AXIS_X / 1000.0;
	gyroOut->y = GYR_Value.AXIS_Y / 1000.0;
	gyroOut->z = GYR_Value.AXIS_Z / 1000.0;
	magOut->x = MAG_Value.AXIS_X / 1000.0f;
	magOut->y = MAG_Value.AXIS_Y / 1000.0f;
	magOut->z = MAG_Value.AXIS_Z / 1000.0f;

	gyroOut->x = gyroOut->x * M_PI / 180.0;
	gyroOut->y = gyroOut->y * M_PI / 180.0;
	gyroOut->z = gyroOut->z * M_PI / 180.0;
	*/


	// Re-map outputs
	
	gyroOut->x = (gyroMpu.x - gyroBias.bias.x) * G_sensitivity / 1000.0f;
	gyroOut->y = (gyroMpu.y - gyroBias.bias.y) * G_sensitivity / 1000.0f;
	gyroOut->z = (gyroMpu.z - gyroBias.bias.z) * G_sensitivity / 1000.0f;

	/*
	gyroOut->x = (gyroMpu.x) * G_sensitivity / 1000.0f;
	gyroOut->y = (gyroMpu.y) * G_sensitivity / 1000.0f;
	gyroOut->z = (gyroMpu.z) * G_sensitivity / 1000.0f;
	*/

	gyroOut->x = - gyroOut->x * M_PI / 180.0f;
	gyroOut->y = gyroOut->y * M_PI / 180.0f;
	gyroOut->z = gyroOut->z * M_PI / 180.0f;

#ifdef IMU_TAKE_ACCEL_BIAS
	/*
	accOut->x = (accelLPFAligned.x - accelBias.bias.x) * X_sensitivity / 1000.0f;
	accOut->y = (accelLPFAligned.y - accelBias.bias.y) * X_sensitivity / 1000.0f;
	accOut->z = (accelLPFAligned.z - accelBias.bias.z) * X_sensitivity / 1000.0f;
	*/
	accOut->x = - (accelLPF.x - accelBias.bias.x) * X_sensitivity / 1000.0f;
	accOut->y = (accelLPF.y - accelBias.bias.y) * X_sensitivity / 1000.0f;
	accOut->z = (accelLPF.z - accelBias.bias.z) * X_sensitivity / 1000.0f;

#else
	//accOut->x = (accelLPFAligned.x) * X_sensitivity / 1000.0f;
	//accOut->y = (accelLPFAligned.y) * X_sensitivity / 1000.0f;
	//accOut->z = (accelLPFAligned.z) * X_sensitivity / 1000.0f;
	accOut->x =  - (accelLPF.x) * X_sensitivity;
	accOut->y = (accelLPF.y) * X_sensitivity;
	accOut->z = (accelLPF.z) * X_sensitivity;

#endif
	magOut->x = (float)mag.x / 10000.0f;
	magOut->y = (float)mag.y / 10000.0f;
	magOut->z = (float)mag.z / 10000.0f;


}