ekf: use uint64_t for time variables

src/modules/ekf2/EKF/control.cpp

@@ -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);
 		}
 	}
 

src/modules/ekf2/EKF/range_finder_consistency_check.cpp

@@ -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;
 		}
 	}

src/modules/ekf2/EKF/range_finder_consistency_check.hpp

@@ -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;
 };