Pitch angle from IMU, not displayed correct [closed]
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??
Could you state what is not correct?
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.
Any help with this please?
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.
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.