Пример #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");
}
Пример #2
0
void SensorInitGYRO()
{
	float Cal[GYRO_CAL_DATA_SIZE];
	bool FlashValid;
	if(!SensorInitState.GYRO_Done) {
#if defined(MPU6050) || defined(MPU6500)
		SensorInitState.GYRO_Done = MPU6050_initialize();
		SensorInitState.ACC_Done = SensorInitState.GYRO_Done;
#else

#endif
	}

	if(SensorInitState.GYRO_Done) {
		printf("GYRO connect     - [OK]\n");
		FlashValid = GetFlashCal(SENSOR_GYRO, Cal);
		
		if(FlashValid) {
			CalFlashState.GYRO_FLASH = true;
			GyroOffset[0] = Cal[0];
			GyroOffset[1] = Cal[1];
			GyroOffset[2] = Cal[2];
			GyroScale[0]  = Cal[3];
			GyroScale[1]  = Cal[4];
			GyroScale[2]  = Cal[5];
			printf("GYRO calibration from [FLASH]\n");
			
		}
		else {
			GyroOffset[0] = 0;
			GyroOffset[1] = 0;
			GyroOffset[2] = 0;
			GyroScale[0] = 1.0;
			GyroScale[1] = 1.0;
			GyroScale[2] = 1.0;
			printf("GYRO calibration from - [DEFAULT]\n");
		}
		printf("Offset: %f  %f  %f\n", GyroOffset[0], GyroOffset[1], GyroOffset[2]);
		printf("Scale: %f  %f  %f\n", GyroScale[0], GyroScale[1], GyroScale[2]);
		nvtSetGyroScale(GyroScale);
		nvtSetGyroOffset(GyroOffset);

#if defined(MPU6050) || defined(MPU6500)
		nvtSetGYRODegPLSB(IMU_DEG_PER_LSB_CFG);
#else
    nvtSetGYRODegPLSB(calcGyro(1));
#endif
	}
	else
		printf("GYRO connect     - [FAIL]\n");
}
Пример #3
0
void SensorInitGYRO()
{
	float Cal[GYRO_CAL_DATA_SIZE];
	bool FlashValid;
	if(!SensorInitState.GYRO_Done)
	{
#ifdef MPU6050
		SensorInitState.GYRO_Done = MPU6050_initialize();
		SensorInitState.ACC_Done = SensorInitState.GYRO_Done;
#endif
	}

	if(SensorInitState.GYRO_Done)
	{
		printf("GYRO connect     - [OK]\n");
		FlashValid = GetFlashCal(SENSOR_GYRO, Cal);

		if(FlashValid)
		{
			CalFlashState.GYRO_FLASH = true;
			GyroOffset[0] = Cal[0];
			GyroOffset[1] = Cal[1];
			GyroOffset[2] = Cal[2];
			GyroScale[0]  = Cal[3]*IMU_DEG_PER_LSB_CFG;
			GyroScale[1]  = Cal[4]*IMU_DEG_PER_LSB_CFG;
			GyroScale[2]  = Cal[5]*IMU_DEG_PER_LSB_CFG;
			printf("GYRO calibration - [FLASH]\n");

		}
		else
		{
			GyroOffset[0] = 0;
			GyroOffset[1] = 0;
			GyroOffset[2] = 0;
			GyroScale[0] = IMU_DEG_PER_LSB_CFG;
			GyroScale[1] = IMU_DEG_PER_LSB_CFG;
			GyroScale[2] = IMU_DEG_PER_LSB_CFG;
			printf("GYRO calibration - [DEF]\n");
		}
		//printf("Offset: %f  %f  %f\n", GyroOffset[0], GyroOffset[1], GyroOffset[2]);
		//printf("Scale: %f  %f  %f\n", GyroScale[0], GyroScale[1], GyroScale[2]);
		nvtSetGyroScale(GyroScale);
		nvtSetGyroOffset(GyroOffset);
		nvtSetGYRODegPLSB(IMU_DEG_PER_LSB_CFG);
	}
	else
		printf("GYRO connect     - [FAIL]\n");
}
Пример #4
0
/* Sensors Init */
void SensorInitACC()
{
	float Cal[ACC_CAL_DATA_SIZE];
	bool FlashValid;

	if(!SensorInitState.ACC_Done)
	{
#ifdef MPU6050
		SensorInitState.ACC_Done = MPU6050_initialize();
		SensorInitState.GYRO_Done = SensorInitState.ACC_Done;
#endif
	}
	if(SensorInitState.ACC_Done)
	{
		printf("ACC connect      - [OK]\n");
		FlashValid = GetFlashCal(SENSOR_ACC, Cal);
		if(FlashValid)
		{
			CalFlashState.ACC_FLASH = true;
			AccOffset[0] = Cal[0];
			AccOffset[1] = Cal[1];
			AccOffset[2] = Cal[2];
			AccScale[0]  = Cal[3];
			AccScale[1]  = Cal[4];
			AccScale[2]  = Cal[5];
			printf("ACC calibration  - [FLASH]\n");

		}
		else
		{
			AccOffset[0] = ACC_OFFSET_X;//0;
			AccOffset[1] = ACC_OFFSET_Y;//0;
			AccOffset[2] = ACC_OFFSET_Z;//0;
			AccScale[0] = ACC_SCALE_X;//IMU_G_PER_LSB_CFG;
			AccScale[1] = ACC_SCALE_Y;//IMU_G_PER_LSB_CFG;
			AccScale[2] = ACC_SCALE_Z;//IMU_G_PER_LSB_CFG;
			printf("ACC calibration  - [DEF]\n");
		}
		//printf("Offset: %f  %f  %f\n", AccOffset[0], AccOffset[1], AccOffset[2]);
		//printf("Scale: %f  %f  %f\n", AccScale[0], AccScale[1], AccScale[2]);
		nvtSetAccScale(AccScale);
		nvtSetAccOffset(AccOffset);
		nvtSetAccG_PER_LSB(IMU_G_PER_LSB_CFG);
	}
	else
		printf("ACC connect      - [FAIL]\n");
}
Пример #5
0
/* Sensors Init */
void SensorInitACC()
{
	float Cal[ACC_CAL_DATA_SIZE];
	bool FlashValid;
#if defined(LSM6DS3)
  status_t status;
#endif
	
	if(!SensorInitState.ACC_Done) {
#if defined(MPU6050) || defined(MPU6500)
		SensorInitState.ACC_Done = MPU6050_initialize();
		SensorInitState.GYRO_Done = SensorInitState.ACC_Done;
#else
    LSM6DS3_init();
    status = begin();
    if(status==0)
      SensorInitState.ACC_Done = true;
    else
      SensorInitState.ACC_Done = false;
		SensorInitState.GYRO_Done = SensorInitState.ACC_Done;
#endif
	}
	if(SensorInitState.ACC_Done) {
		printf("ACC connect      - [OK]\n");
		FlashValid = GetFlashCal(SENSOR_ACC, Cal);
		if(FlashValid) {
			CalFlashState.ACC_FLASH = true;
			AccOffset[0] = Cal[0];
			AccOffset[1] = Cal[1];
			AccOffset[2] = Cal[2];
			AccScale[0]  = Cal[3];
			AccScale[1]  = Cal[4];
			AccScale[2]  = Cal[5];
			AccRotate[0] = Cal[6];
			AccRotate[1] = Cal[7];
			AccRotate[2] = Cal[9];
			AccRotate[3] = Cal[9];
			AccRotate[4] = Cal[10];
			AccRotate[5] = Cal[11];
			AccRotate[6] = Cal[12];
			AccRotate[7] = Cal[13];
			AccRotate[8] = Cal[14];
			printf("ACC calibration from - [FLASH]\n");
			
		}
		else {
			AccOffset[0] = 0;
			AccOffset[1] = 0;
			AccOffset[2] = 0;
			AccScale[0] = IMU_G_PER_LSB_CFG;
			AccScale[1] = IMU_G_PER_LSB_CFG;
			AccScale[2] = IMU_G_PER_LSB_CFG;
			AccRotate[0] = 1;
			AccRotate[1] = 0;
			AccRotate[2] = 0;
			AccRotate[3] = 0;
			AccRotate[4] = 1;
			AccRotate[5] = 0;
			AccRotate[6] = 0;
			AccRotate[7] = 0;
			AccRotate[8] = 1;
			printf("ACC calibration from - [DEFAULT]\n");
		}
	printf("Offset: %f  %f  %f\n", AccOffset[0], AccOffset[1], AccOffset[2]);
	printf("Scale: %f  %f  %f\n", AccScale[0], AccScale[1], AccScale[2]);
	printf("M[0][1][2]: %f %f %f\n", AccRotate[0], AccRotate[1], AccRotate[2]);
	printf("M[3][4][5]: %f %f %f\n", AccRotate[3], AccRotate[4], AccRotate[5]);
	printf("M[6][7][8]: %f %f %f\n", AccRotate[6], AccRotate[7], AccRotate[8]);
	nvtSetAccScale(AccScale);
	nvtSetAccOffset(AccOffset);
	nvtSetAccRotate(AccRotate);
	#if defined(MPU6050) || defined(MPU6500)
  nvtSetAccG_PER_LSB(IMU_G_PER_LSB_CFG);
#else
	nvtSetAccG_PER_LSB(calcAccel(1)/*IMU_G_PER_LSB_CFG*/);
#endif
}
	else {
    __disable_irq();
    SYS_UnlockReg();
    SYS_ResetChip();
    printf("ACC connect      - [FAIL]\n");
  }
}