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

always update lpf

Browse Source
This commit is contained in:
Marco Hauswirth 2024-12-04 10:53:26 +01:00
parent 8c13115e27
commit dade5c3a5c
3 changed files with 11 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.cpp
View File

@ -39,7 +39,7 @@
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();
_A.setIdentity();
@ -54,7 +54,7 @@ void RangeFinderConsistencyCheck::initMiniKF(float var_z, float var_terrain, flo
_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)
{
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;
}
} 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) {
_last_posD_reset_count = current_posD_reset_count;
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);
}

6
src/modules/ekf2/EKF/aid_sources/range_finder/range_finder_consistency_check.hpp
View File

@ -62,10 +62,10 @@ public:
bool isKinematicallyConsistent() const { return _state == KinematicState::CONSISTENT; }
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);
void initMiniKF(float var_z, float var_terrain, float z, float dist_bottom);
void stopMiniKF()
void init(float var_z, float var_terrain, float z, float dist_bottom);
void stop()
{
if (_initialized) {
if (_state == KinematicState::CONSISTENT) {

4
src/modules/ekf2/EKF/aid_sources/range_finder/range_height_control.cpp
View File

@ -80,7 +80,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
if (horizontal_motion) {
if (_rng_consistency_check.isRunning()) {
_rng_consistency_check.stopMiniKF();
_rng_consistency_check.stop();
}
} else if (vertical_motion) {
@ -103,7 +103,7 @@ void Ekf::controlRangeHaglFusion(const imuSample &imu_sample)
_range_sensor.setValidity(true); // bypass the checks
} else {
_rng_consistency_check.stopMiniKF();
_rng_consistency_check.stop();
}
}
}