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");
}
/*
* Name : initialize
* Description : ---
* Author : lynx@sia.
*
* History
* ----------------------
* Rev : 0.00
* Date : 10/20/2013
*
* create.
* ----------------------
*/
static int initialize(void)
{
MPU6050_Init(); //Keep MPU-6050 initialized first
AK8975_initialize(); //Do nothing infact
//MS5611_Init(&imu_sensor.Bar);
imu_sensor.Gyr.RawToTrue = 16.3835f; //2000dps--16.4LSB
imu_sensor.Acc.RawToTrue = 8192.0f; //4g--8192LSB
imu_sensor.Mag.RawToTrue = 3.332f; //1229uT--3.3319772172497965825874694873881LSB
return 0;
}