fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-31 01:11:37 +08:00
Code Issues Packages Projects Releases Wiki Activity

sensors/vehicle_imu: vehicle_imu_status include accel/gyro full raw FIFO sample rate

Browse Source
This commit is contained in:
Daniel Agar
2021-02-24 08:13:53 -05:00
committed by GitHub
parent cec31fd685
commit 5f3e883f2c
9 changed files with 40 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/vehicle_imu/VehicleIMU.cpp
+18 -12
View File
@@ -141,50 +141,54 @@ void VehicleIMU::ParametersUpdate(bool force)
}
}
bool VehicleIMU::UpdateIntervalAverage(IntervalAverage &intavg, const hrt_abstime &timestamp_sample)
bool VehicleIMU::UpdateIntervalAverage(IntervalAverage &intavg, const hrt_abstime &timestamp_sample, uint8_t samples)
{
bool updated = false;
// conservative maximum time between samples to reject large gaps and reset averaging
float max_interval_us = 10000; // 100 Hz
float min_interval_us = 100; // 10,000 Hz
uint32_t max_interval_us = 10000; // 100 Hz
uint32_t min_interval_us = 100; // 10,000 Hz
if (intavg.update_interval > 0) {
if (intavg.update_interval > 0.f) {
// if available use previously calculated interval as bounds
max_interval_us = 1.5f * intavg.update_interval;
min_interval_us = 0.5f * intavg.update_interval;
max_interval_us = roundf(1.5f * intavg.update_interval);
min_interval_us = roundf(0.5f * intavg.update_interval);
}
const float interval_us = (timestamp_sample - intavg.timestamp_sample_last);
const uint32_t interval_us = (timestamp_sample - intavg.timestamp_sample_last);
if ((intavg.timestamp_sample_last > 0) && (interval_us < max_interval_us) && (interval_us > min_interval_us)) {
intavg.interval_sum += interval_us;
intavg.interval_samples += samples;
intavg.interval_count++;
// periodically calculate sensor update rate
if (intavg.interval_count > 10000 || ((intavg.update_interval <= FLT_EPSILON) && intavg.interval_count > 100)) {
const float sample_interval_avg = intavg.interval_sum / intavg.interval_count;
const float sample_interval_avg = (float)intavg.interval_sum / (float)intavg.interval_count;
if (PX4_ISFINITE(sample_interval_avg) && (sample_interval_avg > 0.f)) {
// update if interval has changed by more than 0.5%
if ((fabsf(intavg.update_interval - sample_interval_avg) / intavg.update_interval) > 0.005f) {
intavg.update_interval = sample_interval_avg;
intavg.update_interval_raw = (float)intavg.interval_sum / (float)intavg.interval_samples;
updated = true;
}
}
// reset sample interval accumulator
intavg.interval_sum = 0.f;
intavg.interval_count = 0.f;
intavg.interval_samples = 0;
intavg.interval_count = 0;
}
} else {
// reset
intavg.interval_sum = 0.f;
intavg.interval_count = 0.f;
intavg.interval_samples = 0;
intavg.interval_count = 0;
}
intavg.timestamp_sample_last = timestamp_sample;
@@ -222,10 +226,11 @@ void VehicleIMU::Run()
} else {
// collect sample interval average for filters
if (!_intervals_configured && UpdateIntervalAverage(_gyro_interval, gyro.timestamp_sample)) {
if (!_intervals_configured && UpdateIntervalAverage(_gyro_interval, gyro.timestamp_sample, gyro.samples)) {
update_integrator_config = true;
publish_status = true;
_status.gyro_rate_hz = 1e6f / _gyro_interval.update_interval;
_status.gyro_raw_rate_hz = 1e6f / _gyro_interval.update_interval_raw;
}
}
@@ -268,10 +273,11 @@ void VehicleIMU::Run()
} else {
// collect sample interval average for filters
if (!_intervals_configured && UpdateIntervalAverage(_accel_interval, accel.timestamp_sample)) {
if (!_intervals_configured && UpdateIntervalAverage(_accel_interval, accel.timestamp_sample, accel.samples)) {
update_integrator_config = true;
publish_status = true;
_status.accel_rate_hz = 1e6f / _accel_interval.update_interval;
_status.accel_raw_rate_hz = 1e6f / _accel_interval.update_interval_raw;
}
}
src/modules/sensors/vehicle_imu/VehicleIMU.hpp
+5 -3
View File
@@ -77,12 +77,14 @@ private:
struct IntervalAverage {
hrt_abstime timestamp_sample_last{0};
float interval_sum{0.f};
float interval_count{0.f};
uint32_t interval_sum{0};
uint32_t interval_samples{0};
uint32_t interval_count{0};
float update_interval{0.f};
float update_interval_raw{0.f};
};
bool UpdateIntervalAverage(IntervalAverage &intavg, const hrt_abstime &timestamp_sample);
bool UpdateIntervalAverage(IntervalAverage &intavg, const hrt_abstime &timestamp_sample, uint8_t samples = 1);
void UpdateIntegratorConfiguration();
void UpdateGyroVibrationMetrics(const matrix::Vector3f &delta_angle);
void UpdateAccelVibrationMetrics(const matrix::Vector3f &delta_velocity);