예제 #1
0
// *********************************************************************************
// *****	Compass_Thread													********
// *****	continous thread that reads and calulates compass heading		********
// *****	(r) void														********
// ***** 	(1) void *: pointer to any data						            ********
// *********************************************************************************
void *Compass_Thread(void *ptr)
{
// *********************************************************************************
// *****	Veriable Declaratins 											********
int i;
int v_iteration_number;
compass_struct strc_temp_compass_data;
int v_return_test;
int v_data_count;
unsigned char v_Register_Address;
unsigned char v_Compass_Array[6];
int a_compass[6];

// ----------  END  ----------------------------------------------------------------

/*(printf("Entering Compass calibration Mode\n");
Init_Calibrate_Compass();
usleep(60000000);
printf("Exiting compass calibration mode\n");
End_Calibrate_Compass();*/

// *********************************************************************************
// *****	Main Thread Loop												********
for(;;)
{
	if(!calibrating_compass)
	{
// *********************************************************************************
// *****	Read compass													********
	v_Register_Address = 'A';
	v_data_count = 0;
	v_return_test = i2c_write(v_device, COMPASS_ADDRESS, v_Register_Address, v_Compass_Array, v_data_count);
	usleep(6000);
	v_data_count = 2;
	v_return_test = i2c_read(v_device, COMPASS_ADDRESS, SKIP_WRITE_REGISTER, v_Compass_Array, v_data_count);
	a_compass[0] = array_to_16bit_int(v_Compass_Array[0], v_Compass_Array[1]);
	
	Calculate_Heading(a_compass, &strc_temp_compass_data);
// ----------  END  ----------------------------------------------------------------
	
	strc_compass = strc_temp_compass_data;
	v_iteration_number++;
	new_compass_data = 1;
	}
	usleep(100000);
}
// ----------  END  ----------------------------------------------------------------
}
예제 #2
0
int main(int argc, char **argv)
{
	int mpu9150_fd;
	unsigned int m = 1903;
	FILE *from_fd,*to_fd;
	int magn_fd;
	double *ax,*ay;
	double count_B2;//水平磁场分量

	mpu9150_fd = open("/dev/mpu9150",O_RDWR);
	if (mpu9150_fd < 0){
		printf("can't open /dev/mpu9150\n");
		return -1;
	}else{
		printf("Now,you are in /dev/mpu9150\n");
	}
	
	
	magn_fd = open("/dev/magn",O_RDWR);
	if (magn_fd < 0){
		printf("can't open /dev/magn\n");
		return -1;
	}else{
		printf("Now,you are in /dev/magn\n");
	}

	ax = (double *)malloc(sizeof (double)* m);
	if(NULL==ax){
		printf("error to malloc ax\n");
		exit(1);
	}
	ay = (double *)malloc(sizeof (double)* m);
	if(NULL==ay){
		printf("error to malloc ay\n");
		exit(1);
	}

	from_fd = fopen("/data_file.txt","r");
	if(from_fd == NULL){
		printf("open '/data_file.txt' failed!");
		return -1;
	}

	for(int i=0;i<m;i++){
		fscanf(from_fd,"%lf%*c%lf",&ax[i],&ay[i]);									//中间跳过空格,水平分量值读入数组中
	}
	fclose(from_fd);

	count_B2 = ((MAX(ax,m) - MIN(ax,m))/2.0)*((MAX(ax,m) - MIN(ax,m))/2.0);
	g_ellipse_parameter.F = -count_B2;
	
	g_equation_temp_value.bb = (double *)malloc(sizeof (double)* 5);
	for(int i=0;i<5;i++){
		g_equation_temp_value.bb[i] = count_B2;										//初始化线性常量
	}
		
	g_equation_temp_value = equation_temp_value(ax,ay,g_equation_temp_value,m);		//相关运算,得到设定方程的临时参数

	free(ay);
	ay = NULL;
	free(ax);
	ax = NULL;

	Gauss(g_equation_temp_value.V,g_equation_temp_value.bb,5);						//计算得到拟合椭圆参数,存储在bb[0]~bb[4]中

	free(g_equation_temp_value.V);
	g_ellipse_parameter = ellipse_parameter_calculate(g_equation_temp_value,g_ellipse_parameter);//计算得到椭圆倾斜角等量

	free(g_equation_temp_value.bb);

	cos_phi=cos(g_ellipse_parameter.orientation_rad);
	sin_phi=sin(g_ellipse_parameter.orientation_rad);
	
	to_fd = fopen("/magn_data_output.txt","w+");
	if(to_fd == NULL){
		printf("open '/magn_data_output.txt' failed!");
		return -1;
	}
	
	sleep(2);
	Accel_Correct(mpu9150_fd);				//消除固有误差(放置不水平等问题)
	while(1){
		Get_AccelGyro_Data(mpu9150_fd);
		//Get_9150Magn_Data(mpu9150_fd);	//未驱动使用;

		Coordinate_Transformation();		//将9150坐标系变换到磁力计lis3mdl坐标系;

		Calculate_PitchRoll();				//计算俯仰角和侧倾角
		
		Calculate_Heading(magn_fd,to_fd);
		Printf_AccelGyroMagn_Output();
		usleep(20*1000);
	}
	
	close(mpu9150_fd);
	close(magn_fd);
	fclose(to_fd);
	
	return 0;
}