add IMU_GYRO_RATEMAX to optionally limit gyro control publication rate

src/lib/drivers/gyroscope/PX4Gyroscope.cpp

@@ -123,12 +123,26 @@ PX4Gyroscope::update(hrt_abstime timestamp, float x, float y, float z)
 	// Filtered values
 	const matrix::Vector3f val_filtered{_filter.apply(val_calibrated)};
 
+
 	// publish control data (filtered gyro) immediately
+	bool publish_control = true;
 	sensor_gyro_control_s &control = _sensor_gyro_control_pub.get();
-	control.timestamp_sample = timestamp;
+
-	val_filtered.copyTo(control.xyz);
+	if (_param_imu_gyro_rate_max.get() > 0) {
-	control.timestamp = hrt_absolute_time();
+		const uint64_t interval = 1e6f / _param_imu_gyro_rate_max.get();
-	_sensor_gyro_control_pub.update();	// publish
+
+		if (hrt_elapsed_time(&control.timestamp_sample) < interval) {
+			publish_control = false;
+		}
+	}
+
+	if (publish_control) {
+		control.timestamp_sample = timestamp;
+		val_filtered.copyTo(control.xyz);
+		control.timestamp = hrt_absolute_time();
+		_sensor_gyro_control_pub.update();	// publish
+	}
+
 
 	// Integrated values
 	matrix::Vector3f integrated_value;

src/lib/drivers/gyroscope/PX4Gyroscope.hpp

@@ -85,7 +85,8 @@ private:
 	unsigned		_sample_rate{1000};
 
 	DEFINE_PARAMETERS(
-		(ParamFloat<px4::params::IMU_GYRO_CUTOFF>) _param_imu_gyro_cutoff
+		(ParamFloat<px4::params::IMU_GYRO_CUTOFF>) _param_imu_gyro_cutoff,
+		(ParamInt<px4::params::IMU_GYRO_RATEMAX>) _param_imu_gyro_rate_max
 	)
 
 };

src/modules/sensors/sensor_params.c

@@ -213,9 +213,8 @@ PARAM_DEFINE_INT32(SENS_EN_THERMAL, -1);
 /**
 * Driver level cutoff frequency for gyro
 *
-* The cutoff frequency for the 2nd order butterworth filter on the gyro driver. This features
+* The cutoff frequency for the 2nd order butterworth filter on the gyro driver.
-* is currently supported by the mpu6000 and mpu9250. This only affects the signal sent to the
+* This only affects the signal sent to the controllers, not the estimators. 0 disables the filter.
-* controllers, not the estimators. 0 disables the filter.
 *
 * @min 0
 * @max 1000
@@ -225,12 +224,25 @@ PARAM_DEFINE_INT32(SENS_EN_THERMAL, -1);
 */
 PARAM_DEFINE_FLOAT(IMU_GYRO_CUTOFF, 30.0f);
 
+/**
+* Gyro control data maximum publication rate
+*
+* This is the maximum rate the gyro control data (sensor_gyro_control) will be allowed to publish at.
+* Set to 0 to disable and publish at the native sensor sample rate.
+*
+* @min 0
+* @max 2000
+* @unit Hz
+* @reboot_required true
+* @group Sensors
+*/
+PARAM_DEFINE_INT32(IMU_GYRO_RATEMAX, 0);
+
 /**
 * Driver level cutoff frequency for accel
 *
-* The cutoff frequency for the 2nd order butterworth filter on the accel driver. This features
+* The cutoff frequency for the 2nd order butterworth filter on the accel driver.
-* is currently supported by the mpu6000 and mpu9250. This only affects the signal sent to the
+* This only affects the signal sent to the controllers, not the estimators. 0 disables the filter.
-* controllers, not the estimators. 0 disables the filter.
 *
 * @min 0
 * @max 1000