Commit 8ddd6b71 authored by asantamaria's avatar asantamaria

new imu_z_up parameter

parent eda4c523
......@@ -72,6 +72,8 @@ private:
double t_ini_flow2d_; // Initial RANGE sensor time.
double t_ini_px4_; // Initial PX4 sensor time.
bool imu_z_up_; //Indicates if IMU frame is upwards or downwards.
int seq_; // Odometry sequence counter.
std::string odom_frame_id_; // Odometry frame ID.
std::string robot_frame_id_; // Robot frame ID.
......
......@@ -11,6 +11,9 @@ dwb0_std: [0.0037, 0.0040, 0.0]
dg0_std: [0.0, 0.0, 0.0]
frame: global # Frame r.t. orientation derivation is computed (global or local)
#IMU pose`
imu_z_up: true
#IMU STD values
imu_a_std: [0.2,0.2,0.2]
imu_w_std: [0.005,0.005,0.005]
......
......@@ -95,6 +95,7 @@ void EskfOdomAlgNode::read_and_set_ini_params(void)
imu_params.std.segment(3,3) = read_vec("imu_w_std", 3);
imu_params.std.segment(6,3) = read_vec("imu_ab_std", 3);
imu_params.std.segment(9,3) = read_vec("imu_wb_std", 3);
this->public_node_handle_.param<bool>("imu_z_up", this->imu_z_up_, true);
std::string imu_met;
this->public_node_handle_.param<std::string>("imu_method", imu_met, "continuous");
......@@ -428,14 +429,26 @@ void EskfOdomAlgNode::imu_mutex_exit(void)
void EskfOdomAlgNode::set_imu_reading(const sensor_msgs::Imu::ConstPtr& msg,const float& msg_time)
{
// Get sensor values
this->imu_mutex_enter();
float ax = msg->linear_acceleration.x;
float ay = -msg->linear_acceleration.y; // axis change according to simulation
float az = -msg->linear_acceleration.z; // axis change according to simulation
float wx = msg->angular_velocity.x;
float wy = -msg->angular_velocity.y; // axis change according to simulation
float wz = -msg->angular_velocity.z; // axis change according to simulation
this->imu_mutex_enter();
float ax, ay, az, wx, wy, wz;
if (this->imu_z_up_)
{
ax = msg->linear_acceleration.x;
ay = msg->linear_acceleration.y; // axis change according to simulation
az = msg->linear_acceleration.z; // axis change according to simulation
wx = msg->angular_velocity.x;
wy = msg->angular_velocity.y; // axis change according to simulation
wz = msg->angular_velocity.z; // axis change according to simulation
}
else
{
ax = msg->linear_acceleration.x;
ay = -msg->linear_acceleration.y; // axis change according to simulation
az = -msg->linear_acceleration.z; // axis change according to simulation
wx = msg->angular_velocity.x;
wy = -msg->angular_velocity.y; // axis change according to simulation
wz = -msg->angular_velocity.z; // axis change according to simulation
}
this->imu_mutex_exit();
Eigen::Vector3f a;
......
Markdown is supported
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