diff --git a/src/modules/ekf2/EKF/ekf_helper.cpp b/src/modules/ekf2/EKF/ekf_helper.cpp index ed254ae0ca..4a0dc6aa1f 100644 --- a/src/modules/ekf2/EKF/ekf_helper.cpp +++ b/src/modules/ekf2/EKF/ekf_helper.cpp @@ -760,9 +760,7 @@ void Ekf::get_ekf_soln_status(uint16_t *status) const 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; -#if defined(CONFIG_EKF2_RANGE_FINDER) soln_status.flags.pos_vert_agl = isTerrainEstimateValid(); -#endif // CONFIG_EKF2_RANGE_FINDER 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_horiz_abs; @@ -1026,15 +1024,12 @@ void Ekf::initialiseQuatCovariances(Vector3f &rot_vec_var) void Ekf::updateGroundEffect() { if (_control_status.flags.in_air && !_control_status.flags.fixed_wing) { -#if defined(CONFIG_EKF2_RANGE_FINDER) if (isTerrainEstimateValid()) { // automatically set ground effect if terrain is valid float height = _terrain_vpos - _state.pos(2); _control_status.flags.gnd_effect = (height < _params.gnd_effect_max_hgt); - } else -#endif // CONFIG_EKF2_RANGE_FINDER - if (_control_status.flags.gnd_effect) { + } else if (_control_status.flags.gnd_effect) { // Turn off ground effect compensation if it times out if (isTimedOut(_time_last_gnd_effect_on, GNDEFFECT_TIMEOUT)) { _control_status.flags.gnd_effect = false; diff --git a/src/modules/ekf2/EKF/terrain_estimator.cpp b/src/modules/ekf2/EKF/terrain_estimator.cpp index a44e6cf3a3..657b989f48 100644 --- a/src/modules/ekf2/EKF/terrain_estimator.cpp +++ b/src/modules/ekf2/EKF/terrain_estimator.cpp @@ -415,10 +415,12 @@ void Ekf::controlHaglFakeFusion() bool Ekf::isTerrainEstimateValid() const { +#if defined(CONFIG_EKF2_RANGE_FINDER) // we have been fusing range finder measurements in the last 5 seconds if (_hagl_sensor_status.flags.range_finder && isRecent(_time_last_hagl_fuse, (uint64_t)5e6)) { return true; } +#endif // CONFIG_EKF2_RANGE_FINDER #if defined(CONFIG_EKF2_OPTICAL_FLOW) // we have been fusing optical flow measurements for terrain estimation within the last 5 seconds diff --git a/src/modules/ekf2/EKF2.cpp b/src/modules/ekf2/EKF2.cpp index edd5fe06c6..932b3fc82b 100644 --- a/src/modules/ekf2/EKF2.cpp +++ b/src/modules/ekf2/EKF2.cpp @@ -1462,13 +1462,10 @@ void EKF2::PublishLocalPosition(const hrt_abstime ×tamp) lpos.delta_heading = Eulerf(delta_q_reset).psi(); lpos.heading_good_for_control = _ekf.isYawFinalAlignComplete(); -#if defined(CONFIG_EKF2_RANGE_FINDER) - // Distance to bottom surface (ground) in meters - // constrain the distance to ground to _rng_gnd_clearance - lpos.dist_bottom = math::max(_ekf.getTerrainVertPos() - lpos.z, _param_ekf2_min_rng.get()); + // Distance to bottom surface (ground) in meters, must be positive + lpos.dist_bottom = math::max(_ekf.getTerrainVertPos() - lpos.z, 0.f); lpos.dist_bottom_valid = _ekf.isTerrainEstimateValid(); lpos.dist_bottom_sensor_bitfield = _ekf.getTerrainEstimateSensorBitfield(); -#endif // CONFIG_EKF2_RANGE_FINDER _ekf.get_ekf_lpos_accuracy(&lpos.eph, &lpos.epv); _ekf.get_ekf_vel_accuracy(&lpos.evh, &lpos.evv);