From c887b02f21c20df2b9f774d0297d2f4c40e07624 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Wed, 19 Jul 2017 17:33:48 +1000 Subject: [PATCH] Revert "Split get_terrain_vert_pos() into valid and get_vpos" --- EKF/control.cpp | 10 +++++----- EKF/ekf.h | 8 +++----- EKF/ekf_helper.cpp | 3 ++- EKF/estimator_interface.h | 8 +++----- EKF/terrain_estimator.cpp | 13 +++++-------- 5 files changed, 18 insertions(+), 24 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 825f97a809..4b0d94f438 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -734,7 +734,7 @@ void Ekf::controlHeightFusion() // we have just switched to using range finder, calculate height sensor offset such that current // measurment matches our current height estimate if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) { - if (get_terrain_valid()) { + if (_terrain_initialised) { _hgt_sensor_offset = _terrain_vpos; } else { _hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2); @@ -774,7 +774,7 @@ void Ekf::controlHeightFusion() // measurment matches our current height estimate if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) { // use the parameter rng_gnd_clearance if on ground to avoid a noisy offset initialization (e.g. sonar) - if (_control_status.flags.in_air && get_terrain_valid()) { + if (_control_status.flags.in_air && _terrain_initialised) { _hgt_sensor_offset = _terrain_vpos; } else if (_control_status.flags.in_air) { @@ -807,7 +807,7 @@ void Ekf::controlHeightFusion() // we have just switched to using range finder, calculate height sensor offset such that current // measurment matches our current height estimate if (_control_status_prev.flags.rng_hgt != _control_status.flags.rng_hgt) { - if (get_terrain_valid()) { + if (_terrain_initialised) { _hgt_sensor_offset = _terrain_vpos; } else { _hgt_sensor_offset = _R_rng_to_earth_2_2 * _range_sample_delayed.rng + _state.pos(2); @@ -875,12 +875,12 @@ bool Ekf::rangeAidConditionsMet(bool in_range_aid_mode) // check if we should use range finder measurements to estimate height, use hysteresis to avoid rapid switching bool use_range_finder; if (in_range_aid_mode) { - use_range_finder = (_terrain_vpos - _state.pos(2) < _params.max_hagl_for_range_aid) && get_terrain_valid(); + use_range_finder = (_terrain_vpos - _state.pos(2) < _params.max_hagl_for_range_aid) && _terrain_initialised; } else { // if we were not using range aid in the previous iteration then require the current height above terrain to be // smaller than 70 % of the maximum allowed ground distance for range aid - use_range_finder = (_terrain_vpos - _state.pos(2) < 0.7f * _params.max_hagl_for_range_aid) && get_terrain_valid(); + use_range_finder = (_terrain_vpos - _state.pos(2) < 0.7f * _params.max_hagl_for_range_aid) && _terrain_initialised; } bool horz_vel_valid = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow) diff --git a/EKF/ekf.h b/EKF/ekf.h index 9358899694..adb44eee9e 100644 --- a/EKF/ekf.h +++ b/EKF/ekf.h @@ -154,11 +154,9 @@ public: // return true if the EKF is dead reckoning the position using inertial data only bool inertial_dead_reckoning(); - // 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); + // 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); // 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 ca40c0e3e9..616ba63df9 100644 --- a/EKF/ekf_helper.cpp +++ b/EKF/ekf_helper.cpp @@ -1040,7 +1040,8 @@ 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; - soln_status.flags.pos_vert_agl = get_terrain_valid(); + float dummy_var; + soln_status.flags.pos_vert_agl = get_terrain_vert_pos(&dummy_var); 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 cec722fb2a..bc9954ddda 100644 --- a/EKF/estimator_interface.h +++ b/EKF/estimator_interface.h @@ -199,11 +199,9 @@ 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 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 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 local position estimate is valid bool local_position_is_valid(); diff --git a/EKF/terrain_estimator.cpp b/EKF/terrain_estimator.cpp index 78136a1d2e..a07301fe59 100644 --- a/EKF/terrain_estimator.cpp +++ b/EKF/terrain_estimator.cpp @@ -150,9 +150,12 @@ void Ekf::fuseHagl() } } -// return true if the terrain estimate is valid -bool Ekf::get_terrain_valid() +// 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) { + memcpy(ret, &_terrain_vpos, sizeof(float)); + if (_terrain_initialised && _range_data_continuous && !_innov_check_fail_status.flags.reject_hagl) { return true; @@ -161,12 +164,6 @@ bool Ekf::get_terrain_valid() } } -// 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));