fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 02:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

EKF: Rework nav validity reporting

Browse Source 
Remove duplicate checking for dead reckoning and consolidate into a single function.
Use separate timers to check for start of dead reckoning and check when dead reckoning has been performed for too long for the nav solution to be valid.
Allow the timeout for validity reporting to be adjusted externally.
Separate external reporting of dead reckoning from internal checks.
This commit is contained in:
Paul Riseborough
2018-04-21 13:04:04 -04:00
committed by Daniel Agar
parent bd72f3c521
commit 9747dc778d
6 changed files with 21 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
EKF/ekf_helper.cpp
+11 -3
View File
@@ -1296,11 +1296,11 @@ void Ekf::setDiag(float (&cov_mat)[_k_num_states][_k_num_states], uint8_t first,
bool Ekf::global_position_is_valid()
{
// return true if we are not doing unconstrained free inertial navigation and the origin is set
return (_NED_origin_initialised && !inertial_dead_reckoning());
return (_NED_origin_initialised && !_deadreckon_time_exceeded);
}
// return true if we are totally reliant on inertial dead-reckoning for position
bool Ekf::inertial_dead_reckoning()
void Ekf::update_deadreckoning_status()
{
bool velPosAiding = (_control_status.flags.gps || _control_status.flags.ev_pos)
&& ((_time_last_imu - _time_last_pos_fuse <= _params.no_aid_timeout_max)
@@ -1309,7 +1309,15 @@ bool Ekf::inertial_dead_reckoning()
bool optFlowAiding = _control_status.flags.opt_flow && (_time_last_imu - _time_last_of_fuse <= _params.no_aid_timeout_max);
bool airDataAiding = _control_status.flags.wind && (_time_last_imu - _time_last_arsp_fuse <= _params.no_aid_timeout_max) && (_time_last_imu - _time_last_beta_fuse <= _params.no_aid_timeout_max);
return !velPosAiding && !optFlowAiding && !airDataAiding;
_is_dead_reckoning = !velPosAiding && !optFlowAiding && !airDataAiding;
// record the time we start inertial dead reckoning
if (!_is_dead_reckoning) {
_time_ins_deadreckon_start = _time_last_imu - _params.no_aid_timeout_max;
}
// report if we have been deadreckoning for too long
_deadreckon_time_exceeded = ((_time_last_imu - _time_ins_deadreckon_start) > (unsigned)_params.valid_timeout_max);
}
// perform a vector cross product