From b264577daaed7c28efc16475568378aef43114c0 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Mon, 24 May 2021 21:39:42 -0400
Subject: [PATCH] sensors/vehicle_acceleration: require valid sample rate
 before running

 - this fixes a minor edge case when you only have a single accelerometer and haven't made any parameter changes, the sensor sample rate (used for filtering) might not have been updated from default
---
 .../vehicle_acceleration/VehicleAcceleration.cpp      | 11 ++++++++++-
 .../vehicle_acceleration/VehicleAcceleration.hpp      |  2 +-
 2 files changed, 11 insertions(+), 2 deletions(-)

diff --git a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp
index d7a86c528b..b064627feb 100644
--- a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp
+++ b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp
@@ -94,7 +94,7 @@ void VehicleAcceleration::CheckAndUpdateFilters()
 		if ((imu_status.get().accel_device_id != 0) && (imu_status.get().accel_device_id == _calibration.device_id())
 		    && PX4_ISFINITE(sample_rate_hz) && (sample_rate_hz > 0)) {
 			// check if sample rate error is greater than 1%
-			if ((fabsf(sample_rate_hz - _filter_sample_rate) / _filter_sample_rate) > 0.01f) {
+			if (!PX4_ISFINITE(_filter_sample_rate) || (fabsf(sample_rate_hz - _filter_sample_rate) / _filter_sample_rate) > 0.01f) {
 				PX4_DEBUG("sample rate changed: %.3f Hz -> %.3f Hz", (double)_filter_sample_rate, (double)sample_rate_hz);
 				_filter_sample_rate = sample_rate_hz;
 				sample_rate_changed = true;
@@ -214,6 +214,15 @@ void VehicleAcceleration::Run()
 	SensorBiasUpdate(selection_updated);
 	ParametersUpdate();
 
+	// require valid sensor sample rate to run
+	if (!PX4_ISFINITE(_filter_sample_rate)) {
+		CheckAndUpdateFilters();
+
+		if (!PX4_ISFINITE(_filter_sample_rate)) {
+			return;
+		}
+	}
+
 	// process all outstanding messages
 	sensor_accel_s sensor_data;
 
diff --git a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp
index 13d2d67865..b47ec476c7 100644
--- a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp
+++ b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp
@@ -94,7 +94,7 @@ private:
 	matrix::Vector3f _acceleration_prev{};
 
 	static constexpr const float kInitialRateHz{1000.f}; /**< sensor update rate used for initialization */
-	float _filter_sample_rate{kInitialRateHz};
+	float _filter_sample_rate{NAN};
 
 	math::LowPassFilter2pVector3f _lp_filter{kInitialRateHz, 30.f};