C++ (Cpp) nvtSetGyroOffset示例

示例#1

文件： Sensors.c 项目： JohnsonShen/AHRS_RD

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

示例#2

文件： sensors.c 项目： brucetsao/Nuvoton

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