Commit 0e97cac5 authored by adrianamor's avatar adrianamor

Fixed time stamps of subscriber callbacks to work with NO_read_from_file.

parent c683becb
......@@ -64,11 +64,16 @@ class EskfOdomAlgNode : public algorithm_base::IriBaseAlgorithm<EskfOdomAlgorith
double gnd_dist_; // Ground distance (m) obtained from PX4 optical flow pointing downward.
#ifndef READ_FROM_FILE
bool is_first_; // First reading should not contribute to propagate nominal (integration requirements)
bool is_first_imu_; // First IMU reading should not contribute to propagate nominal (integration requirements).
// Also used to get initial sensor time.
bool is_first_px4_; // Used to get initial sensor time.
bool new_imu_; // Flag indicating a new IMU message is received.
bool new_px4_; // Flag indicating a new PX4 message is received.
double t_ini_px4_; // Initial PX4 sensor time.
double t_ini_imu_; // Initial IMU sensor time.
// [publisher attributes]
// [subscriber attributes]
......
......@@ -14,7 +14,8 @@ EskfOdomAlgNode::EskfOdomAlgNode(void) :
this->loop_rate_ = 100;//in [Hz]
// Initialize vars
this->is_first_ = true;
this->is_first_imu_ = true;
this->is_first_px4_ = true;
this->new_imu_ = false;
this->new_px4_ = false;
......@@ -317,8 +318,8 @@ void EskfOdomAlgNode::mainNodeThread(void)
if (this->new_imu_)
{
this->new_imu_ = false;
if (this->is_first_)
this->is_first_ = false;
if (this->is_first_imu_)
this->is_first_imu_ = false;
else
{
this->alg_.lock(); // protect algorithm
......@@ -389,14 +390,27 @@ void EskfOdomAlgNode::mainNodeThread(void)
val << this->gnd_dist_,vx,vy;
double t = stamp.toSec();
if (this->is_first_px4_){
this->t_ini_px4_= t;
this->is_first_px4_ = false;
}
double curr_t = t-this->t_ini_px4_;
double flowx = msg->flow_x;
double flowy = -msg->flow_y; // axis change according to simulation
Vector2d flow;
flow << flowx,flowy;
this->alg_.lock();
this->alg_.set_px4_reading(t,val); // Set values into filter object
this->alg_.set_px4_reading(curr_t,val,flow); // Set values into filter object
this->alg_.unlock();
if (!this->is_first_) // We first want to propagate with the IMU
if (!this->is_first_imu_) // We first want to propagate with the IMU
this->new_px4_ = true;
printf("PX4 %2.10f \n",t);
// printf("PX4 %2.10f \n",t);
}
......@@ -427,18 +441,23 @@ void EskfOdomAlgNode::mainNodeThread(void)
double t = stamp.toSec();
if (this->is_first_imu_)
this->t_ini_imu_= t;
double curr_t = t-this->t_ini_imu_;
Vector3d a;
a << ax,ay,az;
Vector3d w;
w << wx,wy,wz;
this->alg_.lock();
this->alg_.set_imu_reading(t,a,w); // Set values into filter object
this->alg_.set_imu_reading(curr_t,a,w); // Set values into filter object
this->alg_.unlock();
this->new_imu_ = true;
printf("IMU %2.10f \n",t);
// printf("IMU %2.10f \n",t);
}
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