From 39a33a35ae0f02a58b45e33de5cc8cb844b52755 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Sat, 6 Jun 2020 11:46:01 -0400
Subject: [PATCH] sensors/vehicle_imu: pause computing publication update rate

---
 .../sensors/vehicle_imu/VehicleIMU.cpp        | 27 +++++++++++++++++--
 .../sensors/vehicle_imu/VehicleIMU.hpp        |  1 +
 2 files changed, 26 insertions(+), 2 deletions(-)

diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp
index cffbbb3333..997b755a56 100644
--- a/src/modules/sensors/vehicle_imu/VehicleIMU.cpp
+++ b/src/modules/sensors/vehicle_imu/VehicleIMU.cpp
@@ -103,6 +103,8 @@ void VehicleIMU::ParametersUpdate(bool force)
 		parameter_update_s param_update;
 		_params_sub.copy(&param_update);
 
+		const auto imu_integ_rate_prev = _param_imu_integ_rate.get();
+
 		updateParams();
 
 		_accel_corrections.ParametersUpdate();
@@ -115,6 +117,13 @@ void VehicleIMU::ParametersUpdate(bool force)
 			_param_imu_integ_rate.set(imu_integration_rate_hz);
 			_param_imu_integ_rate.commit_no_notification();
 		}
+
+		if (_param_imu_integ_rate.get() != imu_integ_rate_prev) {
+			// force update
+			_intervals_update = true;
+			_accel_interval.timestamp_sample_last = 0;
+			_gyro_interval.timestamp_sample_last = 0;
+		}
 	}
 }
 
@@ -144,6 +153,11 @@ bool VehicleIMU::UpdateIntervalAverage(IntervalAverage &intavg, const hrt_abstim
 			intavg.interval_sum = 0.f;
 			intavg.interval_count = 0.f;
 		}
+
+	} else {
+		// reset
+		intavg.interval_sum = 0.f;
+		intavg.interval_count = 0.f;
 	}
 
 	intavg.timestamp_sample_last = timestamp_sample;
@@ -169,6 +183,10 @@ void VehicleIMU::Run()
 
 		if (_sensor_gyro_sub.get_last_generation() != _gyro_last_generation + 1) {
 			perf_count(_gyro_generation_gap_perf);
+
+			// if there's a gap in data start monitoring publication interval again
+			_intervals_update = true;
+			_gyro_interval.timestamp_sample_last = 0;
 		}
 
 		_gyro_last_generation = _sensor_gyro_sub.get_last_generation();
@@ -184,7 +202,7 @@ void VehicleIMU::Run()
 		_last_timestamp_sample_gyro = gyro.timestamp_sample;
 
 		// collect sample interval average for filters
-		if (UpdateIntervalAverage(_gyro_interval, gyro.timestamp_sample)) {
+		if (_intervals_update && UpdateIntervalAverage(_gyro_interval, gyro.timestamp_sample)) {
 			update_integrator_config = true;
 			publish_status = true;
 			_status.gyro_rate_hz = roundf(1e6f / _gyro_interval.update_interval);
@@ -203,6 +221,10 @@ void VehicleIMU::Run()
 
 		if (_sensor_accel_sub.get_last_generation() != _accel_last_generation + 1) {
 			perf_count(_accel_generation_gap_perf);
+
+			// if there's a gap in data start monitoring publication interval again
+			_intervals_update = true;
+			_accel_interval.timestamp_sample_last = 0;
 		}
 
 		_accel_last_generation = _sensor_accel_sub.get_last_generation();
@@ -218,7 +240,7 @@ void VehicleIMU::Run()
 		_last_timestamp_sample_accel = accel.timestamp_sample;
 
 		// collect sample interval average for filters
-		if (UpdateIntervalAverage(_accel_interval, accel.timestamp_sample)) {
+		if (_intervals_update && UpdateIntervalAverage(_accel_interval, accel.timestamp_sample)) {
 			update_integrator_config = true;
 			publish_status = true;
 			_status.accel_rate_hz = roundf(1e6f / _accel_interval.update_interval);
@@ -351,6 +373,7 @@ void VehicleIMU::UpdateIntegratorConfiguration()
 		_sensor_accel_sub.unregisterCallback();
 
 		_intervals_configured = true;
+		_intervals_update = false; // stop monitoring topic publication rate
 
 		PX4_DEBUG("accel (%d), gyro (%d), accel samples: %d, gyro samples: %d, accel interval: %.1f, gyro interval: %.1f",
 			  _accel_corrections.get_device_id(), _gyro_corrections.get_device_id(), accel_integral_samples, gyro_integral_samples,
diff --git a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp
index f37f0d62d7..ecf1790633 100644
--- a/src/modules/sensors/vehicle_imu/VehicleIMU.hpp
+++ b/src/modules/sensors/vehicle_imu/VehicleIMU.hpp
@@ -113,6 +113,7 @@ private:
 
 	uint8_t _delta_velocity_clipping{0};
 
+	bool _intervals_update{true};
 	bool _intervals_configured{false};
 
 	perf_counter_t _accel_update_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": accel update interval")};