From e429ecab174fdab785e81e0f7f96bd0dcd494133 Mon Sep 17 00:00:00 2001 From: ChristophTobler Date: Mon, 17 Jul 2017 16:43:23 +0200 Subject: [PATCH] check if terrain valid instead of initialized --- EKF/control.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 4b0d94f438..825f97a809 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 (_terrain_initialised) { + if (get_terrain_valid()) { _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 && _terrain_initialised) { + if (_control_status.flags.in_air && get_terrain_valid()) { _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 (_terrain_initialised) { + if (get_terrain_valid()) { _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) && _terrain_initialised; + use_range_finder = (_terrain_vpos - _state.pos(2) < _params.max_hagl_for_range_aid) && get_terrain_valid(); } 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) && _terrain_initialised; + use_range_finder = (_terrain_vpos - _state.pos(2) < 0.7f * _params.max_hagl_for_range_aid) && get_terrain_valid(); } bool horz_vel_valid = (_control_status.flags.gps || _control_status.flags.ev_pos || _control_status.flags.opt_flow)