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
}
bool sensorsManufacturingTest(void)
{
bool testStatus = false;
Axis3i16 g;
Axis3i16 a;
Axis3f acc; // Accelerometer axis data in mG
float pitch, roll;
uint32_t startTick = xTaskGetTickCount();
testStatus = mpu6500SelfTest();
if (testStatus)
{
sensorsBiasObjInit(&gyroBiasRunning);
while (xTaskGetTickCount() - startTick < SENSORS_VARIANCE_MAN_TEST_TIMEOUT)
{
mpu6500GetMotion6(&a.y, &a.x, &a.z, &g.y, &g.x, &g.z);
if (processGyroBias(g.x, g.y, g.z, &gyroBias))
{
gyroBiasFound = true;
DEBUG_PRINT("Gyro variance test [OK]\n");
break;
}
}
if (gyroBiasFound)
{
acc.x = -(a.x) * SENSORS_G_PER_LSB_CFG;
acc.y = (a.y) * SENSORS_G_PER_LSB_CFG;
acc.z = (a.z) * SENSORS_G_PER_LSB_CFG;
// Calculate pitch and roll based on accelerometer. Board must be level
pitch = tanf(-acc.x/(sqrtf(acc.y*acc.y + acc.z*acc.z))) * 180/(float) M_PI;
roll = tanf(acc.y/acc.z) * 180/(float) M_PI;
if ((fabsf(roll) < SENSORS_MAN_TEST_LEVEL_MAX) && (fabsf(pitch) < SENSORS_MAN_TEST_LEVEL_MAX))
{
DEBUG_PRINT("Acc level test [OK]\n");
testStatus = true;
}
else
{
DEBUG_PRINT("Acc level test Roll:%0.2f, Pitch:%0.2f [FAIL]\n", (double)roll, (double)pitch);
testStatus = false;
}
}
else
{
DEBUG_PRINT("Gyro variance test [FAIL]\n");
testStatus = false;
}
}
return testStatus;
}
