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