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

sensors: filter sample rate calculate with simple interval average instead of perf count

Browse Source 
- the perf counter intervals aren't numerically stable over extended periods (https://github.com/PX4/Firmware/pull/14046)
This commit is contained in:
Daniel Agar
2020-02-07 13:04:02 -05:00
committed by GitHub
parent a3710fcdef
commit 8df22541ef
4 changed files with 56 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
+24 -17
View File
@@ -55,8 +55,6 @@ VehicleAngularVelocity::VehicleAngularVelocity() :
VehicleAngularVelocity::~VehicleAngularVelocity()
{
Stop();
perf_free(_interval_perf);
}
bool VehicleAngularVelocity::Start()
@@ -88,28 +86,27 @@ void VehicleAngularVelocity::Stop()
void VehicleAngularVelocity::CheckFilters()
{
if ((hrt_elapsed_time(&_filter_check_last) > 100_ms)) {
_filter_check_last = hrt_absolute_time();
// check filter periodically (roughly once every 1-3 seconds depending on sensor configuration)
if (_interval_count > 2500) {
bool sample_rate_changed = false;
// calculate sensor update rate
const float sample_interval_avg = perf_mean(_interval_perf);
const float sample_interval_avg = _interval_sum / _interval_count;
if (PX4_ISFINITE(sample_interval_avg) && (sample_interval_avg > 0.0f)) {
const float update_rate_hz = 1.0f / sample_interval_avg;
const float update_rate_hz = 1.e6f / sample_interval_avg;
if ((fabsf(update_rate_hz) > 0.0f) && PX4_ISFINITE(update_rate_hz)) {
_update_rate_hz = update_rate_hz;
// check if sample rate error is greater than 1%
if ((fabsf(_update_rate_hz - _filter_sample_rate) / _filter_sample_rate) > 0.01f) {
++_sample_rate_incorrect_count;
sample_rate_changed = true;
}
}
}
const bool sample_rate_updated = (_sample_rate_incorrect_count > 50);
const bool lp_velocity_updated = (fabsf(_lp_filter_velocity.get_cutoff_freq() - _param_imu_gyro_cutoff.get()) > 0.01f);
const bool notch_updated = ((fabsf(_notch_filter_velocity.getNotchFreq() - _param_imu_gyro_nf_freq.get()) > 0.01f)
|| (fabsf(_notch_filter_velocity.getBandwidth() - _param_imu_gyro_nf_bw.get()) > 0.01f));
@@ -117,8 +114,8 @@ void VehicleAngularVelocity::CheckFilters()
const bool lp_acceleration_updated = (fabsf(_lp_filter_acceleration.get_cutoff_freq() - _param_imu_dgyro_cutoff.get()) >
0.01f);
if (sample_rate_updated || lp_velocity_updated || notch_updated || lp_acceleration_updated) {
PX4_INFO("updating filter, sample rate: %.3f Hz -> %.3f Hz", (double)_filter_sample_rate, (double)_update_rate_hz);
if (sample_rate_changed || lp_velocity_updated || notch_updated || lp_acceleration_updated) {
PX4_DEBUG("resetting filters, sample rate: %.3f Hz -> %.3f Hz", (double)_filter_sample_rate, (double)_update_rate_hz);
_filter_sample_rate = _update_rate_hz;
// update software low pass filters
@@ -130,10 +127,10 @@ void VehicleAngularVelocity::CheckFilters()
_lp_filter_acceleration.set_cutoff_frequency(_filter_sample_rate, _param_imu_dgyro_cutoff.get());
_lp_filter_acceleration.reset(_angular_acceleration_prev);
// reset state
_sample_rate_incorrect_count = 0;
}
// reset sample interval accumulator
_timestamp_sample_last = 0;
}
}
@@ -182,8 +179,8 @@ bool VehicleAngularVelocity::SensorSelectionUpdate(bool force)
_corrections.set_device_id(report.device_id);
// reset sample rate monitor
_sample_rate_incorrect_count = 0;
// reset sample interval accumulator on sensor change
_timestamp_sample_last = 0;
return true;
}
@@ -233,7 +230,17 @@ void VehicleAngularVelocity::Run()
if (_sensor_sub[_selected_sensor_sub_index].copy(&sensor_data)) {
if (sensor_updated) {
perf_count_interval(_interval_perf, sensor_data.timestamp_sample);
// collect sample interval average for filters
if ((_timestamp_sample_last > 0) && (sensor_data.timestamp_sample > _timestamp_sample_last)) {
_interval_sum += (sensor_data.timestamp_sample - _timestamp_sample_last);
_interval_count++;
} else {
_interval_sum = 0.f;
_interval_count = 0.f;
}
_timestamp_sample_last = sensor_data.timestamp_sample;
}
// Guard against too small (< 0.2ms) and too large (> 20ms) dt's.