Commit ca408a5d authored by asantamaria's avatar asantamaria

changed run_nest_step to update

parent 80a57670
......@@ -391,7 +391,7 @@ class EskfOdomAlgorithm
* state: Robot state (6D).
* ang_vel: Robot Angular Velocity (currently not included in the state).
*/
bool run_next_step(Eigen::VectorXf& state, Eigen::Vector3f& ang_vel);
bool update(Eigen::VectorXf& state, Eigen::Vector3f& ang_vel, const bool& flying, const float& gnd_dist);
/**
* \brief Destructor
......
......@@ -32,11 +32,6 @@ void EskfOdomAlgorithm::print_ini_params(void)
this->filter_.print_ini_params();
}
void EskfOdomAlgorithm::set_obs_phase(const bool& flying, const float& gnd_dist)
{
this->filter_.set_obs_phase(flying,gnd_dist);
}
Sensor::imu_params EskfOdomAlgorithm::get_imu_params(void)
{
return this->filter_.get_imu_params();
......@@ -173,7 +168,7 @@ float EskfOdomAlgorithm::get_proc_time(void)
{
return this->filter_.get_proc_time();
}
bool EskfOdomAlgorithm::run_next_step(Eigen::VectorXf& state, Eigen::Vector3f& ang_vel)
bool EskfOdomAlgorithm::update(Eigen::VectorXf& state, Eigen::Vector3f& ang_vel, const bool& flying, const float& gnd_dist)
{
return this->filter_.run_next_step(state,ang_vel);
return this->filter_.update(state,ang_vel, flying, gnd_dist);
}
\ No newline at end of file
......@@ -207,16 +207,12 @@ void EskfOdomAlgNode::mainNodeThread(void)
bool is_flying = this->flying_;
this->flying_mutex_exit();
this->alg_.lock();
this->alg_.set_obs_phase(is_flying,this->range_dist_(0));
this->alg_.unlock();
Eigen::VectorXf state(19,1);
Eigen::Vector3f ang_vel;
bool step_done = false;
this->alg_.lock();
step_done = this->alg_.run_next_step(state,ang_vel);
step_done = this->alg_.update(state,ang_vel,is_flying,this->range_dist_(0));
this->alg_.unlock();
if (step_done)
......
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