Pitch angle from IMU, not displayed correct [closed]

asked 2013-05-16 22:05:56 -0500

Astronaut gravatar image

Hello

Im trying to get the pitch angle when my platform is going up/down a rump and try to get the time needed to up/down a ramp. Im using a pitch angle of the IMU as a Schmitt-Trigger. But the pitch angle displayed is not correct. This is my node.

#include <ros/ros.h>
#include <fstream>
#include "sensor_msgs/Imu.h"
#include "tf/transform_datatypes.h"
#include "/opt/ros/fuerte/stacks/bullet/include/LinearMath/btMatrix3x3.h"
#include "/opt/ros/fuerte/stacks/bullet/include/LinearMath/btQuaternion.h"

#define RAMP_ANGLE 5.0 //degrees

const double ramp_ang = RAMP_ANGLE * (3.1416/180.0);
double r,p,y, initial_pitch, ramp_pitch; tf::Quaternion quat;
ros::Time start_time, end_time;
bool is_first = 1, first_time_rec = 0, end_time_rec = 0;

std::ofstream myfile;

void imuCallback(const sensor_msgs::ImuConstPtr& msg)
{
  tf::quaternionMsgToTF(msg->orientation, quat);
  tf::Matrix3x3(quat).getRPY(r,p,y);

  if(is_first)
  {
    initial_pitch = p;
    is_first = 0;
  }
  else
  {
    if(fabs(p-initial_pitch) >= ramp_ang && !first_time_rec)
    {
        ramp_pitch = (p-initial_pitch) * (3.1416/180.0);
        start_time = ros::Time::now();
        first_time_rec = 1;
    }
    else if((first_time_rec && !end_time_rec) && fabs(p-initial_pitch) <= ramp_ang)
    {
        end_time = ros::Time::now();
        end_time_rec = 1;

    }
  }

    ROS_INFO("roll =  %f, pitch=  %f, yaw= %f",r,p,y); 
    myfile << r << "    " << p << " "<< y << "\n";

}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "imu_odom");
  ros::NodeHandle nh;

  ros::Subscriber sub = nh.subscribe("raw_imu_throttle", 1, imuCallback);
  while(!end_time_rec) ros::spinOnce();

  double time = (end_time-start_time).toSec();

ros::spin();
 return 0;
}

Any help??

edit retag flag offensive reopen merge delete

Closed for the following reason question is not relevant or outdated by tfoote
close date 2017-08-04 18:37:54.147102

Comments

1

Could you state what is not correct?

dornhege gravatar imagedornhege ( 2013-05-17 01:21:52 -0500 )edit

The pitch angle is not correct displayed. So im not sure that roll, pitch and yaw and correctly displayed in the order they should be.. Might be that the pitch angle I got displayed in ROSINFO is a yaw or the roll.

Astronaut gravatar imageAstronaut ( 2013-05-17 04:08:43 -0500 )edit

Any help with this please?

Astronaut gravatar imageAstronaut ( 2013-05-19 17:01:51 -0500 )edit

I can't see something wrong with the code you provided. If you cannot be more precise about your data, i.e. what you get and what you expect it will be hard to impossible to give an answer besides random guessing. "not correct" doesn't really say anything specific.

dornhege gravatar imagedornhege ( 2013-05-21 01:27:46 -0500 )edit

Is it possible that you've mounted your IMU in the wrong orientation? The pitch angle should be the angle about the Y-axis of the IMU.

Ed Venator gravatar imageEd Venator ( 2017-07-07 22:57:16 -0500 )edit