Commit 29b3460a authored by adrianamor's avatar adrianamor

implemented robot phases and px4 STD

parent d622e3f5
......@@ -31,8 +31,6 @@
//include eskf_odom_alg main library
const double PX4_MIN_HEIGHT = 0.31;
/**
* \brief IRI ROS Specific Driver Class
*
......
......@@ -15,7 +15,9 @@ imu_wb_std: [0.0,0.0,0.0]
imu_method: continuous #Sets the data time nature (continous or discrete).
# PX4 STD values
px4_std: [0.01,0.1,0.1]
px4_std_landed: [0.1,0.001,0.001]
px4_std_below30cm: [10.0,10.0,10.0]
px4_std_30cmto5m: [0.01,0.15,0.15]
# Initial Nominal-State vector (p,v,q,ab,wb,g)
xstate0: [0.0,0.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,-9.803057]
......
......@@ -36,38 +36,5 @@ void EskfOdomAlgorithm::print_ini_params()
void EskfOdomAlgorithm::set_obs_phase(const bool& flying, const double& gnd_dist)
{
Robot::kinton_phases phase;
this->filter_.get_robot_phase(phase);
switch (phase) {
case Robot::LANDED:
if (flying){
this->filter_.set_robot_phase(Robot::TOFF);
this->filter_.print_robot_phase();
}
break;
case Robot::TOFF:
if (gnd_dist > PX4_MIN_HEIGHT){
this->filter_.set_robot_phase(Robot::FLYING);
this->filter_.print_robot_phase();
}
break;
case Robot::FLYING:
if (gnd_dist < PX4_MIN_HEIGHT){
this->filter_.set_robot_phase(Robot::LANDING);
this->filter_.print_robot_phase();
}
break;
case Robot::LANDING:
if (!flying){
this->filter_.set_robot_phase(Robot::LANDED);
this->filter_.print_robot_phase();
}
else
if (gnd_dist > PX4_MIN_HEIGHT){
this->filter_.set_robot_phase(Robot::FLYING);
this->filter_.print_robot_phase();
}
break;
}
this->filter_.set_obs_phase(flying,gnd_dist);
}
\ No newline at end of file
......@@ -18,7 +18,7 @@ EskfOdomAlgNode::EskfOdomAlgNode(void) :
// Initialize ROBOT phase
this->flying_ = false;
this->gnd_dist_ = PX4_MIN_HEIGHT; // a little bit more than the minimum distance PX4 can detect (0.3m)
this->gnd_dist_ = 0; // a little bit more than the minimum distance PX4 can detect (0.3m)
// [init publishers]
......@@ -87,7 +87,10 @@ void EskfOdomAlgNode::read_and_set_ini_params(void)
// PX4 STD values
Sensor::px4_params px4_params;
px4_params.std = read_vec("px4_std",3);
px4_params.std_landed = read_vec("px4_std_landed",3);
px4_params.std_below30cm = read_vec("px4_std_below30cm",3);
px4_params.std_30cmto5m = read_vec("px4_std_30cmto5m",3);
px4_params.std = px4_params.std_landed; // Considering initially landed
// Initialize Nominal-State vector
VectorXd x_state = read_vec("xstate0",19);
......
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