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/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