diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp b/src/lib/drivers/gyroscope/PX4Gyroscope.cpp index b8b6ebf167..07a11d07c9 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.cpp +++ b/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); - control.timestamp = hrt_absolute_time(); - _sensor_gyro_control_pub.update(); // publish + + if (_param_imu_gyro_rate_max.get() > 0) { + const uint64_t interval = 1e6f / _param_imu_gyro_rate_max.get(); + + 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; diff --git a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp index 97f9d78be3..7ee7bffbde 100644 --- a/src/lib/drivers/gyroscope/PX4Gyroscope.hpp +++ b/src/lib/drivers/gyroscope/PX4Gyroscope.hpp @@ -85,7 +85,8 @@ private: unsigned _sample_rate{1000}; DEFINE_PARAMETERS( - (ParamFloat) _param_imu_gyro_cutoff + (ParamFloat) _param_imu_gyro_cutoff, + (ParamInt) _param_imu_gyro_rate_max ) }; diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index f246fa30a9..225b2e3723 100644 --- a/src/modules/sensors/sensor_params.c +++ b/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 -* is currently supported by the mpu6000 and mpu9250. This only affects the signal sent to the -* controllers, not the estimators. 0 disables the filter. +* The cutoff frequency for the 2nd order butterworth filter on the gyro driver. +* This only affects the signal sent to the 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 -* is currently supported by the mpu6000 and mpu9250. This only affects the signal sent to the -* controllers, not the estimators. 0 disables the filter. +* The cutoff frequency for the 2nd order butterworth filter on the accel driver. +* This only affects the signal sent to the controllers, not the estimators. 0 disables the filter. * * @min 0 * @max 1000