Pitch angle from IMU, not displayed correct [closed]

edit

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??