Beispiel #1
0
void main()
{
	int i2c_file;
	int i;
	unsigned char val;
	short accel[3]={0};
	short gyro[3]={0};
	if ((i2c_file = open(I2C_FILE_NAME, O_RDWR)) < 0) {
        	perror("Unable to open i2c control file");
        	exit(1);
    	}

	mpu6050_init(i2c_file);
	initkalman();
	while(1)
	{
		getmotion6(i2c_file,&accel[0],&accel[1],&accel[2],&gyro[0],&gyro[1],&gyro[2]);
		kalman((float)gyro[0],(float)gyro[1],(float)gyro[2],(float)accel[0],(float)accel[1],(float)accel[2]);		
		IMUupdate((float)predictdata[3],(float)predictdata[4],(float)predictdata[5],(float)predictdata[0],(float)predictdata[1],(float)predictdata[2]);
		//printf("accel = %d ,%d ,%d ,gyro = %d ,%d ,%d\n",accel[0],accel[1],accel[2],gyro[0],gyro[1],gyro[2]);
		//printf("q0 = %f,q1 = %f,q2 = %f,q3 = %f\n",q0,q1,q2,q3);
		printf("Pitch = %f,Roll = %f,Yaw = %f\n",Pitch,Roll,Yaw);	
	}
	for(i=0;i<=0x75;i++)
	{
		get_i2c_register(i2c_file,0x68,i,&val);
		printf("addr %x= %x\n",i,val);
		
	}
	
	
	
}
Beispiel #2
0
boxtrackproperties::
boxtrackproperties(void) :
k(cvCreateKalman(nstate,nmeasure)),
kvar(9.),
//      k_dt(0.067),
k_dt(0.1),
statepost(k->state_post),
statepre(k->state_pre),
uid_tag(boost::uuids::random_generator(ran)())
{
  initkalman(cv::Mat::zeros(nmeasure,1,CV_32F));
}