bool imu6Test(void)
{
bool testStatus = true;
if (!isInit)
{
DEBUG_PRINT("Uninitialized\n");
testStatus = false;
}
// Test for CF 10-DOF variant with none responding sensor
if((isHmc5883lPresent && !isMs5611Present) ||
(!isHmc5883lPresent && isMs5611Present))
{
DEBUG_PRINT("HMC5883L or MS5611 is not responding\n");
testStatus = false;
}
if (testStatus)
{
isMpu6050TestPassed = mpu6050SelfTest();
testStatus = isMpu6050TestPassed ;
}
if (testStatus && isHmc5883lPresent)
{
isHmc5883lTestPassed = hmc5883lSelfTest();
testStatus = isHmc5883lTestPassed;
}
if (testStatus && isMs5611Present)
{
isMs5611TestPassed = ms5611SelfTest();
testStatus = isMs5611TestPassed;
}
return testStatus;
}
void SensorInitMAG()
{
float Cal[MAG_CAL_DATA_SIZE + QUALITY_FACTOR_SIZE],magCal[3];
bool FlashValid;
int i;
if(!SensorInitState.MAG_Done)
{
#ifdef HMC5883
SensorInitState.MAG_Done = hmc5883lInit();
hmc5883lSelfTest();
hmc5883lGetRatioFactor(&magCal[0],&magCal[1],&magCal[2]);
#endif
#ifdef AK8975
SensorInitState.MAG_Done = AK8975_initialize();
#endif
}
if(SensorInitState.MAG_Done)
{
if (report_format == REPORT_FORMAT_TEXT)
printf("MAG connect - [OK]\n");
FlashValid = GetFlashCal(SENSOR_MAG, Cal);
if(FlashValid)
{
CalFlashState.MAG_FLASH = true;
for(i=0; i<MAG_CAL_DATA_SIZE; i++)
MagCalMatrix[i] = Cal[i];
CalFlashState.MAG_QFACTOR = Cal[i];
if (report_format == REPORT_FORMAT_TEXT)
printf("MAG calibration from - [FLASH], Q:%d\n",CalFlashState.MAG_QFACTOR);
}
else
{
for(i=0; i<MAG_CAL_DATA_SIZE; i++)
MagCalMatrix[i] = 0;
MagCalMatrix[3] = magCal[0];//MAG_GAUSS_PER_LSB;
MagCalMatrix[4] = magCal[1];//MAG_GAUSS_PER_LSB;
MagCalMatrix[5] = magCal[2];//MAG_GAUSS_PER_LSB;
if (report_format == REPORT_FORMAT_TEXT)
printf("MAG calibration from - [DEFAULT], Q:%d\n",CalFlashState.MAG_QFACTOR);
}
/*printf("M[0][1][2]: %f %f %f\n", MagCalMatrix[0], MagCalMatrix[1], MagCalMatrix[2]);
printf("M[3][4][5]: %f %f %f\n", MagCalMatrix[3], MagCalMatrix[4], MagCalMatrix[5]);
printf("M[6][7][8]: %f %f %f\n", MagCalMatrix[6], MagCalMatrix[7], MagCalMatrix[8]);
printf("M[9]: %f\n", MagCalMatrix[9]);*/
nvtSetMagCalMatrix(MagCalMatrix);
nvtSetMagGaussPLSB(MAG_GAUSS_PER_LSB);
}
else
printf("MAG connect - [FAIL]\n");
}
