Commit 6c8950b5 authored by asantamaria's avatar asantamaria

Added set flying when range of px4 is above 0.31 in set_px4_range

parent 8ddd6b71
......@@ -485,7 +485,7 @@ void EskfOdomAlgNode::set_px4_reading(const px_comm::OpticalFlow::ConstPtr& msg,
this->px4_of_mutex_exit();
Eigen::Vector3f val;
val << this->range_dist_, vx, vy;
val << this->range_dist_(0), vx, vy;
float flowx = msg->flow_x;
float flowy = -msg->flow_y; // axis change according to simulation
......@@ -510,7 +510,7 @@ void EskfOdomAlgNode::set_px4_reading(const px_comm::OpticalFlow::ConstPtr& msg,
// // this->alg_.set_px4_params(px4_params);
// // }
//this->alg_.set_px4_reading(msg_time, val, flow); // Set values into filter object
this->alg_.set_px4_reading(msg_time, val, flow); // Set values into filter object
//*****************************
// USING RANGE AND RAW FLOW
......@@ -534,8 +534,14 @@ void EskfOdomAlgNode::set_px4_reading(const px_comm::OpticalFlow::ConstPtr& msg,
// this->alg_.set_flow2d_params(flow2d_params);
// }
this->alg_.set_range_reading(msg_time, this->range_dist_); // Set values into filter object
this->alg_.set_flow2d_reading(msg_time, flow); // Set values into filter object
//this->alg_.set_range_reading(msg_time, this->range_dist_); // Set values into filter object
//this->alg_.set_flow2d_reading(msg_time, flow); // Set values into filter object
// Temporary
if (this->range_dist_(0) > 0.31)
this->flying_ = true;
else
this->flying_ = false;
this->alg_.unlock();
}
......
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