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
}
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;
}
void GetEulerAngle(void)
{
float deltaT;
Struct_3f gy,ac;
GetMPU6050Data();
imuAccIIRLPFilter(&mpu_info.acc, &accelLPF, &accelStoredFilterValues,imuAccLpfAttFactor);
gy.x = mpu_info.gyro.x * Gyro2Radian;
gy.y = mpu_info.gyro.y * Gyro2Radian;
gy.z = mpu_info.gyro.z * Gyro2Radian;
ac.x = accelLPF.x;
ac.y = accelLPF.y;
ac.z = accelLPF.z;
deltaT = GetDeltaT(GetSysTime_us());
DCM_CF(gy,ac,deltaT);
}
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
}
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;
}