bool imu6Test(void) { bool testStatus = true; if (!isInit) { DEBUG_PRINT("Uninitialized"); testStatus = false; } #ifdef IMU_ENABLE_MAG_AK8963 testStatus &= isMagPresent; if (testStatus) { isAK8963TestPassed = ak8963SelfTest(); testStatus = isAK8963TestPassed; } #endif #ifdef IMU_ENABLE_PRESSURE_LPS25H testStatus &= isBaroPresent; if (testStatus) { isLPS25HTestPassed = lps25hSelfTest(); testStatus = isLPS25HTestPassed; } #endif ///testStatus &= imu6ManufacturingTest(); /* TODO Remove */ while (imu6ManufacturingTest() == false); return testStatus; }
bool sensorsTest(void) { bool testStatus = true; if (!isInit) { DEBUG_PRINT("Error while initializing sensor task\r\n"); testStatus = false; } // Try for 3 seconds so the quad has stabilized enough to pass the test for (int i = 0; i < 300; i++) { if(mpu6500SelfTest() == true) { isMpu6500TestPassed = true; break; } else { vTaskDelay(M2T(10)); } } testStatus &= isMpu6500TestPassed; #ifdef SENSORS_ENABLE_MAG_AK8963 testStatus &= isMagnetometerPresent; if (testStatus) { isAK8963TestPassed = ak8963SelfTest(); testStatus = isAK8963TestPassed; } #endif #ifdef SENSORS_ENABLE_PRESSURE_LPS25H testStatus &= isBarometerPresent; if (testStatus) { isLPS25HTestPassed = lps25hSelfTest(); testStatus = isLPS25HTestPassed; } #endif return testStatus; }
static void sensorsDeviceInit(void) { isMagnetometerPresent = false; isBarometerPresent = false; // Wait for sensors to startup while (xTaskGetTickCount() < 1000); i2cdevInit(I2C3_DEV); mpu6500Init(I2C3_DEV); if (mpu6500TestConnection() == true) { DEBUG_PRINT("MPU9250 I2C connection [OK].\n"); } else { DEBUG_PRINT("MPU9250 I2C connection [FAIL].\n"); } mpu6500Reset(); vTaskDelay(M2T(50)); // Activate MPU6500 mpu6500SetSleepEnabled(false); // Enable temp sensor mpu6500SetTempSensorEnabled(true); // Disable interrupts mpu6500SetIntEnabled(false); // Connect the HMC5883L to the main I2C bus mpu6500SetI2CBypassEnabled(true); // Set x-axis gyro as clock source mpu6500SetClockSource(MPU6500_CLOCK_PLL_XGYRO); // Set gyro full scale range mpu6500SetFullScaleGyroRange(IMU_GYRO_FS_CFG); // Set accelerometer full scale range mpu6500SetFullScaleAccelRange(IMU_ACCEL_FS_CFG); #ifdef IMU_MPU6500_DLPF_256HZ // 256Hz digital low-pass filter only works with little vibrations // Set output rate (15): 8000 / (1 + 15) = 500Hz mpu6500SetRate(15); // Set digital low-pass bandwidth mpu6500SetDLPFMode(MPU6500_DLPF_BW_256); #else // To low DLPF bandwidth might cause instability and decrease agility // but it works well for handling vibrations and unbalanced propellers // Set output rate (1): 1000 / (1 + 1) = 500Hz mpu6500SetRate(1); // Set digital low-pass bandwidth mpu6500SetDLPFMode(MPU6500_DLPF_BW_98); #endif // delay 3 seconds until the quad has stabilized enough to pass the test bool mpu6500SelfTestPassed = false; for (int i=0; i<300; i++) { if(mpu6500SelfTest() == true) { mpu6500SelfTestPassed = true; break; } else { vTaskDelay(M2T(10)); } } configASSERT(mpu6500SelfTestPassed); // Now begin to set up the slaves mpu6500SetSlave4MasterDelay(4); // read slaves at 100Hz = (500Hz / (1 + 4)) #ifdef IMU_ENABLE_MAG_AK8963 ak8963Init(I2C3_DEV); if (ak8963TestConnection() == true) { isMagnetometerPresent = true; ak8963SetMode(AK8963_MODE_16BIT | AK8963_MODE_CONT2); // 16bit 100Hz configASSERT(ak8963SelfTest()); DEBUG_PRINT("AK8963 I2C connection [OK].\n"); mpu6500SetSlaveAddress(0, 0x80 | AK8963_ADDRESS_00); // set the magnetometer to Slave 0, enable read mpu6500SetSlaveRegister(0, AK8963_RA_HXL); // read the magnetometer heading register mpu6500SetSlaveDataLength(0, 6); // read 6 bytes (x, y, z heading) mpu6500SetSlaveDelayEnabled(0, true); mpu6500SetSlaveEnabled(0, true); } else { DEBUG_PRINT("AK8963 I2C connection [FAIL].\n"); } #endif #ifdef IMU_ENABLE_PRESSURE_LPS25H lps25hInit(I2C3_DEV); if (lps25hTestConnection() == true) { lps25hSetEnabled(true); isBarometerPresent = true; configASSERT(lps25hSelfTest()); DEBUG_PRINT("LPS25H I2C connection [OK].\n"); mpu6500SetSlaveAddress(1, 0x80 | LPS25H_I2C_ADDR); // set the barometer to Slave 1, enable read mpu6500SetSlaveRegister(1, LPS25H_PRESS_OUT_XL | LPS25H_ADDR_AUTO_INC); mpu6500SetSlaveDataLength(1, 5); mpu6500SetSlaveDelayEnabled(1, true); mpu6500SetSlaveEnabled(1, true); } else { //TODO: Should sensor test fail hard if no connection DEBUG_PRINT("LPS25H I2C connection [FAIL].\n"); } #endif mpu6500SetI2CBypassEnabled(false); mpu6500SetI2CMasterModeEnabled(true); mpu6500SetWaitForExternalSensorEnabled(false); // the slave data isn't so important for the state estimation mpu6500SetInterruptMode(0); // active high mpu6500SetInterruptDrive(0); // push pull mpu6500SetInterruptLatch(0); // latched until clear mpu6500SetInterruptLatchClear(1); // cleared on any register read mpu6500SetIntDataReadyEnabled(true); }