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