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; }