dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

ekf: use uint64_t for time variables

This commit is contained in:
bresch 2022-03-18 11:38:01 +01:00 committed by Mathieu Bresciani
parent 9fc331b7ea
commit baf9cc9597
3 changed files with 15 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/modules/ekf2/EKF/control.cpp
View File

@ -140,7 +140,7 @@ void Ekf::controlFusionModes()
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());
_rng_consistency_check.update(_range_sensor.getDistBottom(), getRngHeightVariance(), _state.vel(2), P(6, 6), static_cast<float>(_time_last_imu) * 1e-6f);
_rng_consistency_check.update(_range_sensor.getDistBottom(), getRngHeightVariance(), _state.vel(2), P(6, 6), _time_last_imu);
}
}

18
src/modules/ekf2/EKF/range_finder_consistency_check.cpp
View File

@ -37,13 +37,13 @@
#include "range_finder_consistency_check.hpp"
void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, float time_s)
void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, uint64_t time_us)
{
const float dt = time_s - _time_last_update_s;
const float dt = static_cast<float>(time_us - _time_last_update_us) * 1e-6f;
if ((_time_last_update_s < FLT_EPSILON)
if ((_time_last_update_us == 0)
|| (dt < 0.001f) || (dt > 0.5f)) {
_time_last_update_s = time_s;
_time_last_update_us = time_us;
_dist_bottom_prev = dist_bottom;
return;
}
@ -59,20 +59,20 @@ void RangeFinderConsistencyCheck::update(float dist_bottom, float dist_bottom_va
const float signed_test_ratio = matrix::sign(innov) * _vel_bottom_test_ratio;
_vel_bottom_signed_test_ratio_lpf.update(signed_test_ratio);
updateConsistency(vz, time_s);
updateConsistency(vz, time_us);
_time_last_update_s = time_s;
_time_last_update_us = time_us;
_dist_bottom_prev = dist_bottom;
}
void RangeFinderConsistencyCheck::updateConsistency(float vz, float time_s)
void RangeFinderConsistencyCheck::updateConsistency(float vz, uint64_t time_us)
{
if (fabsf(_vel_bottom_signed_test_ratio_lpf.getState()) >= 1.f) {
_is_kinematically_consistent = false;
_time_last_inconsistent = time_s;
_time_last_inconsistent_us = time_us;
} else {
if (fabsf(vz) > _min_vz_for_valid_consistency && _vel_bottom_test_ratio < 1.f && ((time_s - _time_last_inconsistent) > _consistency_hyst_time)) {
if (fabsf(vz) > _min_vz_for_valid_consistency && _vel_bottom_test_ratio < 1.f && ((time_us - _time_last_inconsistent_us) > _consistency_hyst_time_us)) {
_is_kinematically_consistent = true;
}
}

10
src/modules/ekf2/EKF/range_finder_consistency_check.hpp
View File

@ -47,27 +47,27 @@ public:
RangeFinderConsistencyCheck() = default;
~RangeFinderConsistencyCheck() = default;
void update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, float time_s);
void update(float dist_bottom, float dist_bottom_var, float vz, float vz_var, uint64_t time_us);
float getTestRatio() const { return _vel_bottom_test_ratio; }
float getSignedTestRatioLpf() const { return _vel_bottom_signed_test_ratio_lpf.getState(); }
bool isKinematicallyConsistent() const { return _is_kinematically_consistent; }
private:
void updateConsistency(float vz, float time_s);
void updateConsistency(float vz, uint64_t time_us);
float _time_last_update_s{};
uint64_t _time_last_update_us{};
float _dist_bottom_prev{};
float _vel_bottom_test_ratio{};
AlphaFilter<float> _vel_bottom_signed_test_ratio_lpf{}; // average signed test ratio used to detect a bias in the data
bool _is_kinematically_consistent{true};
float _time_last_inconsistent{};
uint64_t _time_last_inconsistent_us{};
static constexpr float _vel_bottom_signed_test_ratio_tau = 2.f;
static constexpr float _vel_bottom_gate = 0.1f;
static constexpr float _min_vz_for_valid_consistency = 0.5f;
static constexpr float _consistency_hyst_time = 1.f;
static constexpr uint64_t _consistency_hyst_time_us = 1e6;
};