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