dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

add IMU_GYRO_RATEMAX to optionally limit gyro control publication rate

This commit is contained in:
Daniel Agar 2019-09-02 20:06:50 -04:00 committed by GitHub
parent 49c74ab021
commit 4d9f2bf776
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 38 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
src/lib/drivers/gyroscope/PX4Gyroscope.cpp
View File

@ -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;

3
src/lib/drivers/gyroscope/PX4Gyroscope.hpp
View File

@ -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
)
};

24
src/modules/sensors/sensor_params.c
View File

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