sensors: move gyro filtering to sensors/vehicle_angular_velocity

- gyro filtering (low-pass and notch) only performed on primary gyro in `sensors/vehicle_angular_velocity` instead of every gyro in `PX4Gyroscope`
 - sample rate is calculated from actual updates (the fixed value was slightly wrong in many cases, and very wrong in a few)
 - In the FIFO case the array is now averaged and published in `sensor_gyro` for filtering downstream. I'll update this in the future to use the full FIFO array (if available), but right now it should be fine.

src/lib/drivers/gyroscope/PX4Gyroscope.cpp

@@ -65,7 +65,6 @@ static inline unsigned clipping(const int16_t samples[16], int16_t clip_limit, u
 
 PX4Gyroscope::PX4Gyroscope(uint32_t device_id, uint8_t priority, enum Rotation rotation) :
 	CDev(nullptr),
-	ModuleParams(nullptr),
 	_sensor_pub{ORB_ID(sensor_gyro), priority},
 	_sensor_fifo_pub{ORB_ID(sensor_gyro_fifo), priority},
 	_sensor_integrated_pub{ORB_ID(sensor_gyro_integrated), priority},
@@ -75,11 +74,6 @@ PX4Gyroscope::PX4Gyroscope(uint32_t device_id, uint8_t priority, enum Rotation r
 	_rotation_dcm{get_rot_matrix(rotation)}
 {
 	_class_device_instance = register_class_devname(GYRO_BASE_DEVICE_PATH);
-
-	// set software low pass filter for controllers
-	updateParams();
-	ConfigureFilter(_param_imu_gyro_cutoff.get());
-	ConfigureNotchFilter(_param_imu_gyro_nf_freq.get(), _param_imu_gyro_nf_bw.get());
 }
 
 PX4Gyroscope::~PX4Gyroscope()
@@ -123,14 +117,6 @@ void PX4Gyroscope::set_device_type(uint8_t devtype)
 	_device_id = device_id.devid;
 }
 
-void PX4Gyroscope::set_sample_rate(uint16_t rate)
-{
-	_sample_rate = rate;
-
-	ConfigureFilter(_filter.get_cutoff_freq());
-	ConfigureNotchFilter(_notch_filter.getNotchFreq(), _notch_filter.getBandwidth());
-}
-
 void PX4Gyroscope::set_update_rate(uint16_t rate)
 {
 	const uint32_t update_interval = 1000000 / rate;
@@ -155,21 +141,16 @@ void PX4Gyroscope::update(hrt_abstime timestamp_sample, float x, float y, float
 	// Apply range scale and the calibrating offset/scale
 	const Vector3f val_calibrated{((raw * _scale) - _calibration_offset)};
 
-	// Filtered values: apply notch and then low-pass
-	Vector3f val_filtered{_notch_filter.apply(val_calibrated)};
-	val_filtered = _filter.apply(val_filtered);
-
-
-	// publish control data (filtered) immediately
+	// publish raw data immediately
 	{
 		sensor_gyro_s report{};
 
 		report.timestamp_sample = timestamp_sample;
 		report.device_id = _device_id;
 		report.temperature = _temperature;
-		report.x = val_filtered(0);
-		report.y = val_filtered(1);
-		report.z = val_filtered(2);
+		report.x = val_calibrated(0);
+		report.y = val_calibrated(1);
+		report.z = val_calibrated(2);
 		report.timestamp = hrt_absolute_time();
 
 		_sensor_pub.publish(report);
@@ -213,45 +194,30 @@ void PX4Gyroscope::updateFIFO(const FIFOSample &sample)
 	const uint8_t N = sample.samples;
 	const float dt = sample.dt;
 
-	// filtered data (control)
-	float x_filtered = _filterArrayX.apply(sample.x, N);
-	float y_filtered = _filterArrayY.apply(sample.y, N);
-	float z_filtered = _filterArrayZ.apply(sample.z, N);
-
-	// Apply rotation (before scaling)
-	rotate_3f(_rotation, x_filtered, y_filtered, z_filtered);
-
-	// Apply range scale and the calibration offset
-	const Vector3f val_calibrated{(Vector3f{x_filtered, y_filtered, z_filtered} * _scale) - _calibration_offset};
-
-
-	// publish control data (filtered) immediately
+	// publish raw data immediately
 	{
-		bool publish_control = true;
+		// average
+		float x = (float)sum(sample.x, N) / (float)N;
+		float y = (float)sum(sample.y, N) / (float)N;
+		float z = (float)sum(sample.z, N) / (float)N;
 
-		if (_param_imu_gyro_rate_max.get() > 0) {
-			const uint64_t interval = 1e6f / _param_imu_gyro_rate_max.get();
+		// Apply rotation (before scaling)
+		rotate_3f(_rotation, x, y, z);
 
-			if (hrt_elapsed_time(&_control_last_publish) < interval) {
-				publish_control = false;
-			}
-		}
+		// Apply range scale and the calibration offset
+		const Vector3f val_calibrated{(Vector3f{x, y, z} * _scale) - _calibration_offset};
 
-		if (publish_control) {
-			sensor_gyro_s report{};
+		sensor_gyro_s report{};
 
-			report.timestamp_sample = sample.timestamp_sample;
-			report.device_id = _device_id;
-			report.temperature = _temperature;
-			report.x = val_calibrated(0);
-			report.y = val_calibrated(1);
-			report.z = val_calibrated(2);
-			report.timestamp = hrt_absolute_time();
+		report.timestamp_sample = sample.timestamp_sample;
+		report.device_id = _device_id;
+		report.temperature = _temperature;
+		report.x = val_calibrated(0);
+		report.y = val_calibrated(1);
+		report.z = val_calibrated(2);
+		report.timestamp = hrt_absolute_time();
 
-			_sensor_pub.publish(report);
-
-			_control_last_publish = report.timestamp_sample;
-		}
+		_sensor_pub.publish(report);
 	}
 
 
@@ -356,7 +322,6 @@ void PX4Gyroscope::PublishStatus()
 		status.full_scale_range = _range;
 		status.rotation = _rotation;
 		status.measure_rate = _update_rate;
-		status.sample_rate = _sample_rate;
 		status.temperature = _temperature;
 		status.vibration_metric = _vibration_metric;
 		status.coning_vibration = _coning_vibration;
@@ -380,20 +345,6 @@ void PX4Gyroscope::ResetIntegrator()
 	_timestamp_sample_prev = 0;
 }
 
-void PX4Gyroscope::ConfigureFilter(float cutoff_freq)
-{
-	_filter.set_cutoff_frequency(_sample_rate, cutoff_freq);
-
-	_filterArrayX.set_cutoff_frequency(_sample_rate, cutoff_freq);
-	_filterArrayY.set_cutoff_frequency(_sample_rate, cutoff_freq);
-	_filterArrayZ.set_cutoff_frequency(_sample_rate, cutoff_freq);
-}
-
-void PX4Gyroscope::ConfigureNotchFilter(float notch_freq, float bandwidth)
-{
-	_notch_filter.setParameters(_sample_rate, notch_freq, bandwidth);
-}
-
 void PX4Gyroscope::UpdateClipLimit()
 {
 	// 95% of potential max
@@ -417,10 +368,6 @@ void PX4Gyroscope::UpdateVibrationMetrics(const Vector3f &delta_angle)
 void PX4Gyroscope::print_status()
 {
 	PX4_INFO(GYRO_BASE_DEVICE_PATH " device instance: %d", _class_device_instance);
-	PX4_INFO("sample rate: %d Hz", _sample_rate);
-	PX4_INFO("filter cutoff: %.3f Hz", (double)_filter.get_cutoff_freq());
-	PX4_INFO("notch filter freq: %.3f Hz\tbandwidth: %.3f Hz", (double)_notch_filter.getNotchFreq(),
-		 (double)_notch_filter.getBandwidth());
 
 	PX4_INFO("calibration offset: %.5f %.5f %.5f", (double)_calibration_offset(0), (double)_calibration_offset(1),
 		 (double)_calibration_offset(2));

src/lib/drivers/gyroscope/PX4Gyroscope.hpp

@@ -38,17 +38,13 @@
 #include <lib/cdev/CDev.hpp>
 #include <lib/conversion/rotation.h>
 #include <lib/drivers/device/integrator.h>
-#include <lib/mathlib/math/filter/LowPassFilter2pArray.hpp>
-#include <lib/mathlib/math/filter/LowPassFilter2pVector3f.hpp>
-#include <lib/mathlib/math/filter/NotchFilter.hpp>
-#include <px4_platform_common/module_params.h>
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/topics/sensor_gyro.h>
 #include <uORB/topics/sensor_gyro_fifo.h>
 #include <uORB/topics/sensor_gyro_integrated.h>
 #include <uORB/topics/sensor_gyro_status.h>
 
-class PX4Gyroscope : public cdev::CDev, public ModuleParams
+class PX4Gyroscope : public cdev::CDev
 {
 public:
 	PX4Gyroscope(uint32_t device_id, uint8_t priority = ORB_PRIO_DEFAULT, enum Rotation rotation = ROTATION_NONE);
@@ -62,7 +58,6 @@ public:
 	void set_device_type(uint8_t devtype);
 	void set_error_count(uint64_t error_count) { _error_count += error_count; }
 	void set_range(float range) { _range = range; UpdateClipLimit(); }
-	void set_sample_rate(uint16_t rate);
 	void set_scale(float scale) { _scale = scale; UpdateClipLimit(); }
 	void set_temperature(float temperature) { _temperature = temperature; }
 	void set_update_rate(uint16_t rate);
@@ -88,8 +83,6 @@ public:
 
 private:
 
-	void ConfigureFilter(float cutoff_freq);
-	void ConfigureNotchFilter(float notch_freq, float bandwidth);
 	void PublishStatus();
 	void ResetIntegrator();
 	void UpdateClipLimit();
@@ -100,16 +93,8 @@ private:
 	uORB::PublicationMulti<sensor_gyro_integrated_s> _sensor_integrated_pub;
 	uORB::PublicationMulti<sensor_gyro_status_s>     _sensor_status_pub;
 
-	math::LowPassFilter2pVector3f _filter{1000, 100};
-	math::NotchFilter<matrix::Vector3f> _notch_filter{};
-
-	hrt_abstime	_control_last_publish{0};
 	hrt_abstime	_status_last_publish{0};
 
-	math::LowPassFilter2pArray _filterArrayX{8000, 100};
-	math::LowPassFilter2pArray _filterArrayY{8000, 100};
-	math::LowPassFilter2pArray _filterArrayZ{8000, 100};
-
 	Integrator		_integrator{4000, true};
 
 	matrix::Vector3f	_calibration_offset{0.f, 0.f, 0.f};
@@ -134,7 +119,6 @@ private:
 
 	uint32_t		_clipping[3] {};
 
-	uint16_t		_sample_rate{1000};
 	uint16_t		_update_rate{1000};
 
 	// integrator
@@ -146,10 +130,4 @@ private:
 	uint8_t			_integrator_fifo_samples{0};
 	uint8_t			_integrator_clipping{0};
 
-	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::IMU_GYRO_CUTOFF>) _param_imu_gyro_cutoff,
-		(ParamFloat<px4::params::IMU_GYRO_NF_FREQ>) _param_imu_gyro_nf_freq,
-		(ParamFloat<px4::params::IMU_GYRO_NF_BW>) _param_imu_gyro_nf_bw,
-		(ParamInt<px4::params::IMU_GYRO_RATEMAX>) _param_imu_gyro_rate_max
-	)
 };

src/lib/mathlib/math/filter/LowPassFilter2pArray.hpp

@@ -38,6 +38,8 @@
 
 #include "LowPassFilter2p.hpp"
 
+#include <px4_platform_common/defines.h>
+
 namespace math
 {
 

src/lib/perf/perf_counter.cpp

@@ -207,47 +207,9 @@ perf_count(perf_counter_t handle)
 		((struct perf_ctr_count *)handle)->event_count++;
 		break;
 
-	case PC_INTERVAL: {
-			struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle;
-			hrt_abstime now = hrt_absolute_time();
-
-			switch (pci->event_count) {
-			case 0:
-				pci->time_first = now;
-				break;
-
-			case 1:
-				pci->time_least = (uint32_t)(now - pci->time_last);
-				pci->time_most = (uint32_t)(now - pci->time_last);
-				pci->mean = pci->time_least / 1e6f;
-				pci->M2 = 0;
-				break;
-
-			default: {
-					hrt_abstime interval = now - pci->time_last;
-
-					if ((uint32_t)interval < pci->time_least) {
-						pci->time_least = (uint32_t)interval;
-					}
-
-					if ((uint32_t)interval > pci->time_most) {
-						pci->time_most = (uint32_t)interval;
-					}
-
-					// maintain mean and variance of interval in seconds
-					// Knuth/Welford recursive mean and variance of update intervals (via Wikipedia)
-					float dt = interval / 1e6f;
-					float delta_intvl = dt - pci->mean;
-					pci->mean += delta_intvl / pci->event_count;
-					pci->M2 += delta_intvl * (dt - pci->mean);
-					break;
-				}
-			}
-
-			pci->time_last = now;
-			pci->event_count++;
-			break;
-		}
+	case PC_INTERVAL:
+		perf_count_interval(handle, hrt_absolute_time());
+		break;
 
 	default:
 		break;
@@ -357,6 +319,60 @@ perf_set_elapsed(perf_counter_t handle, int64_t elapsed)
 	}
 }
 
+void
+perf_count_interval(perf_counter_t handle, hrt_abstime now)
+{
+	if (handle == nullptr) {
+		return;
+	}
+
+	switch (handle->type) {
+	case PC_INTERVAL: {
+			struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle;
+
+			switch (pci->event_count) {
+			case 0:
+				pci->time_first = now;
+				break;
+
+			case 1:
+				pci->time_least = (uint32_t)(now - pci->time_last);
+				pci->time_most = (uint32_t)(now - pci->time_last);
+				pci->mean = pci->time_least / 1e6f;
+				pci->M2 = 0;
+				break;
+
+			default: {
+					hrt_abstime interval = now - pci->time_last;
+
+					if ((uint32_t)interval < pci->time_least) {
+						pci->time_least = (uint32_t)interval;
+					}
+
+					if ((uint32_t)interval > pci->time_most) {
+						pci->time_most = (uint32_t)interval;
+					}
+
+					// maintain mean and variance of interval in seconds
+					// Knuth/Welford recursive mean and variance of update intervals (via Wikipedia)
+					float dt = interval / 1e6f;
+					float delta_intvl = dt - pci->mean;
+					pci->mean += delta_intvl / pci->event_count;
+					pci->M2 += delta_intvl * (dt - pci->mean);
+					break;
+				}
+			}
+
+			pci->time_last = now;
+			pci->event_count++;
+			break;
+		}
+
+	default:
+		break;
+	}
+}
+
 void
 perf_set_count(perf_counter_t handle, uint64_t count)
 {
@@ -396,8 +412,6 @@ perf_cancel(perf_counter_t handle)
 	}
 }
 
-
-
 void
 perf_reset(perf_counter_t handle)
 {
@@ -571,6 +585,31 @@ perf_event_count(perf_counter_t handle)
 	return 0;
 }
 
+float
+perf_mean(perf_counter_t handle)
+{
+	if (handle == nullptr) {
+		return 0;
+	}
+
+	switch (handle->type) {
+	case PC_ELAPSED: {
+			struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
+			return pce->mean;
+		}
+
+	case PC_INTERVAL: {
+			struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle;
+			return pci->mean;
+		}
+
+	default:
+		break;
+	}
+
+	return 0.0f;
+}
+
 void
 perf_iterate_all(perf_callback cb, void *user)
 {

src/lib/perf/perf_counter.h

@@ -132,6 +132,18 @@ __EXPORT extern void		perf_end(perf_counter_t handle);
  */
 __EXPORT extern void		perf_set_elapsed(perf_counter_t handle, int64_t elapsed);
 
+/**
+ * Register a measurement
+ *
+ * This call applies to counters that operate over ranges of time; PC_ELAPSED etc.
+ * If a call is made without a corresponding perf_begin call. It sets the
+ * value provided as argument as a new measurement.
+ *
+ * @param handle		The handle returned from perf_alloc.
+ * @param time			The time for the interval.
+ */
+__EXPORT extern void		perf_count_interval(perf_counter_t handle, uint64_t time);
+
 /**
  * Set a counter
  *
@@ -228,6 +240,14 @@ __EXPORT extern void		perf_reset_all(void);
  */
 __EXPORT extern uint64_t	perf_event_count(perf_counter_t handle);
 
+/**
+ * Return current mean
+ *
+ * @param handle		The handle returned from perf_alloc.
+ * @param return		mean
+ */
+__EXPORT extern float		perf_mean(perf_counter_t handle);
+
 __END_DECLS
 
 #endif