Commit 46fb2716 authored by adrianamor's avatar adrianamor

Ready to start debug. Now state get too big

parent 29b3460a
......@@ -152,7 +152,57 @@ class EskfOdomAlgorithm
*/
void set_obs_phase(const bool& flying, const double& gnd_dist);
/**
* \brief Set IMU Readings
*
* Store new IMU readings
*
* Input:
* t: Time stamp.
* a: Acc. readings (m/s^2). a = [ax,ay,az].
* w: Gyro. readings (rad/s). w = [wx,wy,wz].
*/
void set_imu_reading(const double& t, const Vector3d& a, const Vector3d& w);
/**
* \brief Set PX4 Optical Flow Readings
*
* Store new PX4 Optical Flow readings
*
* Input:
* t: Time stamp.
* val: Sensor readings: val = [pz;vx;vy].
*/
void set_px4_reading(const double& t, const Vector3d val);
/**
* \brief Nominal State Propagation
*
* Nominal State propagation using IMU model.
*
* Inputs:
* There are no specific inputs due to all are class variables.
*/
void prop_nominal(void);
/**
* \brief Observe estimation error and Update Nominal state
*
* Observe estimation error and Update Nominal state.
*
* Inputs:
* There are no specific inputs due to all are class variables.
*/
void obs_error_and_up_nominal(void);
/**
* \brief Get nominal-state vector
*
* Get nominal-state vector. Function to prevent external modifications.
*
*/
VectorXd get_x_state(void);
/**
* \brief Destructor
*
......
......@@ -37,6 +37,8 @@
// [action server client headers]
#include <tf/transform_broadcaster.h>
#include <Eigen/Dense>
using namespace std;
......
......@@ -29,7 +29,7 @@ void EskfOdomAlgorithm::set_init_params(const params& f_params, const VectorXd&
this->filter_.set_init_params(f_params,x0,dx0,imu,px4);
}
void EskfOdomAlgorithm::print_ini_params()
void EskfOdomAlgorithm::print_ini_params(void)
{
this->filter_.print_ini_params();
}
......@@ -37,4 +37,29 @@ void EskfOdomAlgorithm::print_ini_params()
void EskfOdomAlgorithm::set_obs_phase(const bool& flying, const double& gnd_dist)
{
this->filter_.set_obs_phase(flying,gnd_dist);
}
void EskfOdomAlgorithm::set_imu_reading(const double& t, const Vector3d& a, const Vector3d& w)
{
this->filter_.set_imu_reading(t,a,w);
}
void EskfOdomAlgorithm::set_px4_reading(const double& t, const Vector3d val)
{
this->filter_.set_px4_reading(t,val);
}
void EskfOdomAlgorithm::prop_nominal(void)
{
this->filter_.prop_nominal();
}
void EskfOdomAlgorithm::obs_error_and_up_nominal(void)
{
this->filter_.obs_error_and_up_nominal();
}
VectorXd EskfOdomAlgorithm::get_x_state(void)
{
return this->filter_.get_x_state();
}
\ No newline at end of file
......@@ -98,11 +98,16 @@ void EskfOdomAlgNode::read_and_set_ini_params(void)
// Initialize Error-State vector
VectorXd dx_state = read_vec("dxstate0",18);
this->alg_.lock(); // protect algorithm
// Set filter and sensors initial parameters
this->alg_.set_init_params(f_params,x_state,dx_state,imu_params,px4_params);
// Print already set values of filter and sensors initial parameters
this->alg_.print_ini_params();
this->alg_.unlock();
}
VectorXd EskfOdomAlgNode::read_vec(const string& param_name, const int& exp_long)
......@@ -138,13 +143,31 @@ void EskfOdomAlgNode::mainNodeThread(void)
// Set Quadrotor State
this->ll_status_mutex_enter();
this->px4_of_mutex_enter();
this->ll_status_mutex_enter(); // protect flying var
this->px4_of_mutex_enter(); // protect gnd_dist
this->alg_.lock(); // protect algorithm
// Set observation phase (Landed, toff, Flying or Landing)
this->alg_.set_obs_phase(this->flying_,this->gnd_dist_);
// get state
VectorXd state = this->alg_.get_x_state();
cout << "TODO: Debug. Why the states get so big?" << endl;
cout << state(0) << "," << state(1) << "," << state(2) << endl;
this->alg_.unlock();
this->ll_status_mutex_exit();
this->px4_of_mutex_exit();
// Broadcast TF with state
static tf::TransformBroadcaster br;
tf::Transform transform;
transform.setOrigin(tf::Vector3(state(0), state(1), state(2)));
tf::Quaternion q(state(7),state(8),state(9),state(6)); //ROS:[qx,qy,qz,qw] Filter:[qw,qx,qy,qz]
transform.setRotation(q);
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "base_link"));
}
/* [subscriber callbacks] */
......@@ -152,14 +175,8 @@ void EskfOdomAlgNode::ll_status_callback(const asctec_msgs::LLStatus::ConstPtr&
{
ROS_DEBUG("EskfOdomAlgNode::ll_status_callback: New Message Received");
//use appropiate mutex to shared variables if necessary
//this->alg_.lock();
this->ll_status_mutex_enter();
this->flying_ = msg->flying;
//unlock previously blocked shared variables
//this->alg_.unlock();
this->ll_status_mutex_exit();
}
......@@ -176,15 +193,27 @@ void EskfOdomAlgNode::px4_of_callback(const px_comm::OpticalFlow::ConstPtr& msg)
{
ROS_DEBUG("EskfOdomAlgNode::px4_of_callback: New Message Received");
//use appropiate mutex to shared variables if necessary
//this->alg_.lock();
// Get sensor values
this->px4_of_mutex_enter();
this->gnd_dist_ = msg->ground_distance;
//unlock previously blocked shared variables
//this->alg_.unlock();
double vx = msg->velocity_x;
double vy = msg->velocity_y;
ros::Time stamp = msg->header.stamp;
this->px4_of_mutex_exit();
Vector3d val;
val << this->gnd_dist_,vx,vy;
double t = stamp.toSec();
this->alg_.lock();
// Set values into filter object
this->alg_.set_px4_reading(t,val);
// Error observation and Nominal-State Update
this->alg_.obs_error_and_up_nominal();
this->alg_.unlock();
}
void EskfOdomAlgNode::px4_of_mutex_enter(void)
......@@ -200,13 +229,32 @@ void EskfOdomAlgNode::imu_callback(const sensor_msgs::Imu::ConstPtr& msg)
{
ROS_DEBUG("EskfOdomAlgNode::imu_callback: New Message Received");
//use appropiate mutex to shared variables if necessary
//this->alg_.lock();
//this->imu_mutex_enter();
// Get sensor values
this->imu_mutex_enter();
double ax = msg->linear_acceleration.x;
double ay = msg->linear_acceleration.y;
double az = msg->linear_acceleration.z;
double wx = msg->angular_velocity.x;
double wy = msg->angular_velocity.y;
double wz = msg->angular_velocity.z;
ros::Time stamp = msg->header.stamp;
this->imu_mutex_exit();
double t = stamp.toSec();
Vector3d a;
a << ax,ay,az;
Vector3d w;
w << wx,wy,wz;
this->alg_.lock();
// Set values into filter object
this->alg_.set_imu_reading(t,a,w);
// Propagate Nominal-State
this->alg_.prop_nominal();
//unlock previously blocked shared variables
//this->alg_.unlock();
//this->imu_mutex_exit();
this->alg_.unlock();
}
void EskfOdomAlgNode::imu_mutex_enter(void)
......
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