sensors: filter sample rate calculate with simple interval average instead of perf count

- the perf counter intervals aren't numerically stable over extended periods (https://github.com/PX4/Firmware/pull/14046)

src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp

@@ -52,8 +52,6 @@ VehicleAcceleration::VehicleAcceleration() :
 VehicleAcceleration::~VehicleAcceleration()
 {
 	Stop();
-
-	perf_free(_interval_perf);
 }
 
 bool VehicleAcceleration::Start()
@@ -85,40 +83,40 @@ void VehicleAcceleration::Stop()
 
 void VehicleAcceleration::CheckFilters()
 {
-	if ((hrt_elapsed_time(&_filter_check_last) > 100_ms)) {
+	// check filter periodically (roughly once every 1-3 seconds depending on sensor configuration)
-		_filter_check_last = hrt_absolute_time();
+	if (_interval_count > 2500) {
+		bool sample_rate_changed = false;
 
 		// calculate sensor update rate
-		const float sample_interval_avg = perf_mean(_interval_perf);
+		const float sample_interval_avg = _interval_sum / _interval_count;
 
 		if (PX4_ISFINITE(sample_interval_avg) && (sample_interval_avg > 0.0f)) {
 
-			const float update_rate_hz = 1.0f / sample_interval_avg;
+			const float update_rate_hz = 1.e6f / sample_interval_avg;
 
 			if ((fabsf(update_rate_hz) > 0.0f) && PX4_ISFINITE(update_rate_hz)) {
 				_update_rate_hz = update_rate_hz;
 
 				// check if sample rate error is greater than 1%
 				if ((fabsf(_update_rate_hz - _filter_sample_rate) / _filter_sample_rate) > 0.01f) {
-					++_sample_rate_incorrect_count;
+					sample_rate_changed = true;
 				}
 			}
 		}
 
-		const bool sample_rate_updated = (_sample_rate_incorrect_count > 50);
 		const bool lp_updated = (fabsf(_lp_filter.get_cutoff_freq() - _param_imu_accel_cutoff.get()) > 0.01f);
 
-		if (sample_rate_updated || lp_updated) {
+		if (sample_rate_changed || lp_updated) {
-			PX4_INFO("updating filter, sample rate: %.3f Hz -> %.3f Hz", (double)_filter_sample_rate, (double)_update_rate_hz);
+			PX4_DEBUG("resetting filters, sample rate: %.3f Hz -> %.3f Hz", (double)_filter_sample_rate, (double)_update_rate_hz);
 			_filter_sample_rate = _update_rate_hz;
 
 			// update software low pass filters
 			_lp_filter.set_cutoff_frequency(_filter_sample_rate, _param_imu_accel_cutoff.get());
 			_lp_filter.reset(_acceleration_prev);
-
-			// reset state
-			_sample_rate_incorrect_count = 0;
 		}
+
+		// reset sample interval accumulator
+		_timestamp_sample_last = 0;
 	}
 }
 
@@ -167,8 +165,8 @@ bool VehicleAcceleration::SensorSelectionUpdate(bool force)
 
 						_corrections.set_device_id(report.device_id);
 
-						// reset sample rate monitor
+						// reset sample interval accumulator on sensor change
-						_sample_rate_incorrect_count = 0;
+						_timestamp_sample_last = 0;
 
 						return true;
 					}
@@ -218,7 +216,17 @@ void VehicleAcceleration::Run()
 		if (_sensor_sub[_selected_sensor_sub_index].copy(&sensor_data)) {
 
 			if (sensor_updated) {
-				perf_count_interval(_interval_perf, sensor_data.timestamp_sample);
+				// collect sample interval average for filters
+				if ((_timestamp_sample_last > 0) && (sensor_data.timestamp_sample > _timestamp_sample_last)) {
+					_interval_sum += (sensor_data.timestamp_sample - _timestamp_sample_last);
+					_interval_count++;
+
+				} else {
+					_interval_sum = 0.f;
+					_interval_count = 0.f;
+				}
+
+				_timestamp_sample_last = sensor_data.timestamp_sample;
 			}
 
 			CheckFilters();

src/modules/sensors/vehicle_acceleration/VehicleAcceleration.hpp

@@ -90,25 +90,25 @@ private:
 
 	SensorCorrections _corrections;
 
-	perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};
-
 	matrix::Vector3f _bias{0.f, 0.f, 0.f};
 
 	matrix::Vector3f _acceleration_prev{0.f, 0.f, 0.f};
 
 	hrt_abstime _last_publish{0};
-	hrt_abstime _filter_check_last{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] */
 
 	math::LowPassFilter2pVector3f _lp_filter{kInitialRateHz, 30.0f};
 
 	float _filter_sample_rate{kInitialRateHz};
-	int _sample_rate_incorrect_count{0};
 
 	uint32_t _selected_sensor_device_id{0};
 	uint8_t _selected_sensor_sub_index{0};
 
+	hrt_abstime _timestamp_sample_last{0};
+	float _interval_sum{0.f};
+	float _interval_count{0.f};
+
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::IMU_ACCEL_CUTOFF>) _param_imu_accel_cutoff
 	)

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp

@@ -55,8 +55,6 @@ VehicleAngularVelocity::VehicleAngularVelocity() :
 VehicleAngularVelocity::~VehicleAngularVelocity()
 {
 	Stop();
-
-	perf_free(_interval_perf);
 }
 
 bool VehicleAngularVelocity::Start()
@@ -88,28 +86,27 @@ void VehicleAngularVelocity::Stop()
 
 void VehicleAngularVelocity::CheckFilters()
 {
-	if ((hrt_elapsed_time(&_filter_check_last) > 100_ms)) {
+	// check filter periodically (roughly once every 1-3 seconds depending on sensor configuration)
-		_filter_check_last = hrt_absolute_time();
+	if (_interval_count > 2500) {
+		bool sample_rate_changed = false;
 
 		// calculate sensor update rate
-		const float sample_interval_avg = perf_mean(_interval_perf);
+		const float sample_interval_avg = _interval_sum / _interval_count;
 
 		if (PX4_ISFINITE(sample_interval_avg) && (sample_interval_avg > 0.0f)) {
 
-			const float update_rate_hz = 1.0f / sample_interval_avg;
+			const float update_rate_hz = 1.e6f / sample_interval_avg;
 
 			if ((fabsf(update_rate_hz) > 0.0f) && PX4_ISFINITE(update_rate_hz)) {
 				_update_rate_hz = update_rate_hz;
 
 				// check if sample rate error is greater than 1%
 				if ((fabsf(_update_rate_hz - _filter_sample_rate) / _filter_sample_rate) > 0.01f) {
-					++_sample_rate_incorrect_count;
+					sample_rate_changed = true;
 				}
 			}
 		}
 
-		const bool sample_rate_updated = (_sample_rate_incorrect_count > 50);
-
 		const bool lp_velocity_updated = (fabsf(_lp_filter_velocity.get_cutoff_freq() - _param_imu_gyro_cutoff.get()) > 0.01f);
 		const bool notch_updated = ((fabsf(_notch_filter_velocity.getNotchFreq() - _param_imu_gyro_nf_freq.get()) > 0.01f)
 					    || (fabsf(_notch_filter_velocity.getBandwidth() - _param_imu_gyro_nf_bw.get()) > 0.01f));
@@ -117,8 +114,8 @@ void VehicleAngularVelocity::CheckFilters()
 		const bool lp_acceleration_updated = (fabsf(_lp_filter_acceleration.get_cutoff_freq() - _param_imu_dgyro_cutoff.get()) >
 						      0.01f);
 
-		if (sample_rate_updated || lp_velocity_updated || notch_updated || lp_acceleration_updated) {
+		if (sample_rate_changed || lp_velocity_updated || notch_updated || lp_acceleration_updated) {
-			PX4_INFO("updating filter, sample rate: %.3f Hz -> %.3f Hz", (double)_filter_sample_rate, (double)_update_rate_hz);
+			PX4_DEBUG("resetting filters, sample rate: %.3f Hz -> %.3f Hz", (double)_filter_sample_rate, (double)_update_rate_hz);
 			_filter_sample_rate = _update_rate_hz;
 
 			// update software low pass filters
@@ -130,10 +127,10 @@ void VehicleAngularVelocity::CheckFilters()
 
 			_lp_filter_acceleration.set_cutoff_frequency(_filter_sample_rate, _param_imu_dgyro_cutoff.get());
 			_lp_filter_acceleration.reset(_angular_acceleration_prev);
-
-			// reset state
-			_sample_rate_incorrect_count = 0;
 		}
+
+		// reset sample interval accumulator
+		_timestamp_sample_last = 0;
 	}
 }
 
@@ -182,8 +179,8 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(bool force)
 
 						_corrections.set_device_id(report.device_id);
 
-						// reset sample rate monitor
+						// reset sample interval accumulator on sensor change
-						_sample_rate_incorrect_count = 0;
+						_timestamp_sample_last = 0;
 
 						return true;
 					}
@@ -233,7 +230,17 @@ void VehicleAngularVelocity::Run()
 		if (_sensor_sub[_selected_sensor_sub_index].copy(&sensor_data)) {
 
 			if (sensor_updated) {
-				perf_count_interval(_interval_perf, sensor_data.timestamp_sample);
+				// collect sample interval average for filters
+				if ((_timestamp_sample_last > 0) && (sensor_data.timestamp_sample > _timestamp_sample_last)) {
+					_interval_sum += (sensor_data.timestamp_sample - _timestamp_sample_last);
+					_interval_count++;
+
+				} else {
+					_interval_sum = 0.f;
+					_interval_count = 0.f;
+				}
+
+				_timestamp_sample_last = sensor_data.timestamp_sample;
 			}
 
 			// Guard against too small (< 0.2ms) and too large (> 20ms) dt's.

src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp

@@ -93,8 +93,6 @@ private:
 
 	SensorCorrections _corrections;
 
-	perf_counter_t _interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};
-
 	matrix::Vector3f _bias{0.f, 0.f, 0.f};
 
 	matrix::Vector3f _angular_acceleration_prev{0.f, 0.f, 0.f};
@@ -102,7 +100,6 @@ private:
 	hrt_abstime _timestamp_sample_prev{0};
 
 	hrt_abstime _last_publish{0};
-	hrt_abstime _filter_check_last{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] */
 
@@ -114,11 +111,14 @@ private:
 	math::LowPassFilter2pVector3f _lp_filter_acceleration{kInitialRateHz, 10.0f};
 
 	float _filter_sample_rate{kInitialRateHz};
-	int _sample_rate_incorrect_count{0};
 
 	uint32_t _selected_sensor_device_id{0};
 	uint8_t _selected_sensor_sub_index{0};
 
+	hrt_abstime _timestamp_sample_last{0};
+	float _interval_sum{0.f};
+	float _interval_count{0.f};
+
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::IMU_GYRO_CUTOFF>) _param_imu_gyro_cutoff,
 		(ParamFloat<px4::params::IMU_GYRO_NF_FREQ>) _param_imu_gyro_nf_freq,