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