diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp index d6fa31dcb8..01d1c48a27 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp @@ -39,11 +39,16 @@ #include "ekf_derivation/generated/range_validation_filter_h.h" #include "ekf_derivation/generated/range_validation_filter_P_init.h" +#include + + using namespace matrix; void RangeFinderConsistencyCheck::init(const float z, const float z_var, const float dist_bottom, const float dist_bottom_var) { + PX4_INFO("RangeFinderConsistencyCheck::init"); + _P = sym::RangeValidationFilterPInit(z_var, dist_bottom_var); _Ht = sym::RangeValidationFilterH(); @@ -89,7 +94,6 @@ void RangeFinderConsistencyCheck::update(const float z, const float z_var, const H(RangeFilter::z.idx) = 1.f; H(RangeFilter::terrain.idx) = 0.f; R = z_var; - } // residual @@ -130,25 +134,19 @@ void RangeFinderConsistencyCheck::update(const float z, const float z_var, const void RangeFinderConsistencyCheck::evaluateState(const float dt, const float vz, const float vz_var) { // start the consistency check after 1s - if (_t_since_first_sample > _t_to_init) { - if (fabsf(_test_ratio_lpf.getState()) < 1.f) { - const bool vertical_motion = sq(vz) > fmaxf(vz_var, 0.1f); - - if (!horizontal_motion && vertical_motion) { - _state = KinematicState::CONSISTENT; - - } else if (_state == KinematicState::CONSISTENT || vertical_motion) { - _state = KinematicState::UNKNOWN; - } - - } else { - _t_since_first_sample = 0.f; - _state = KinematicState::INCONSISTENT; - } - - } else { + if (_t_since_first_sample < _t_to_init) { _t_since_first_sample += dt; + return; } + + if (fabsf(_test_ratio_lpf.getState()) > 1.f) { + PX4_INFO("_test_ratio_lpf failed (>1)"); + _t_since_first_sample = 0.f; + _state = KinematicState::INCONSISTENT; + return; + } + + _state = KinematicState::CONSISTENT; } void RangeFinderConsistencyCheck::run(const float z, const float z_var, const float vz, const float vz_var, diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp index 0fb9e11a5e..6ecc56c29a 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp @@ -73,7 +73,6 @@ public: } uint8_t current_posD_reset_count{0}; - bool horizontal_motion{false}; private: diff --git a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp index 7507abd93e..7e378ce375 100644 --- a/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp +++ b/src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp @@ -44,64 +44,56 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) { static constexpr const char *HGT_SRC_NAME = "RNG"; - bool rng_data_ready = false; - - if (_range_buffer) { - // Get range data from buffer and check validity - rng_data_ready = _range_buffer->pop_first_older_than(imu_sample.time_us, _range_sensor.getSampleAddress()); - _range_sensor.setDataReadiness(rng_data_ready); - - if (_range_sensor.isDataReady()) { - - _range_sensor.setPitchOffset(_params.rng_sens_pitch); - _range_sensor.setCosMaxTilt(_params.range_cos_max_tilt); - _range_sensor.setQualityHysteresis(_params.range_valid_quality_s); - _range_sensor.setMaxFogDistance(_params.rng_fog); - _rng_consistency_check.setGate(_params.range_kin_consistency_gate); - - _range_sensor.runChecks(imu_sample.time_us, _R_to_earth); - - if (_range_sensor.isDataHealthy()) { - // correct the range data for position offset relative to the IMU - const Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body; - const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; - _range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt()); - - // TODO: this is a constant - const float dist_var = getRngVar(); - _rng_consistency_check.current_posD_reset_count = get_posD_reset_count(); - - const bool updated_horizontal_motion = sq(_state.vel(0)) + sq(_state.vel(1)) > fmaxf(P.trace<2>(State::vel.idx), 0.1f); - - if (!updated_horizontal_motion && _rng_consistency_check.horizontal_motion) { - _rng_consistency_check.reset(); - } - - _rng_consistency_check.horizontal_motion = updated_horizontal_motion; - const float z_var = P(State::pos.idx + 2, State::pos.idx + 2); - const float vz_var = P(State::vel.idx + 2, State::vel.idx + 2); - - // TODO: review -- variance - _rng_consistency_check.run(_gpos.altitude(), z_var, _state.vel(2), vz_var, _range_sensor.getDistBottom(), - dist_var, imu_sample.time_us); - - } else if (_range_sensor.isRegularlySendingData() && !_control_status.flags.in_air) { - _range_sensor.setRange(_params.rng_gnd_clearance); - _range_sensor.setValidity(true); - - } else { - _rng_consistency_check.reset(); - } - } - - _control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent(); - _control_status.flags.rng_kin_unknown = !_rng_consistency_check.isKinematicallyConsistent() - && _rng_consistency_check.isNotKinematicallyInconsistent(); - - } else { + if (!_range_buffer) { return; } + // Get range data from buffer and check validity + bool rng_data_ready = _range_buffer->pop_first_older_than(imu_sample.time_us, _range_sensor.getSampleAddress()); + _range_sensor.setDataReadiness(rng_data_ready); + + if (_range_sensor.isDataReady()) { + + _range_sensor.setPitchOffset(_params.rng_sens_pitch); + _range_sensor.setCosMaxTilt(_params.range_cos_max_tilt); + _range_sensor.setQualityHysteresis(_params.range_valid_quality_s); + _range_sensor.setMaxFogDistance(_params.rng_fog); + _rng_consistency_check.setGate(_params.range_kin_consistency_gate); + + _range_sensor.runChecks(imu_sample.time_us, _R_to_earth); + + if (_range_sensor.isDataHealthy()) { + // correct the range data for position offset relative to the IMU + const Vector3f pos_offset_body = _params.rng_pos_body - _params.imu_pos_body; + const Vector3f pos_offset_earth = _R_to_earth * pos_offset_body; + _range_sensor.setRange(_range_sensor.getRange() + pos_offset_earth(2) / _range_sensor.getCosTilt()); + + // TODO: this is a constant + const float dist_var = getRngVar(); + _rng_consistency_check.current_posD_reset_count = get_posD_reset_count(); + + const float z_var = P(State::pos.idx + 2, State::pos.idx + 2); + const float vz_var = P(State::vel.idx + 2, State::vel.idx + 2); + + // TODO: review -- variance + _rng_consistency_check.run(_gpos.altitude(), z_var, _state.vel(2), vz_var, _range_sensor.getDistBottom(), + dist_var, imu_sample.time_us); + + } else if (_range_sensor.isRegularlySendingData() && !_control_status.flags.in_air) { + _range_sensor.setRange(_params.rng_gnd_clearance); + _range_sensor.setValidity(true); + + } else { + _rng_consistency_check.reset(); + } + } + + _control_status.flags.rng_kin_consistent = _rng_consistency_check.isKinematicallyConsistent(); + + _control_status.flags.rng_kin_unknown = !_rng_consistency_check.isKinematicallyConsistent() + && _rng_consistency_check.isNotKinematicallyInconsistent(); + + auto &aid_src = _aid_src_rng_hgt; if (_range_sensor.isDataReady() && _range_sensor.getSampleAddress()) { @@ -116,6 +108,8 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) && _range_sensor.isDataHealthy() && _rng_consistency_check.isNotKinematicallyInconsistent(); + // SUS: _rng_consistency_check.isNotKinematicallyInconsistent() + const bool starting_conditions_passing = continuing_conditions_passing && isNewestSampleRecent(_time_last_range_buffer_push, 2 * estimator::sensor::RNG_MAX_INTERVAL) && _range_sensor.isRegularlySendingData(); @@ -124,12 +118,14 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) && (_params.rng_ctrl == static_cast(RngCtrl::CONDITIONAL)) && isConditionalRangeAidSuitable(); + // SUS: isConditionalRangeAidSuitable() + const bool do_range_aid = (_control_status.flags.rng_terrain || _control_status.flags.rng_hgt) && (_params.rng_ctrl == static_cast(RngCtrl::ENABLED)); if (_control_status.flags.rng_hgt) { if (!(do_conditional_range_aid || do_range_aid)) { - ECL_INFO("stopping %s fusion", HGT_SRC_NAME); + PX4_INFO("stopping %s fusion", HGT_SRC_NAME); stopRngHgtFusion(); } @@ -137,7 +133,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) if (_params.height_sensor_ref == static_cast(HeightSensor::RANGE)) { if (do_conditional_range_aid) { // Range finder is used while hovering to stabilize the height estimate. Don't reset but use it as height reference. - ECL_INFO("starting conditional %s height fusion", HGT_SRC_NAME); + PX4_INFO("starting conditional %s height fusion", HGT_SRC_NAME); _height_sensor_ref = HeightSensor::RANGE; _control_status.flags.rng_hgt = true; @@ -152,7 +148,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) } else if (do_range_aid) { // Range finder is the primary height source, the ground is now the datum used // to compute the local vertical position - ECL_INFO("starting %s height fusion, resetting height", HGT_SRC_NAME); + PX4_INFO("starting %s height fusion, resetting height", HGT_SRC_NAME); _height_sensor_ref = HeightSensor::RANGE; _information_events.flags.reset_hgt_to_rng = true; @@ -167,7 +163,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) } else { if (do_conditional_range_aid || do_range_aid) { - ECL_INFO("starting %s height fusion", HGT_SRC_NAME); + PX4_INFO("starting %s height fusion", HGT_SRC_NAME); _control_status.flags.rng_hgt = true; if (!_control_status.flags.opt_flow_terrain && aid_src.innovation_rejected @@ -188,7 +184,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) if (isHeightResetRequired() && _control_status.flags.rng_hgt && (_height_sensor_ref == HeightSensor::RANGE)) { // All height sources are failing - ECL_WARN("%s height fusion reset required, all height sources failing", HGT_SRC_NAME); + PX4_INFO("%s height fusion reset required, all height sources failing", HGT_SRC_NAME); _information_events.flags.reset_hgt_to_rng = true; resetAltitudeTo(aid_src.observation - _state.terrain); @@ -204,7 +200,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) } else if (is_fusion_failing) { // Some other height source is still working if (_control_status.flags.opt_flow_terrain && isTerrainEstimateValid()) { - ECL_WARN("stopping %s fusion, fusion failing", HGT_SRC_NAME); + PX4_INFO("stopping %s fusion, fusion failing", HGT_SRC_NAME); stopRngHgtFusion(); stopRngTerrFusion(); @@ -215,7 +211,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) } } else { - ECL_WARN("stopping %s fusion, continuing conditions failing", HGT_SRC_NAME); + PX4_INFO("stopping %s fusion, continuing conditions failing", HGT_SRC_NAME); stopRngHgtFusion(); stopRngTerrFusion(); } @@ -242,7 +238,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample) } else if ((_control_status.flags.rng_hgt || _control_status.flags.rng_terrain) && !isNewestSampleRecent(_time_last_range_buffer_push, 2 * estimator::sensor::RNG_MAX_INTERVAL)) { // No data anymore. Stop until it comes back. - ECL_WARN("stopping %s fusion, no data", HGT_SRC_NAME); + PX4_INFO("stopping %s fusion, no data", HGT_SRC_NAME); stopRngHgtFusion(); stopRngTerrFusion(); }