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;
}
void sensorsMpu9250Lps25hInit(void)
{
if (isInit)
{
return;
}
sensorsBiasObjInit(&gyroBiasRunning);
sensorsDeviceInit();
sensorsInterruptInit();
sensorsTaskInit();
isInit = true;
}
void sensorsBmi088Bmp388Init(void)
{
if (isInit)
{
return;
}
i2cdevInit(I2C3_DEV);
sensorsBiasObjInit(&gyroBiasRunning);
sensorsDeviceInit();
sensorsInterruptInit();
sensorsTaskInit();
}
void sensorsInit(void)
{
if (isInit)
{
return;
}
sensorsDataReady = xSemaphoreCreateBinary();
sensorsBiasObjInit(&gyroBiasRunning);
sensorsDeviceInit();
sensorsInterruptInit();
sensorsTaskInit();
isInit = true;
}
