int main(int argc, char *argv[])
{
//-------------------- IMU setup and main loop ----------------------------
imuSetup();
ros::init(argc, argv, "ros_erle_imu_euler");
ros::NodeHandle n;
ros::Publisher imu_euler_pub = n.advertise<ros_erle_imu_euler::euler>("euler", 1000);
ros::Rate loop_rate(50);
while (ros::ok()){
//----------------------- Calculate delta time ----------------------------
gettimeofday(&tv,NULL);
previoustime = currenttime;
currenttime = 1000000 * tv.tv_sec + tv.tv_usec;
dt = (currenttime - previoustime) / 1000000.0;
if(dt < 1/1300.0)
usleep((1/1300.0-dt)*1000000);
gettimeofday(&tv,NULL);
currenttime = 1000000 * tv.tv_sec + tv.tv_usec;
dt = (currenttime - previoustime) / 1000000.0;
//-------- Read raw measurements from the MPU and update AHRS --------------
// Accel + gyro.
imu.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);
ahrs.updateIMU(ax, ay, az, gx*0.0175, gy*0.0175, gz*0.0175, dt);
//------------------------ Read Euler angles ------------------------------
ahrs.getEuler(&roll, &pitch, &yaw);
ros_erle_imu_euler::euler msg;
msg.roll = roll;
msg.pitch = pitch;
msg.yaw = yaw;
imu_euler_pub.publish(msg);
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
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);
}
