C++ (Cpp) nvtSetAccOffset Examples

Example #1

File: sensors.c Project: brucetsao/Nuvoton

/* 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");
}

Example #2

File: Sensors.c Project: JohnsonShen/AHRS_RD

/* 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");
  }
}