Commit c6456e04 authored by adrianamor's avatar adrianamor

TAG: Debugged with read_from_file! Working with computation using global and local frame method

parent d125cf91
......@@ -172,8 +172,9 @@ class EskfOdomAlgorithm
* Input:
* t: Time stamp.
* val: Sensor readings: val = [pz;vx;vy].
* flow: Optical flow raw reading flow = [flow_x,flow_y].
*/
void set_px4_reading(const double& t, const Vector3d val);
void set_px4_reading(const double& t, const Vector3d& val, const Vector2d& flow);
/**
* \brief Nominal State Propagation
......
......@@ -12,7 +12,7 @@ imu_a_std: [0.2,0.2,0.2]
imu_w_std: [0.005,0.005,0.005]
imu_ab_std: [0.0,0.0,0.0]
imu_wb_std: [0.0,0.0,0.0]
imu_method: discrete #Sets the data time nature (continous or discrete).
imu_method: discrete #Sets the data time nature (continous: simulated IMU or discrete: IMU sensor).
# PX4 STD values
px4_std_landed: [0.1,0.001,0.001]
......
......@@ -44,9 +44,9 @@ void EskfOdomAlgorithm::set_imu_reading(const double& t, const Vector3d& a, cons
this->filter_.set_imu_reading(t,a,w);
}
void EskfOdomAlgorithm::set_px4_reading(const double& t, const Vector3d val)
void EskfOdomAlgorithm::set_px4_reading(const double& t, const Vector3d& val, const Vector2d& flow)
{
this->filter_.set_px4_reading(t,val);
this->filter_.set_px4_reading(t,val,flow);
}
void EskfOdomAlgorithm::prop_nominal(void)
......
......@@ -246,6 +246,9 @@ void EskfOdomAlgNode::mainNodeThread(void)
// [publish messages]
// Set Quadrotor State
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();
#ifdef READ_FROM_FILE
......@@ -284,12 +287,17 @@ void EskfOdomAlgNode::mainNodeThread(void)
this->gnd_dist_ = this->px4_[this->px4_idx_][4];
double vx = this->px4_[this->px4_idx_][7];
double vy = -this->px4_[this->px4_idx_][8]; // axis change according to simulation
double flowx = this->px4_[this->px4_idx_][5];
double flowy = -this->px4_[this->px4_idx_][6]; // axis change according to simulation
Vector3d val;
val << this->gnd_dist_,vx,vy;
Vector2d flow;
flow << flowx,flowy;
this->alg_.lock();
this->alg_.set_px4_reading(t_px4,val); // Set values into filter object
this->alg_.set_px4_reading(t_px4,val,flow); // Set values into filter object
this->alg_.obs_error_and_up_nominal(); // Error observation and Nominal-State Update
this->alg_.unlock();
......@@ -311,10 +319,6 @@ void EskfOdomAlgNode::mainNodeThread(void)
#endif
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();
#ifndef READ_FROM_FILE
if (this->new_imu_)
......
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