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/drivers/imu/adis16448/ADIS16448.cpp

@@ -259,7 +259,6 @@ ADIS16448::set_sample_rate(uint16_t desired_sample_rate_hz)
 	}
 
 	_px4_accel.set_sample_rate(desired_sample_rate_hz);
-	_px4_gyro.set_sample_rate(desired_sample_rate_hz);
 
 	return true;
 }

src/drivers/imu/adis16477/ADIS16477.cpp

@@ -71,7 +71,6 @@ ADIS16477::ADIS16477(int bus, uint32_t device, enum Rotation rotation) :
 	_px4_accel.set_scale(1.25f * CONSTANTS_ONE_G / 1000.0f); // accel 1.25 mg/LSB
 
 	_px4_gyro.set_device_type(DRV_GYR_DEVTYPE_ADIS16477);
-	_px4_gyro.set_sample_rate(ADIS16477_DEFAULT_RATE);
 	_px4_gyro.set_scale(math::radians(0.025f)); // gyro 0.025 °/sec/LSB
 }
 

src/drivers/imu/adis16497/ADIS16497.cpp

@@ -87,7 +87,6 @@ ADIS16497::ADIS16497(int bus, uint32_t device, enum Rotation rotation) :
 	_px4_accel.set_sample_rate(ADIS16497_DEFAULT_RATE);
 
 	_px4_gyro.set_device_type(DRV_GYR_DEVTYPE_ADIS16497);
-	_px4_gyro.set_sample_rate(ADIS16497_DEFAULT_RATE);
 }
 
 ADIS16497::~ADIS16497()

src/drivers/imu/fxas21002c/FXAS21002C.cpp

@@ -429,8 +429,6 @@ FXAS21002C::set_samplerate(unsigned frequency)
 	modify_reg(FXAS21002C_CTRL_REG1, CTRL_REG1_DR_MASK, bits);
 	set_standby(_current_rate, false);
 
-	_px4_gyro.set_sample_rate(_current_rate);
-
 	return OK;
 }
 

src/drivers/imu/invensense/icm20602/ICM20602.cpp

@@ -59,7 +59,6 @@ ICM20602::ICM20602(int bus, uint32_t device, enum Rotation rotation) :
 	_px4_gyro.set_device_type(DRV_GYR_DEVTYPE_ICM20602);
 
 	_px4_accel.set_sample_rate(ACCEL_RATE);
-	_px4_gyro.set_sample_rate(GYRO_RATE);
 
 	_px4_accel.set_update_rate(1000000 / FIFO_INTERVAL);
 	_px4_gyro.set_update_rate(1000000 / FIFO_INTERVAL);

src/drivers/imu/invensense/icm20608-g/ICM20608G.cpp

@@ -59,7 +59,6 @@ ICM20608G::ICM20608G(int bus, uint32_t device, enum Rotation rotation) :
 	_px4_gyro.set_device_type(DRV_GYR_DEVTYPE_ICM20608);
 
 	_px4_accel.set_sample_rate(ACCEL_RATE);
-	_px4_gyro.set_sample_rate(GYRO_RATE);
 
 	_px4_accel.set_update_rate(1000000 / FIFO_INTERVAL);
 	_px4_gyro.set_update_rate(1000000 / FIFO_INTERVAL);

src/drivers/imu/mpu6000/MPU6000.cpp

@@ -649,13 +649,6 @@ MPU6000::stop()
 	memset(_last_accel, 0, sizeof(_last_accel));
 }
 
-void
-MPU6000::Run()
-{
-	/* make another measurement */
-	measure();
-}
-
 void
 MPU6000::check_registers(void)
 {
@@ -717,17 +710,16 @@ MPU6000::check_registers(void)
 	_checked_next = (_checked_next + 1) % MPU6000_NUM_CHECKED_REGISTERS;
 }
 
-int
-MPU6000::measure()
+void MPU6000::Run()
 {
 	if (_in_factory_test) {
 		// don't publish any data while in factory test mode
-		return OK;
+		return;
 	}
 
 	if (hrt_absolute_time() < _reset_wait) {
 		// we're waiting for a reset to complete
-		return OK;
+		return;
 	}
 
 	struct MPUReport mpu_report;
@@ -756,7 +748,8 @@ MPU6000::measure()
 	if (sizeof(mpu_report) != _interface->read(MPU6000_SET_SPEED(MPUREG_INT_STATUS, MPU6000_HIGH_BUS_SPEED),
 			(uint8_t *)&mpu_report, sizeof(mpu_report))) {
 
-		return -EIO;
+		perf_end(_sample_perf);
+		return;
 	}
 
 	check_registers();
@@ -774,9 +767,11 @@ MPU6000::measure()
 		perf_end(_sample_perf);
 		perf_count(_duplicates);
 		_got_duplicate = true;
-		return OK;
+		return;
 	}
 
+	perf_end(_sample_perf);
+
 	memcpy(&_last_accel[0], &mpu_report.accel_x[0], 6);
 	_got_duplicate = false;
 
@@ -804,20 +799,19 @@ MPU6000::measure()
 
 		// all zero data - probably a SPI bus error
 		perf_count(_bad_transfers);
-		perf_end(_sample_perf);
 
 		// note that we don't call reset() here as a reset()
 		// costs 20ms with interrupts disabled. That means if
 		// the mpu6k does go bad it would cause a FMU failure,
 		// regardless of whether another sensor is available,
-		return -EIO;
+		return;
 	}
 
 	if (_register_wait != 0) {
 		// we are waiting for some good transfers before using
 		// the sensor again, don't return any data yet
 		_register_wait--;
-		return OK;
+		return;
 	}
 
 
@@ -876,10 +870,6 @@ MPU6000::measure()
 
 	_px4_accel.update(timestamp_sample, report.accel_x, report.accel_y, report.accel_z);
 	_px4_gyro.update(timestamp_sample, report.gyro_x, report.gyro_y, report.gyro_z);
-
-	/* stop measuring */
-	perf_end(_sample_perf);
-	return OK;
 }
 
 void

src/drivers/imu/mpu6000/MPU6000.hpp

@@ -394,11 +394,6 @@ private:
 	 */
 	bool 		is_mpu_device() { return _device_type == MPU_DEVICE_TYPE_MPU6000; }
 
-	/**
-	 * Fetch measurements from the sensor and update the report buffers.
-	 */
-	int			measure();
-
 	/**
 	 * Read a register from the MPU6000
 	 *

src/drivers/imu/st/ism330dlc/ISM330DLC.cpp

@@ -53,7 +53,6 @@ ISM330DLC::ISM330DLC(int bus, uint32_t device, enum Rotation rotation) :
 	_px4_gyro.set_device_type(DRV_DEVTYPE_ST_ISM330DLC);
 
 	_px4_accel.set_sample_rate(ST_ISM330DLC::LA_ODR);
-	_px4_gyro.set_sample_rate(ST_ISM330DLC::G_ODR);
 
 	_px4_accel.set_update_rate(1000000 / FIFO_INTERVAL);
 	_px4_gyro.set_update_rate(1000000 / FIFO_INTERVAL);

src/drivers/imu/st/lsm9ds1/LSM9DS1.cpp

@@ -49,7 +49,6 @@ LSM9DS1::LSM9DS1(int bus, uint32_t device, enum Rotation rotation) :
 	_px4_gyro.set_device_type(DRV_IMU_DEVTYPE_ST_LSM9DS1_AG);
 
 	_px4_accel.set_sample_rate(ST_LSM9DS1::LA_ODR);
-	_px4_gyro.set_sample_rate(ST_LSM9DS1::G_ODR);
 
 	_px4_accel.set_update_rate(1000000 / _fifo_interval);
 	_px4_gyro.set_update_rate(1000000 / _fifo_interval);