From eae0522dc2a2762b19e3993224bfb37ebfc771b8 Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Mon, 17 Jul 2017 16:42:42 +0200 Subject: [PATCH] split into get_terrain_valid and get_terrain_vert_pos --- EKF/ekf.h | 8 +++++--- EKF/ekf_helper.cpp | 3 +-- EKF/estimator_interface.h | 8 +++++--- EKF/terrain_estimator.cpp | 13 ++++++++----- 4 files changed, 19 insertions(+), 13 deletions(-) diff --git a/EKF/ekf.h b/EKF/ekf.h index adb44eee9e..9358899694 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -154,9 +154,11 @@ public: // 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); + // return true if the terrain estimate is valid + bool get_terrain_valid(); + + // get the estimated terrain vertical position relative to the NED origin + void get_terrain_vert_pos(float *ret); // get the accerometer bias in m/s/s void get_accel_bias(float bias[3]); diff --git a/EKF/ekf_helper.cpp b/EKF/ekf_helper.cpp index 616ba63df9..ca40c0e3e9 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -1040,8 +1040,7 @@ void Ekf::get_ekf_soln_status(uint16_t *status) soln_status.flags.pos_horiz_rel = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow) && (_fault_status.value == 0); soln_status.flags.pos_horiz_abs = (_control_status.flags.gps || _control_status.flags.ev_pos) && (_fault_status.value == 0); soln_status.flags.pos_vert_abs = soln_status.flags.velocity_vert; - float dummy_var; - soln_status.flags.pos_vert_agl = get_terrain_vert_pos(&dummy_var); + soln_status.flags.pos_vert_agl = get_terrain_valid(); soln_status.flags.const_pos_mode = !soln_status.flags.velocity_horiz; soln_status.flags.pred_pos_horiz_rel = soln_status.flags.pos_horiz_rel; soln_status.flags.pred_pos_horiz_abs = soln_status.flags.pos_vert_abs; diff --git a/EKF/estimator_interface.h b/EKF/estimator_interface.h index bc9954ddda..cec722fb2a 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -199,9 +199,11 @@ public: // 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; + // return true if the terrain estimate is valid + virtual bool get_terrain_valid() = 0; + + // get the estimated terrain vertical position relative to the NED origin + virtual void get_terrain_vert_pos(float *ret) = 0; // return true if the local position estimate is valid bool local_position_is_valid(); diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index a07301fe59..78136a1d2e 100644 --- a/EKF/terrain_estimator.cpp +++ b/EKF/terrain_estimator.cpp @@ -150,12 +150,9 @@ void Ekf::fuseHagl() } } -// return true if the estimate is fresh -// return the estimated vertical position of the terrain relative to the NED origin -bool Ekf::get_terrain_vert_pos(float *ret) +// return true if the terrain estimate is valid +bool Ekf::get_terrain_valid() { - memcpy(ret, &_terrain_vpos, sizeof(float)); - if (_terrain_initialised && _range_data_continuous && !_innov_check_fail_status.flags.reject_hagl) { return true; @@ -164,6 +161,12 @@ bool Ekf::get_terrain_vert_pos(float *ret) } } +// get the estimated vertical position of the terrain relative to the NED origin +void Ekf::get_terrain_vert_pos(float *ret) +{ + memcpy(ret, &_terrain_vpos, sizeof(float)); +} + void Ekf::get_hagl_innov(float *hagl_innov) { memcpy(hagl_innov, &_hagl_innov, sizeof(_hagl_innov));