Commit 50c02295 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Solved the NED -> ENU conversion for the microstrain IMU

parent 80363d9d
......@@ -121,9 +121,10 @@ void ImuMsgFixAlgNode::imu_in_callback(const sensor_msgs::Imu::ConstPtr& msg)
//TODO: modify yaw, pitch or roll
double tmp_pitch = pitch;
pitch=roll;
roll=tmp_pitch;
yaw= -yaw;
//pitch=-roll;
//roll=tmp_pitch;
//yaw=yaw;
yaw-=1.5707;
tf2::Quaternion quat_tf;
quat_tf.setRPY(roll, pitch, yaw);
......@@ -132,13 +133,21 @@ void ImuMsgFixAlgNode::imu_in_callback(const sensor_msgs::Imu::ConstPtr& msg)
this->imu_out_Imu_msg_.orientation = quat_msg;
this->imu_out_Imu_msg_.angular_velocity.x=msg->angular_velocity.y;
this->imu_out_Imu_msg_.angular_velocity.y=msg->angular_velocity.x;
this->imu_out_Imu_msg_.angular_velocity.z=-msg->angular_velocity.z;
//this->imu_out_Imu_msg_.angular_velocity.x=msg->angular_velocity.y;
//this->imu_out_Imu_msg_.angular_velocity.y=msg->angular_velocity.x;
//this->imu_out_Imu_msg_.angular_velocity.z=-msg->angular_velocity.z;
this->imu_out_Imu_msg_.linear_acceleration.x=msg->linear_acceleration.y;
this->imu_out_Imu_msg_.linear_acceleration.y=msg->linear_acceleration.x;
this->imu_out_Imu_msg_.linear_acceleration.z=-msg->linear_acceleration.z;
//this->imu_out_Imu_msg_.linear_acceleration.x=msg->linear_acceleration.y;
//this->imu_out_Imu_msg_.linear_acceleration.y=msg->linear_acceleration.x;
//this->imu_out_Imu_msg_.linear_acceleration.z=-msg->linear_acceleration.z;
this->imu_out_Imu_msg_.angular_velocity.x=msg->angular_velocity.x;
this->imu_out_Imu_msg_.angular_velocity.y=msg->angular_velocity.y;
this->imu_out_Imu_msg_.angular_velocity.z=msg->angular_velocity.z;
this->imu_out_Imu_msg_.linear_acceleration.x=msg->linear_acceleration.x;
this->imu_out_Imu_msg_.linear_acceleration.y=msg->linear_acceleration.y;
this->imu_out_Imu_msg_.linear_acceleration.z=msg->linear_acceleration.z;
}
else
{
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment