Commit 9bd1f1fa authored by adrianamor's avatar adrianamor

debugging: nominal propagation done. ROS Threading needs to be solved now.

parent 29a361a7
......@@ -56,6 +56,9 @@ class EskfOdomAlgNode : public algorithm_base::IriBaseAlgorithm<EskfOdomAlgorith
double gnd_dist_; // Ground distance (m) obtained from PX4 optical flow pointing downward.
bool is_first_; // First reading should not contribute to propagate nominal (integration requirements)
bool new_imu_; // Flag indicating a new IMU message is received.
bool new_px4_; // Flag indicating a new PX4 message is received.
// [publisher attributes]
// [subscriber attributes]
......
......@@ -14,12 +14,15 @@ EskfOdomAlgNode::EskfOdomAlgNode(void) :
algorithm_base::IriBaseAlgorithm<EskfOdomAlgorithm>()
{
//init class attributes if necessary
//this->loop_rate_ = 2;//in [Hz]
// this->loop_rate_ = 100;//in [Hz]
// Initialize ROBOT phase
// Initialize vars
this->flying_ = false;
this->gnd_dist_ = 0.0; // a little bit more than the minimum distance PX4 can detect (0.3m)
this->is_first_ = true;
this->new_imu_ = false;
this->new_px4_ = false;
// [init publishers]
......@@ -144,19 +147,40 @@ void EskfOdomAlgNode::mainNodeThread(void)
// Set Quadrotor State
this->ll_status_mutex_enter(); // protect flying var
this->px4_of_mutex_enter(); // protect gnd_dist
// this->ll_status_mutex_enter(); // protect flying var
// this->px4_of_mutex_enter(); // protect gnd_dist
this->alg_.lock(); // protect algorithm
this->alg_.set_obs_phase(this->flying_,this->gnd_dist_); // Set observation phase (Landed, toff, Flying or Landing)
this->alg_.unlock();
// Set observation phase (Landed, toff, Flying or Landing)
this->alg_.set_obs_phase(this->flying_,this->gnd_dist_);
// this->ll_status_mutex_exit();
// this->px4_of_mutex_exit();
// get state
VectorXd state = this->alg_.get_x_state();
if (this->new_imu_)
{
this->new_imu_ = false;
if (this->is_first_)
this->is_first_ = false;
else
{
this->alg_.lock(); // protect algorithm
this->alg_.prop_nominal(); // Propagate Nominal-State
this->alg_.unlock();
}
}
if (this->new_px4_)
{
this->new_px4_ = false;
this->alg_.lock(); // protect algorithm
this->alg_.obs_error_and_up_nominal(); // Error observation and Nominal-State Update
this->alg_.unlock();
}
this->alg_.lock(); // protect algorithm
VectorXd state = this->alg_.get_x_state(); // get state
this->alg_.unlock();
this->ll_status_mutex_exit();
this->px4_of_mutex_exit();
// Broadcast TF with state
static tf::TransformBroadcaster br;
......@@ -165,6 +189,9 @@ void EskfOdomAlgNode::mainNodeThread(void)
tf::Quaternion q(state(7),state(8),state(9),state(6)); //TF:[qx,qy,qz,qw] Filter:[qw,qx,qy,qz]
transform.setRotation(q);
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "base_link"));
cout << "MAIN" << endl;
}
/* [subscriber callbacks] */
......@@ -192,7 +219,6 @@ void EskfOdomAlgNode::px4_of_callback(const px_comm::OpticalFlow::ConstPtr& msg)
// Get sensor values
this->px4_of_mutex_enter();
this->gnd_dist_ = msg->ground_distance;
double vx = msg->velocity_x;
double vy = -msg->velocity_y; // axis change according to simulation
......@@ -204,14 +230,14 @@ void EskfOdomAlgNode::px4_of_callback(const px_comm::OpticalFlow::ConstPtr& msg)
double t = stamp.toSec();
this->alg_.lock();
// Set values into filter object
this->alg_.set_px4_reading(t,val);
this->alg_.set_px4_reading(t,val); // Set values into filter object
this->alg_.unlock();
// Error observation and Nominal-State Update
this->alg_.obs_error_and_up_nominal();
if (!this->is_first_) // We first want to propagate with the IMU
this->new_px4_ = true;
cout << "PX4" << endl;
this->alg_.unlock();
}
void EskfOdomAlgNode::px4_of_mutex_enter(void)
......@@ -246,19 +272,13 @@ void EskfOdomAlgNode::imu_callback(const sensor_msgs::Imu::ConstPtr& msg)
w << wx,wy,wz;
this->alg_.lock();
this->alg_.set_imu_reading(t,a,w); // Set values into filter object
this->alg_.unlock();
this->new_imu_ = true;
// Set values into filter object
this->alg_.set_imu_reading(t,a,w);
cout << "IMU" << endl;
if (this->is_first_)
this->is_first_ = false;
else
{
// Propagate Nominal-State
this->alg_.prop_nominal();
}
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