fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-05 07:41:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

always update lpf

Browse Source
This commit is contained in:
Marco Hauswirth
2025-08-26 15:08:09 +02:00
parent 8c13115e27
commit dade5c3a5c
3 changed files with 11 additions and 9 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
+6 -4
View File
@@ -39,7 +39,7 @@
using namespace matrix; using namespace matrix;
void RangeFinderConsistencyCheck::initMiniKF(float var_z, float var_terrain, float z, float dist_bottom) void RangeFinderConsistencyCheck::init(float var_z, float var_terrain, float z, float dist_bottom)
{ {
_R.setZero(); _R.setZero();
_A.setIdentity(); _A.setIdentity();
@@ -54,7 +54,7 @@ void RangeFinderConsistencyCheck::initMiniKF(float var_z, float var_terrain, flo
_state = KinematicState::UNKNOWN; _state = KinematicState::UNKNOWN;
} }
void RangeFinderConsistencyCheck::UpdateMiniKF(float z, float z_var, float vz, float vz_var, float dist_bottom, void RangeFinderConsistencyCheck::update(float z, float z_var, float vz, float vz_var, float dist_bottom,
float dist_bottom_var, uint64_t time_us) float dist_bottom_var, uint64_t time_us)
{ {
const float dt = static_cast<float>(time_us - _time_last_update_us) * 1e-6f; const float dt = static_cast<float>(time_us - _time_last_update_us) * 1e-6f;
@@ -112,6 +112,8 @@ void RangeFinderConsistencyCheck::UpdateMiniKF(float z, float z_var, float vz, f
_state = KinematicState::INCONSISTENT; _state = KinematicState::INCONSISTENT;
} }
} else {
_test_ratio_lpf.update(test_ratio);
} }
} }
@@ -125,8 +127,8 @@ void RangeFinderConsistencyCheck::run(const float z, const float vz,
if (!_initialized || current_posD_reset_count != _last_posD_reset_count) { if (!_initialized || current_posD_reset_count != _last_posD_reset_count) {
_last_posD_reset_count = current_posD_reset_count; _last_posD_reset_count = current_posD_reset_count;
const float terrain_var = P(estimator::State::terrain.idx, estimator::State::terrain.idx); const float terrain_var = P(estimator::State::terrain.idx, estimator::State::terrain.idx);
initMiniKF(z_var, terrain_var, z, dist_bottom); init(z_var, terrain_var, z, dist_bottom);
} }
UpdateMiniKF(z, z_var, vz, vz_var, dist_bottom, dist_bottom_var, time_us); update(z, z_var, vz, vz_var, dist_bottom, dist_bottom_var, time_us);
} }
src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp
+3 -3
View File
@@ -62,10 +62,10 @@ public:
bool isKinematicallyConsistent() const { return _state == KinematicState::CONSISTENT; } bool isKinematicallyConsistent() const { return _state == KinematicState::CONSISTENT; }
bool isNotKinematicallyInconsistent() const { return _state != KinematicState::INCONSISTENT || _fixed_wing; } bool isNotKinematicallyInconsistent() const { return _state != KinematicState::INCONSISTENT || _fixed_wing; }
void UpdateMiniKF(float z, float z_var, float vz, float vz_var, float dist_bottom, float dist_bottom_var, void update(float z, float z_var, float vz, float vz_var, float dist_bottom, float dist_bottom_var,
uint64_t time_us); uint64_t time_us);
void initMiniKF(float var_z, float var_terrain, float z, float dist_bottom); void init(float var_z, float var_terrain, float z, float dist_bottom);
void stopMiniKF() void stop()
{ {
if (_initialized) { if (_initialized) {
if (_state == KinematicState::CONSISTENT) { if (_state == KinematicState::CONSISTENT) {
src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp
+2 -2
View File
@@ -80,7 +80,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
if (horizontal_motion) { if (horizontal_motion) {
if (_rng_consistency_check.isRunning()) { if (_rng_consistency_check.isRunning()) {
_rng_consistency_check.stopMiniKF(); _rng_consistency_check.stop();
} }
} else if (vertical_motion) { } else if (vertical_motion) {
@@ -103,7 +103,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
_range_sensor.setValidity(true); // bypass the checks _range_sensor.setValidity(true); // bypass the checks
} else { } else {
_rng_consistency_check.stopMiniKF(); _rng_consistency_check.stop();
} }
} }
} }