From 96f043cd6247ea8ab3531c99b9399cf5b17e4964 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Mon, 1 Jun 2020 10:14:21 -0400
Subject: [PATCH] sensors/vehicle_acceleration: limit scheduling to
 IMU_INTEG_RATE

---
 .../VehicleAcceleration.cpp                    | 18 +++++++++++++++++-
 .../VehicleAcceleration.hpp                    |  6 ++++--
 2 files changed, 21 insertions(+), 3 deletions(-)

diff --git a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp
index 82e31e3044..bc2a06404e 100644
--- a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp
+++ b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp
@@ -100,6 +100,22 @@ void VehicleAcceleration::CheckFilters()
 			if ((fabsf(_update_rate_hz - _filter_sample_rate) / _filter_sample_rate) > 0.01f) {
 				reset_filters = true;
 			}
+
+			if (reset_filters || (_required_sample_updates == 0)) {
+				if (_param_imu_integ_rate.get() > 0) {
+					// determine number of sensor samples that will get closest to the desired rate
+					const float configured_interval_us = 1e6f / _param_imu_integ_rate.get();
+					const uint8_t samples = math::constrain(roundf(configured_interval_us / sample_interval_avg), 1.f,
+										(float)sensor_accel_s::ORB_QUEUE_LENGTH);
+
+					_sensor_sub[_selected_sensor_sub_index].set_required_updates(samples);
+					_required_sample_updates = samples;
+
+				} else {
+					_sensor_sub[_selected_sensor_sub_index].set_required_updates(1);
+					_required_sample_updates = 1;
+				}
+			}
 		}
 
 		if (!reset_filters) {
@@ -170,6 +186,7 @@ bool VehicleAcceleration::SensorSelectionUpdate(bool force)
 
 						// reset sample interval accumulator on sensor change
 						_timestamp_sample_last = 0;
+						_required_sample_updates = 0;
 
 						return true;
 					}
@@ -256,7 +273,6 @@ void VehicleAcceleration::Run()
 				v_acceleration.timestamp = hrt_absolute_time();
 				_vehicle_acceleration_pub.publish(v_acceleration);
 
-				_last_publish = v_acceleration.timestamp_sample;
 				return;
 			}
 		}
diff --git a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp
index 0fc52419b4..66fe52b9c8 100644
--- a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp
+++ b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp
@@ -94,10 +94,11 @@ private:
 
 	matrix::Vector3f _acceleration_prev{0.f, 0.f, 0.f};
 
-	hrt_abstime _last_publish{0};
 	static constexpr const float kInitialRateHz{1000.0f}; /**< sensor update rate used for initialization */
 	float _update_rate_hz{kInitialRateHz}; /**< current rate-controller loop update rate in [Hz] */
 
+	uint8_t _required_sample_updates{0}; /**< number or sensor publications required for configured rate */
+
 	math::LowPassFilter2pVector3f _lp_filter{kInitialRateHz, 30.0f};
 
 	float _filter_sample_rate{kInitialRateHz};
@@ -110,7 +111,8 @@ private:
 	float _interval_count{0.f};
 
 	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::IMU_ACCEL_CUTOFF>) _param_imu_accel_cutoff
+		(ParamFloat<px4::params::IMU_ACCEL_CUTOFF>) _param_imu_accel_cutoff,
+		(ParamInt<px4::params::IMU_INTEG_RATE>) _param_imu_integ_rate
 	)
 };