From d430e13401040464885795a9a986c1369c1d0eca Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Mon, 31 May 2021 13:20:39 -0400
Subject: [PATCH] sensors: wait for real sample rate to initialize
 LowPassFilter2p

---
 src/lib/mathlib/math/filter/LowPassFilter2p.hpp             | 2 ++
 .../sensors/vehicle_acceleration/VehicleAcceleration.hpp    | 3 +--
 .../vehicle_angular_velocity/VehicleAngularVelocity.hpp     | 6 ++----
 3 files changed, 5 insertions(+), 6 deletions(-)

diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.hpp b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp
index fd3a531e03..88b00b7ee8 100644
--- a/src/lib/mathlib/math/filter/LowPassFilter2p.hpp
+++ b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp
@@ -49,6 +49,8 @@ template<typename T>
 class LowPassFilter2p
 {
 public:
+	LowPassFilter2p() = default;
+
 	LowPassFilter2p(float sample_freq, float cutoff_freq)
 	{
 		// set initial parameters
diff --git a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp
index 3b3a5863dc..68c0652082 100644
--- a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp
+++ b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp
@@ -93,10 +93,9 @@ private:
 
 	matrix::Vector3f _acceleration_prev{};
 
-	static constexpr const float kInitialRateHz{1000.f}; /**< sensor update rate used for initialization */
 	float _filter_sample_rate{NAN};
 
-	math::LowPassFilter2p<matrix::Vector3f> _lp_filter{kInitialRateHz, 30.f};
+	math::LowPassFilter2p<matrix::Vector3f> _lp_filter{};
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::IMU_ACCEL_CUTOFF>) _param_imu_accel_cutoff,
diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp
index 023ad5877c..6b893a659d 100644
--- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp
+++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp
@@ -130,10 +130,8 @@ private:
 
 	float _filter_sample_rate_hz{NAN};
 
-	static constexpr const float kInitialRateHz{1000.f}; /**< sensor update rate used for initialization */
-
 	// angular velocity filters
-	math::LowPassFilter2p<float> _lp_filter_velocity[3] {{kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}};
+	math::LowPassFilter2p<float> _lp_filter_velocity[3] {};
 	math::NotchFilter<float> _notch_filter_velocity[3] {};
 
 #if !defined(CONSTRAINED_FLASH)
@@ -167,7 +165,7 @@ private:
 #endif // !CONSTRAINED_FLASH
 
 	// angular acceleration filter
-	math::LowPassFilter2p<float> _lp_filter_acceleration[3] {{kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}, {kInitialRateHz, 30.f}};
+	math::LowPassFilter2p<float> _lp_filter_acceleration[3] {};
 
 	uint32_t _selected_sensor_device_id{0};