Exemple #1
0
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;
}