fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-13 19:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

remove vertical and horizontal motion check for rangefinder consistency check

Browse Source
This commit is contained in:
Jacob Dahl
2025-06-17 15:26:20 -08:00
parent 594c65cccb
commit a0d17d1daa
3 changed files with 75 additions and 82 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp
+16 -18
View File
@@ -39,11 +39,16 @@
#include "ekf_derivation/generated/range_validation_filter_h.h"
#include "ekf_derivation/generated/range_validation_filter_P_init.h"
#include <px4_platform_common/log.h>
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<float>();
@@ -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,
src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp
-1
View File
@@ -73,7 +73,6 @@ public:
}
uint8_t current_posD_reset_count{0};
bool horizontal_motion{false};
private:
src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp
+59 -63
View File
@@ -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<int32_t>(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<int32_t>(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<int32_t>(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();
}