dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

EKF: Use consistent test for navigation validity reporting 

This will enable controller to take advantage of non-inertial dead reckoning.
This commit is contained in:
Paul Riseborough 2017-10-20 11:16:48 +11:00
parent 19074fdd9e
commit e10ec59058
2 changed files with 4 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
EKF/ekf_helper.cpp
View File

@ -1144,9 +1144,8 @@ void Ekf::zeroCols(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first
bool Ekf::global_position_is_valid() bool Ekf::global_position_is_valid()
{ {
// return true if the position estimate is valid // return true if we are not doing unconstrained free inertial navigation and the origin is set
// TODO implement proper check based on published GPS accuracy, innovation consistency checks and timeout status return (_NED_origin_initialised && !inertial_dead_reckoning());
return (_NED_origin_initialised && ((_time_last_imu - _time_last_gps) < 5e6) && _control_status.flags.gps);
} }
// return true if we are totally reliant on inertial dead-reckoning for position // return true if we are totally reliant on inertial dead-reckoning for position

6
EKF/estimator_interface.cpp
View File

@ -464,8 +464,6 @@ void EstimatorInterface::unallocate_buffers()
bool EstimatorInterface::local_position_is_valid() bool EstimatorInterface::local_position_is_valid()
{ {
// return true if the position estimate is valid // return true if we are not doing unconstrained free inertial navigation
return (((_time_last_imu - _time_last_optflow) < 5e6) && _control_status.flags.opt_flow) || return !inertial_dead_reckoning();
(((_time_last_imu - _time_last_ext_vision) < 5e6) && _control_status.flags.ev_pos) ||
global_position_is_valid();
} }