pitch angle from IMU by driving the robot up/down a ramp [closed]

edit

Hello

I trying to get the total angle of a ramp when my robot is driving it up/down of the ramp. I can get the current ramp angle but cant get the total angle of the ramp(when robot start going up/down a ramp till the end of the ramp). I made a Callback but the variable ramp_pitch is not given me the wonted angle. Im posting the code so please any help?

#define RAMP_ANGLE 1.0 //degrees

const double ramp_ang = RAMP_ANGLE * (3.1416/180.0);
double r,p,y, initial_pitch, ramp_pitch, deg_p; 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);
        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, ramp_pitch= %f, yaw= %f",r,p*57.3,ramp_pitch*57.3,y); 


}

The ramp total angle should be given by ramp_pitch.

Any help?