int main()
{
//-------------------------------------------------------------------------
MPU9250 imu;
imu.initialize();
/* test code
if(imu.testConnection()){
printf("true");
}
else{
printf("false");
}*/
float ax, ay, az, gx, gy, gz, mx, my, mz, pitch, roll, realX, realY, heading, cx, cy, cz;
// FILE * pFile;
// pFile = fopen("AGMOUTPUT.txt","w");
//-------------------------------------------------------------------------
imu.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);
//WEIRDASS AXES - SEE DATASHEET
// cx = gy;
// cy = gx;
// cz = -gz;
//adjust for tilt
//pitch = cy * (PI / 180);
//roll = cx * (PI / 180);
//realX = cx*cos(pitch) + cz*sin(pitch);
//realY = cx*sin(roll)*sin(pitch) + cy*cos(roll) - cz*sin(roll)*sin(pitch);
//Calculate heading
//heading = (180 / PI) * atan2(realY, realX);
//printf("Heading: %+07.3f", heading);
/*
// print to file
// fprintf(pFile,"Heading: %+07.3f\n", heading);
// fprintf(pFile,"Acc: %+07.3f %+07.3f %+07.3f ", ax, ay, az);
fprintf(pFile,"Gyr: %+07.3f %+07.3f %+07.3f", gx, gy, gz);
fprintf(pFile,"Mag: %+07.3f %+07.3f %+07.3f\n", mx, my, mz);
fflush(pFile);
*/
//Nice Pretty Terminal output/
printf("Acc: %+07.3f %07.3f %+07.3f ", ax, ay, az);
printf("Gyr: %+07.3f %+07.3f %+07.3f ", gx, gy, gz);
printf("Mag: %+07.3f %+07.3f %+07.3f\n", mx, my, mz);
fflush(stdout);
}
void imuSetup()
{
//----------------------- MPU initialization ------------------------------
imu.initialize();
//-------------------------------------------------------------------------
printf("Beginning Gyro calibration...\n");
for(int i = 0; i<100; i++)
{
imu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
offset[0] += (-gx*0.0175);
offset[1] += (-gy*0.0175);
offset[2] += (-gz*0.0175);
usleep(10000);
}
offset[0]/=100.0;
offset[1]/=100.0;
offset[2]/=100.0;
printf("Offsets are: %f %f %f\n", offset[0], offset[1], offset[2]);
ahrs.setGyroOffset(offset[0], offset[1], offset[2]);
}
