sensors/vehicle_imu: improve initial sensor interval monitoring

- add perf counters for monitoring

src/modules/sensors/sensors.cpp

@@ -636,6 +636,7 @@ int Sensors::print_status()
 
 	for (auto &i : _vehicle_imu_list) {
 		if (i != nullptr) {
+			PX4_INFO_RAW("\n");
 			i->PrintStatus();
 		}
 	}

src/modules/sensors/vehicle_imu/VehicleIMU.cpp

@@ -71,6 +71,11 @@ VehicleIMU::VehicleIMU(uint8_t accel_index, uint8_t gyro_index) :
 VehicleIMU::~VehicleIMU()
 {
 	Stop();
+
+	perf_free(_accel_generation_gap_perf);
+	perf_free(_accel_update_perf);
+	perf_free(_gyro_generation_gap_perf);
+	perf_free(_gyro_update_perf);
 }
 
 bool VehicleIMU::Start()
@@ -78,7 +83,7 @@ bool VehicleIMU::Start()
 	// force initial updates
 	ParametersUpdate(true);
 
-	return _sensor_gyro_sub.registerCallback();
+	return _sensor_gyro_sub.registerCallback() && _sensor_accel_sub.registerCallback();
 }
 
 void VehicleIMU::Stop()
@@ -121,31 +126,28 @@ bool VehicleIMU::UpdateIntervalAverage(IntervalAverage &intavg, const hrt_abstim
 		intavg.interval_sum += (timestamp_sample - intavg.timestamp_sample_last);
 		intavg.interval_count++;
 
-	} else {
+		// periodically calculate sensor update rate
-		intavg.interval_sum = 0.f;
+		if (intavg.interval_count > 10000 || ((intavg.update_interval <= FLT_EPSILON) && intavg.interval_count > 100)) {
-		intavg.interval_count = 0.f;
+
+			const float sample_interval_avg = intavg.interval_sum / intavg.interval_count;
+
+			if (PX4_ISFINITE(sample_interval_avg) && (sample_interval_avg > 0.f)) {
+				// update if interval has changed by more than 0.5%
+				if ((fabsf(intavg.update_interval - sample_interval_avg) / intavg.update_interval) > 0.005f) {
+
+					intavg.update_interval = sample_interval_avg;
+					updated = true;
+				}
+			}
+
+			// reset sample interval accumulator
+			intavg.interval_sum = 0.f;
+			intavg.interval_count = 0.f;
+		}
 	}
 
 	intavg.timestamp_sample_last = timestamp_sample;
 
-	// periodically calculate sensor update rate
-	if (intavg.interval_count > 10000 || ((intavg.update_interval <= FLT_EPSILON) && intavg.interval_count > 100)) {
-
-		const float sample_interval_avg = intavg.interval_sum / intavg.interval_count;
-
-		if (PX4_ISFINITE(sample_interval_avg) && (sample_interval_avg > 0.f)) {
-			// update if interval has changed by more than 1%
-			if ((fabsf(intavg.update_interval - sample_interval_avg) / intavg.update_interval) > 0.01f) {
-
-				intavg.update_interval = sample_interval_avg;
-				updated = true;
-			}
-		}
-
-		// reset sample interval accumulator
-		intavg.timestamp_sample_last = 0;
-	}
-
 	return updated;
 }
 
@@ -163,7 +165,15 @@ void VehicleIMU::Run()
 	// integrate queued gyro
 	sensor_gyro_s gyro;
 
-	while (!_gyro_integrator.integral_ready() && _sensor_gyro_sub.update(&gyro)) {
+	while (_sensor_gyro_sub.update(&gyro)) {
+		perf_count_interval(_gyro_update_perf, gyro.timestamp_sample);
+
+		if (_sensor_gyro_sub.get_last_generation() != _gyro_last_generation + 1) {
+			perf_count(_gyro_generation_gap_perf);
+		}
+
+		_gyro_last_generation = _sensor_gyro_sub.get_last_generation();
+
 		_gyro_corrections.set_device_id(gyro.device_id);
 		_gyro_error_count = gyro.error_count;
 
@@ -175,12 +185,24 @@ void VehicleIMU::Run()
 		if (UpdateIntervalAverage(_gyro_interval, gyro.timestamp_sample)) {
 			update_integrator_config = true;
 		}
+
+		if (_intervals_configured && _gyro_integrator.integral_ready()) {
+			break;
+		}
 	}
 
 	// update accel, stopping once caught up to the last gyro sample
 	sensor_accel_s accel;
 
 	while (_sensor_accel_sub.update(&accel)) {
+		perf_count_interval(_accel_update_perf, accel.timestamp_sample);
+
+		if (_sensor_accel_sub.get_last_generation() != _accel_last_generation + 1) {
+			perf_count(_accel_generation_gap_perf);
+		}
+
+		_accel_last_generation = _sensor_accel_sub.get_last_generation();
+
 		_accel_corrections.set_device_id(accel.device_id);
 		_accel_error_count = accel.error_count;
 
@@ -222,7 +244,9 @@ void VehicleIMU::Run()
 		}
 
 		// break once caught up to gyro
-		if (_last_timestamp_sample_accel >= (_last_timestamp_sample_gyro - 0.5f * _accel_interval.update_interval)) {
+		if (_intervals_configured
+		    && (_last_timestamp_sample_accel >= (_last_timestamp_sample_gyro - 0.5f * _accel_interval.update_interval))) {
+
 			break;
 		}
 	}
@@ -314,8 +338,11 @@ void VehicleIMU::UpdateIntegratorConfiguration()
 		// run when there are enough new gyro samples, unregister accel
 		_sensor_accel_sub.unregisterCallback();
 
-		PX4_DEBUG("accel (%d), gyro (%d), accel samples: %d, gyro samples: %d",
+		_intervals_configured = true;
-			  _accel_corrections.get_device_id(), _gyro_corrections.get_device_id(), accel_integral_samples, gyro_integral_samples);
+
+		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,
+			  (double)_accel_interval.update_interval, (double)_gyro_interval.update_interval);
 	}
 }
 
@@ -347,6 +374,11 @@ void VehicleIMU::PrintStatus()
 		 _accel_corrections.get_device_id(), (double)_accel_interval.update_interval,
 		 _gyro_corrections.get_device_id(), (double)_gyro_interval.update_interval);
 
+	perf_print_counter(_accel_generation_gap_perf);
+	perf_print_counter(_gyro_generation_gap_perf);
+	perf_print_counter(_accel_update_perf);
+	perf_print_counter(_gyro_update_perf);
+
 	_accel_corrections.PrintStatus();
 	_gyro_corrections.PrintStatus();
 }

src/modules/sensors/vehicle_imu/VehicleIMU.hpp

@@ -39,6 +39,7 @@
 
 #include <lib/mathlib/math/Limits.hpp>
 #include <lib/matrix/matrix/math.hpp>
+#include <lib/perf/perf_counter.h>
 #include <px4_platform_common/log.h>
 #include <px4_platform_common/module_params.h>
 #include <px4_platform_common/px4_config.h>
@@ -102,6 +103,9 @@ private:
 	IntervalAverage _accel_interval{};
 	IntervalAverage _gyro_interval{};
 
+	unsigned _accel_last_generation{0};
+	unsigned _gyro_last_generation{0};
+
 	uint32_t _accel_error_count{0};
 	uint32_t _gyro_error_count{0};
 
@@ -114,6 +118,13 @@ private:
 	uint8_t _delta_velocity_clipping{0};
 	uint32_t _delta_velocity_clipping_total[3] {};
 
+	bool _intervals_configured{false};
+
+	perf_counter_t _accel_update_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": accel update interval")};
+	perf_counter_t _accel_generation_gap_perf{perf_alloc(PC_COUNT, MODULE_NAME": accel data gap")};
+	perf_counter_t _gyro_update_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": gyro update interval")};
+	perf_counter_t _gyro_generation_gap_perf{perf_alloc(PC_COUNT, MODULE_NAME": gyro data gap")};
+
 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::IMU_INTEG_RATE>) _param_imu_integ_rate
 	)

src/modules/uORB/SubscriptionInterval.hpp

@@ -135,8 +135,9 @@ public:
 	bool		valid() const { return _subscription.valid(); }
 
 	uint8_t		get_instance() const { return _subscription.get_instance(); }
-	orb_id_t	get_topic() const { return _subscription.get_topic(); }
+	unsigned	get_last_generation() const { return _subscription.get_last_generation(); }
 	ORB_PRIO	get_priority() { return _subscription.get_priority(); }
+	orb_id_t	get_topic() const { return _subscription.get_topic(); }
 
 	/**
 	 * Set the interval in microseconds