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;
}
void imuLoop()
{
float dtsum = 0.0f; //sum of delta t's
while(dtsum < 1.0f/freq) //run this loop at 1300 Hz (Max frequency gives best results for Mahony filter)
{
//----------------------- 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->update();
imu->read_accelerometer(&ax, &ay, &az);
imu->read_gyroscope(&gx, &gy, &gz);
ax /= G_SI;
ay /= G_SI;
az /= G_SI;
gx *= 180 / PI;
gy *= 180 / PI;
gz *= 180 / PI;
ahrs.updateIMU(ax, ay, az, gx*0.0175, gy*0.0175, gz*0.0175, dt);
// Accel + gyro + mag.
// Soft and hard iron calibration required for proper function.
/*
imu->update();
imu->read_accelerometer(&ay, &ax, &az);
az *= -1;
imu->read_gyroscope(&gy, &gx, &gz);
gz *= -1;
imu->read_magnetometer(&mx, &my, &mz);
ax /= G_SI;
ay /= G_SI;
az /= G_SI;
gx *= 180 / PI;
gy *= 180 / PI;
gz *= 180 / PI;
ahrs.update(-ax, -ay, -az, gx*0.0175, gy*0.0175, gz*0.0175, mx, my, mz, dt);
*/
//------------------------ Read Euler angles ------------------------------
ahrs.getEuler(&pitch, &roll, &yaw);
//------------------- Discard the time of the first cycle -----------------
if (!isFirst)
{
if (dt > maxdt) maxdt = dt;
if (dt < mindt) mindt = dt;
}
isFirst = 0;
dtsum += dt;
}
}
void imuLoop ( void ) {
//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 + mag.
// Soft and hard iron calibration required for proper function.
imu -> update();
imu -> read_accelerometer( &ay , &ax , &az );
imu -> read_gyroscope( &gy , &gx , &gz );
imu -> read_magnetometer( &mx , &my , &mz );
ax /= G_SI;
ay /= G_SI;
az /= G_SI;
my = - my;
ahrs.update( ax , ay , az , gx , gy , gz , mx , my , mz , dt );
//Read Euler angles
ahrs.getEuler( &pitch , &roll , &yaw );
//Discard the time of the first cycle
if ( !isFirst ) {
if ( dt > maxdt ) maxdt = dt;
if ( dt < mindt ) mindt = dt;
}
isFirst = 0;
//Console and network output with a lowered rate
dtsumm += dt;
if( dtsumm > 0.05 ) {
//Console output
//printf("ROLL: %+05.2f PITCH: %+05.2f YAW: %+05.2f PERIOD %.4fs RATE %dHz \n", roll, pitch, gz/*yaw * -1*/, dt, int(1/dt));
// Network output
//sprintf( sendline , "%10f %10f %10f %10f %dHz\n" ,ahrs.getW() ,ahrs.getX() ,ahrs.getY() ,ahrs.getZ() ,int( 1/dt ) );
//sendto( sockfd , sendline , strlen( sendline ) , 0 , ( struct sockaddr * )&servaddr , sizeof( servaddr ) );
dtsumm = 0;
}
}
void imuLoop()
{
//----------------------- 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->update();
imu->read_accelerometer(&ax, &ay, &az);
imu->read_gyroscope(&gx, &gy, &gz);
ax /= G_SI;
ay /= G_SI;
az /= G_SI;
gx *= 180 / PI;
gy *= 180 / PI;
gz *= 180 / PI;
ahrs.updateIMU(ax, ay, az, gx*0.0175, gy*0.0175, gz*0.0175, dt);
*/
// Accel + gyro + mag.
// Soft and hard iron calibration required for proper function.
imu->update();
imu->read_accelerometer(&ay, &ax, &az);
imu->read_gyroscope(&gy, &gx, &gz);
imu->read_magnetometer(&my, &mx, &mz);
ax /= -G_SI;
ay /= -G_SI;
az /= G_SI;
gx *= 180 / PI;
gy *= 180 / PI;
gz *= -180 / PI;
ahrs.update(ax, ay, az, gx*0.0175, gy*0.0175, gz*0.0175, my, mx, -mz, dt);
//------------------------ Read Euler angles ------------------------------
//ahrs.getEuler(&pitch, &roll, &yaw);//roll and pitch inverted
ahrs.getEuler(&roll, &pitch, &yaw);
//------------------- Discard the time of the first cycle -----------------
if (!isFirst)
{
if (dt > maxdt) maxdt = dt;
if (dt < mindt) mindt = dt;
}
isFirst = 0;
}