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