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

sensors/vehicle_angular_velocity: limit scheduling to IMU_GYRO_RATEMAX

This commit is contained in:
Daniel Agar 2020-06-01 10:14:59 -04:00
parent 96f043cd62
commit a9dacae57e
2 changed files with 19 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
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;
}

2
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp
View File

@ -103,6 +103,8 @@ private:
static constexpr const float kInitialRateHz{1000.0f}; /**< sensor update rate used for initialization */
float _update_rate_hz{kInitialRateHz}; /**< current rate-controller loop update rate in [Hz] */
uint8_t _required_sample_updates{0}; /**< number or sensor publications required for configured rate */
// angular velocity filters
math::LowPassFilter2pVector3f _lp_filter_velocity{kInitialRateHz, 30.0f};
math::NotchFilter<matrix::Vector3f> _notch_filter_velocity{};