Commit c33900a5 authored by adrianamor's avatar adrianamor

debugging. done after nom_prop. Next step imu trans mat

parent 0347a213
......@@ -108,6 +108,9 @@ class EskfOdomAlgNode : public algorithm_base::IriBaseAlgorithm<EskfOdomAlgorith
double t_ini_px4_;
double t_ini_llstatus_;
// double time_fix_;
// double dt_;
double max_freq_;
// vector<bool> ll_flying_; // To store bool due to different Hz
......
......@@ -44,9 +44,13 @@ EskfOdomAlgNode::EskfOdomAlgNode(void) :
#ifdef READ_FROM_FILE
this->imu_idx_ = 0;
this->px4_idx_ = 0;
this->llstatus_idx_ = 0;
// this->time_fix = 0.0;
// this->dt_=dt = 0.00999999;
this->imu_idx_ = 0.0;
this->px4_idx_ = 0.0;
this->llstatus_idx_ = 0.0;
this->a_ = read_from_file("/home/arcas/iri-lab/ros/catkin_ws/src/external/angel/eskf_odom/data/kinton_outdoor_paper/2/linear_acceleration.txt");
this->w_ = read_from_file("/home/arcas/iri-lab/ros/catkin_ws/src/external/angel/eskf_odom/data/kinton_outdoor_paper/2/angular_velocity.txt");
......@@ -57,8 +61,9 @@ EskfOdomAlgNode::EskfOdomAlgNode(void) :
this->t_ini_px4_ = this->px4_[0][0];
this->t_ini_llstatus_ = this->llstatus_[0][0];
// The first IMU value is not used (integration purposes)
double t_imu = (this->a_[this->imu_idx_][0]-t_ini_imu_)*1e-9;
double ax = this->a_[this->imu_idx_][1];
......@@ -79,7 +84,6 @@ EskfOdomAlgNode::EskfOdomAlgNode(void) :
++this->imu_idx_;
// Find message with max frequency
double imu_freq = this->a_.size()/((this->a_[this->a_.size()-1][0]-this->t_ini_imu_)*1e-9);
......@@ -244,8 +248,6 @@ void EskfOdomAlgNode::mainNodeThread(void)
// Set Quadrotor State
#ifdef READ_FROM_FILE
// ROS_DEBUG("gdist: %f, vx: %f, vy: %f", this->px4_[this->imu_idx_][4],this->px4_[this->imu_idx_][7],this->px4_[this->imu_idx_][8]);
// ROS_DEBUG("flying: %f", this->llstatus_[this->imu_idx_][10]);
ROS_DEBUG("IMU process");
......@@ -272,7 +274,6 @@ void EskfOdomAlgNode::mainNodeThread(void)
double next_t_imu = (this->a_[this->imu_idx_][0]-t_ini_imu_)*1e-9;
// PX4 process ***************
double t_px4 = (this->px4_[this->px4_idx_][0]-t_ini_px4_)*1e-9;
......
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