Exemplos de mpu9150_set_mag_cal em C++ (Cpp)

Exemplo n.º 1

Arquivo: imu.c Projeto: xyliaa2336/linux-mpu9150

int set_cal(int mag, char *cal_file)
{
	int i;
	FILE *f;
	char buff[32];
	long val[6];
	caldata_t cal;

	if (cal_file) {
		f = fopen(cal_file, "r");
		
		if (!f) {
			perror("open(<cal-file>)");
			return -1;
		}
	}
	else {
		if (mag) {
			f = fopen("./magcal.txt", "r");
		
			if (!f) {
				printf("Default magcal.txt not found\n");
				return 0;
			}
		}
		else {
			f = fopen("./accelcal.txt", "r");
		
			if (!f) {
				printf("Default accelcal.txt not found\n");
				return 0;
			}
		}		
	}

	memset(buff, 0, sizeof(buff));
	
	for (i = 0; i < 6; i++) {
		if (!fgets(buff, 20, f)) {
			printf("Not enough lines in calibration file\n");
			break;
		}

		val[i] = atoi(buff);

		if (val[i] == 0) {
			printf("Invalid cal value: %s\n", buff);
			break;
		}
	}

	fclose(f);

	if (i != 6) 
		return -1;

	cal.offset[0] = (short)((val[0] + val[1]) / 2);
	cal.offset[1] = (short)((val[2] + val[3]) / 2);
	cal.offset[2] = (short)((val[4] + val[5]) / 2);

	cal.range[0] = (short)(val[1] - cal.offset[0]);
	cal.range[1] = (short)(val[3] - cal.offset[1]);
	cal.range[2] = (short)(val[5] - cal.offset[2]);
	
	if (mag) 
		mpu9150_set_mag_cal(&cal);
	else 
		mpu9150_set_accel_cal(&cal);

	return 0;
}

Exemplo n.º 2

Arquivo: am_mpu9150_node.cpp Projeto: agneevguin/catkin_ws

int load_calibration(int mag, std::string cal_file)
{
	int i;
	FILE *f;
	char buff[32];
	long val[6];
	caldata_t cal;

	f = fopen(cal_file.c_str(), "r");
	
	if (!f) 
	{
		ROS_ERROR("am_mpu9150::load_calibration:: Failed to open file %s", cal_file.c_str());
		perror("File problem:");
		return -1;
	}
	
	memset(buff, 0, sizeof(buff));
	
	for (i = 0; i < 6; i++) 
	{
		if (!fgets(buff, 20, f)) 
		{
			ROS_ERROR("am_mpu9150::load_calibration::Not enough lines in calibration file\n");
			break;
		}

		val[i] = atoi(buff);

		if (val[i] == 0) 
		{
			ROS_ERROR("am_mpu9150::load_calibration::Invalid cal value: %s\n", buff);
			break;
		}
	}

	fclose(f);

	if (i != 6) 
	{
		ROS_ERROR("am_mpu9150::load_calibration:: Failed to load calibration!");
		return -1;
	}

	// Store calibration
	cal.offset[0] = (short)((val[0] + val[1]) / 2);
	cal.offset[1] = (short)((val[2] + val[3]) / 2);
	cal.offset[2] = (short)((val[4] + val[5]) / 2);

	cal.range[0] = (short)(val[1] - cal.offset[0]);
	cal.range[1] = (short)(val[3] - cal.offset[1]);
	cal.range[2] = (short)(val[5] - cal.offset[2]);
	
	if (mag) 
	{
		mpu9150_set_mag_cal(&cal);
	}
	else 
	{
		mpu9150_set_accel_cal(&cal);
	}

	return 0;
}