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;

}
Beispiel #2
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;
    }


    
}
Beispiel #3
0
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;

	}

}
Beispiel #4
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;
}