fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 16:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

sensors/vehicle_angular_velocity: limit scheduling to IMU_GYRO_RATEMAX

Browse Source
This commit is contained in:
Daniel Agar
2020-06-01 15:08:50 -04:00
parent 96f043cd62
commit a9dacae57e
2 changed files with 19 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
+17
View File
@@ -103,6 +103,22 @@ void VehicleAngularVelocity::CheckFilters()
if ((fabsf(_update_rate_hz - _filter_sample_rate) / _filter_sample_rate) > 0.01f) {
reset_filters = true;
}
if (reset_filters || (_required_sample_updates == 0)) {
if (_param_imu_gyro_rate_max.get() > 0) {
// determine number of sensor samples that will get closest to the desired rate
const float configured_interval_us = 1e6f / _param_imu_gyro_rate_max.get();
const uint8_t samples = math::constrain(roundf(configured_interval_us / sample_interval_avg), 1.f,
(float)sensor_gyro_s::ORB_QUEUE_LENGTH);
_sensor_sub[_selected_sensor_sub_index].set_required_updates(samples);
_required_sample_updates = samples;
} else {
_sensor_sub[_selected_sensor_sub_index].set_required_updates(1);
_required_sample_updates = 1;
}
}
}
if (!reset_filters) {
@@ -190,6 +206,7 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(bool force)
// reset sample interval accumulator on sensor change
_timestamp_sample_last = 0;
_required_sample_updates = 0;
return true;
}