From d22599b440a959ed6d8e32fbcc7e04a751b1ad69 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Thu, 23 Feb 2017 21:24:28 +1100 Subject: [PATCH] EKF: Add reporting of inertial dead-reckoning status --- EKF/ekf.h | 3 +++ EKF/ekf_helper.cpp | 12 +++++++++++- EKF/estimator_interface.h | 3 +++ 3 files changed, 17 insertions(+), 1 deletion(-) diff --git a/EKF/ekf.h b/EKF/ekf.h index 8403daf28a..9f2060f17e 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -139,6 +139,9 @@ public: // return true if the global position estimate is valid bool global_position_is_valid(); + // return true if the EKF is dead reckoning the position using inertial data only + bool inertial_dead_reckoning(); + // return true if the etimate is valid // return the estimated terrain vertical position relative to the NED origin bool get_terrain_vert_pos(float *ret); diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index e011f36af3..5e04e2cd22 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -741,7 +741,7 @@ void Ekf::get_ekf_accuracy(float *ekf_eph, float *ekf_epv, bool *dead_reckoning) } // report dead reckoning if it is more than a second since we fused in position measurements - bool is_dead_reckoning = (_time_last_imu - _time_last_pos_fuse > 1e6) && vel_pos_aiding; + bool is_dead_reckoning = (_time_last_imu - _time_last_pos_fuse > 1e6); memcpy(ekf_eph, &hpos_err, sizeof(float)); memcpy(ekf_epv, &vpos_err, sizeof(float)); @@ -857,6 +857,16 @@ bool Ekf::global_position_is_valid() 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 +bool Ekf::inertial_dead_reckoning() +{ + bool velPosAiding = (_control_status.flags.gps || _control_status.flags.ev_pos) && ((_time_last_imu - _time_last_pos_fuse <= 1E6) || (_time_last_imu - _time_last_vel_fuse <= 1E6)); + bool optFlowAiding = _control_status.flags.opt_flow && (_time_last_imu - _time_last_of_fuse <= 1E6); + bool airDataAiding = _control_status.flags.wind && (_time_last_imu - _time_last_arsp_fuse <= 1E6); + + return !velPosAiding && !optFlowAiding && !airDataAiding; +} + // perform a vector cross product Vector3f EstimatorInterface::cross_product(const Vector3f &vecIn1, const Vector3f &vecIn2) { diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index fd9d983448..77fe746cc2 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -175,6 +175,9 @@ public: // return true if the global position estimate is valid virtual bool global_position_is_valid() = 0; + // return true if the EKF is dead reckoning the position using inertial data only + virtual bool inertial_dead_reckoning() = 0; + // return true if the estimate is valid // return the estimated terrain vertical position relative to the NED origin virtual bool get_terrain_vert_pos(float *ret) = 0;