sensors/vehicle_imu: vehicle_imu_status include accel/gyro full raw FIFO sample rate

msg/sensor_accel.msg

@@ -13,4 +13,6 @@ uint32 error_count
 
 uint8[3] clip_counter     # clip count per axis in the sample period
 
+uint8 samples             # number of raw samples that went into this message
+
 uint8 ORB_QUEUE_LENGTH = 8

msg/sensor_gyro.msg

@@ -11,4 +11,6 @@ float32 temperature       # temperature in degrees Celsius
 
 uint32 error_count
 
+uint8 samples             # number of raw samples that went into this message
+
 uint8 ORB_QUEUE_LENGTH = 8

msg/vehicle_imu_status.msg

@@ -11,6 +11,9 @@ uint32 gyro_error_count
 float32 accel_rate_hz
 float32 gyro_rate_hz
 
+float32 accel_raw_rate_hz       # full raw sensor sample rate (Hz)
+float32 gyro_raw_rate_hz        # full raw sensor sample rate (Hz)
+
 float32 accel_vibration_metric  # high frequency vibration level in the IMU delta velocity data (m/s)
 float32 gyro_vibration_metric   # high frequency vibration level in the IMU delta velocity data (m/s)
 float32 gyro_coning_vibration   # Level of coning vibration in the IMU delta angles (rad^2)

src/lib/drivers/accelerometer/PX4Accelerometer.cpp

@@ -140,11 +140,12 @@ void PX4Accelerometer::updateFIFO(sensor_accel_fifo_s &sample)
 		const float z = integral(2) / (float)N;
 
 		// publish
-		Publish(sample.timestamp_sample, x, y, z, clip_count);
+		Publish(sample.timestamp_sample, x, y, z, clip_count, N);
 	}
 }
 
-void PX4Accelerometer::Publish(const hrt_abstime &timestamp_sample, float x, float y, float z, uint8_t clip_count[3])
+void PX4Accelerometer::Publish(const hrt_abstime &timestamp_sample, float x, float y, float z, uint8_t clip_count[3],
+			       uint8_t samples)
 {
 	// Apply rotation (before scaling)
 	rotate_3f(_rotation, x, y, z);
@@ -166,6 +167,7 @@ void PX4Accelerometer::Publish(const hrt_abstime &timestamp_sample, float x, flo
 	report.clip_counter[0] = fabsf(roundf(clipping_x));
 	report.clip_counter[1] = fabsf(roundf(clipping_y));
 	report.clip_counter[2] = fabsf(roundf(clipping_z));
+	report.samples = samples;
 	report.timestamp = hrt_absolute_time();
 
 	_sensor_pub.publish(report);

src/lib/drivers/accelerometer/PX4Accelerometer.hpp

@@ -63,7 +63,8 @@ public:
 	void updateFIFO(sensor_accel_fifo_s &sample);
 
 private:
-	void Publish(const hrt_abstime &timestamp_sample, float x, float y, float z, uint8_t clip_count[3]);
+	void Publish(const hrt_abstime &timestamp_sample, float x, float y, float z, uint8_t clip_count[3],
+		     uint8_t samples = 1);
 	void UpdateClipLimit();
 
 	uORB::PublicationMulti<sensor_accel_s> _sensor_pub{ORB_ID(sensor_accel)};

src/lib/drivers/gyroscope/PX4Gyroscope.cpp

@@ -114,11 +114,11 @@ void PX4Gyroscope::updateFIFO(sensor_gyro_fifo_s &sample)
 		const float z = integral(2) / (float)N;
 
 		// publish
-		Publish(sample.timestamp_sample, x, y, z);
+		Publish(sample.timestamp_sample, x, y, z, N);
 	}
 }
 
-void PX4Gyroscope::Publish(const hrt_abstime &timestamp_sample, float x, float y, float z)
+void PX4Gyroscope::Publish(const hrt_abstime &timestamp_sample, float x, float y, float z, uint8_t samples)
 {
 	// Apply rotation (before scaling)
 	rotate_3f(_rotation, x, y, z);
@@ -132,6 +132,7 @@ void PX4Gyroscope::Publish(const hrt_abstime &timestamp_sample, float x, float y
 	report.x = x * _scale;
 	report.y = y * _scale;
 	report.z = z * _scale;
+	report.samples = samples;
 	report.timestamp = hrt_absolute_time();
 
 	_sensor_pub.publish(report);

src/lib/drivers/gyroscope/PX4Gyroscope.hpp

@@ -62,7 +62,7 @@ public:
 	void updateFIFO(sensor_gyro_fifo_s &sample);
 
 private:
-	void Publish(const hrt_abstime &timestamp_sample, float x, float y, float z);
+	void Publish(const hrt_abstime &timestamp_sample, float x, float y, float z, uint8_t samples = 1);
 
 	uORB::PublicationMulti<sensor_gyro_s> _sensor_pub{ORB_ID(sensor_gyro)};
 	uORB::PublicationMulti<sensor_gyro_fifo_s>  _sensor_fifo_pub{ORB_ID(sensor_gyro_fifo)};

src/modules/sensors/vehicle_imu/VehicleIMU.cpp

@@ -141,50 +141,54 @@ void VehicleIMU::ParametersUpdate(bool force)
 	}
 }
 
-bool VehicleIMU::UpdateIntervalAverage(IntervalAverage &intavg, const hrt_abstime &timestamp_sample)
+bool VehicleIMU::UpdateIntervalAverage(IntervalAverage &intavg, const hrt_abstime &timestamp_sample, uint8_t samples)
 {
 	bool updated = false;
 
 	// conservative maximum time between samples to reject large gaps and reset averaging
-	float max_interval_us = 10000; // 100 Hz
+	uint32_t max_interval_us = 10000; // 100 Hz
-	float min_interval_us = 100;   // 10,000 Hz
+	uint32_t min_interval_us = 100;   // 10,000 Hz
 
-	if (intavg.update_interval > 0) {
+	if (intavg.update_interval > 0.f) {
 		// if available use previously calculated interval as bounds
-		max_interval_us = 1.5f * intavg.update_interval;
+		max_interval_us = roundf(1.5f * intavg.update_interval);
-		min_interval_us = 0.5f * intavg.update_interval;
+		min_interval_us = roundf(0.5f * intavg.update_interval);
 	}
 
-	const float interval_us = (timestamp_sample - intavg.timestamp_sample_last);
+	const uint32_t interval_us = (timestamp_sample - intavg.timestamp_sample_last);
 
 	if ((intavg.timestamp_sample_last > 0) && (interval_us < max_interval_us) && (interval_us > min_interval_us)) {
 
 		intavg.interval_sum += interval_us;
+		intavg.interval_samples += samples;
 		intavg.interval_count++;
 
 		// 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;
+			const float sample_interval_avg = (float)intavg.interval_sum / (float)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;
+					intavg.update_interval_raw = (float)intavg.interval_sum / (float)intavg.interval_samples;
 					updated = true;
 				}
 			}
 
 			// reset sample interval accumulator
 			intavg.interval_sum = 0.f;
-			intavg.interval_count = 0.f;
+			intavg.interval_samples = 0;
+			intavg.interval_count = 0;
 		}
 
 	} else {
 		// reset
 		intavg.interval_sum = 0.f;
-		intavg.interval_count = 0.f;
+		intavg.interval_samples = 0;
+		intavg.interval_count = 0;
 	}
 
 	intavg.timestamp_sample_last = timestamp_sample;
@@ -222,10 +226,11 @@ void VehicleIMU::Run()
 
 		} else {
 			// collect sample interval average for filters
-			if (!_intervals_configured && UpdateIntervalAverage(_gyro_interval, gyro.timestamp_sample)) {
+			if (!_intervals_configured && UpdateIntervalAverage(_gyro_interval, gyro.timestamp_sample, gyro.samples)) {
 				update_integrator_config = true;
 				publish_status = true;
 				_status.gyro_rate_hz = 1e6f / _gyro_interval.update_interval;
+				_status.gyro_raw_rate_hz = 1e6f / _gyro_interval.update_interval_raw;
 			}
 		}
 
@@ -268,10 +273,11 @@ void VehicleIMU::Run()
 
 		} else {
 			// collect sample interval average for filters
-			if (!_intervals_configured && UpdateIntervalAverage(_accel_interval, accel.timestamp_sample)) {
+			if (!_intervals_configured && UpdateIntervalAverage(_accel_interval, accel.timestamp_sample, accel.samples)) {
 				update_integrator_config = true;
 				publish_status = true;
 				_status.accel_rate_hz = 1e6f / _accel_interval.update_interval;
+				_status.accel_raw_rate_hz = 1e6f / _accel_interval.update_interval_raw;
 			}
 		}
 

src/modules/sensors/vehicle_imu/VehicleIMU.hpp

@@ -77,12 +77,14 @@ private:
 
 	struct IntervalAverage {
 		hrt_abstime timestamp_sample_last{0};
-		float interval_sum{0.f};
+		uint32_t interval_sum{0};
-		float interval_count{0.f};
+		uint32_t interval_samples{0};
+		uint32_t interval_count{0};
 		float update_interval{0.f};
+		float update_interval_raw{0.f};
 	};
 
-	bool UpdateIntervalAverage(IntervalAverage &intavg, const hrt_abstime &timestamp_sample);
+	bool UpdateIntervalAverage(IntervalAverage &intavg, const hrt_abstime &timestamp_sample, uint8_t samples = 1);
 	void UpdateIntegratorConfiguration();
 	void UpdateGyroVibrationMetrics(const matrix::Vector3f &delta_angle);
 	void UpdateAccelVibrationMetrics(const matrix::Vector3f &delta_velocity);