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

sensors/vehicle_imu: add protections for bad sensor data

Browse Source 
- check for inconsistent timestamps, zero samples, and numerical issues in online mean
This commit is contained in:
Daniel Agar
2021-09-11 13:04:46 -04:00
parent 3c57c67ce3
commit 8866215d1d
1 changed files with 96 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/vehicle_imu/VehicleIMU.cpp
+96 -41
View File
@@ -237,35 +237,62 @@ bool VehicleIMU::UpdateAccel()
} else {
// collect sample interval average for filters
if (_accel_timestamp_sample_last != 0) {
if ((_accel_timestamp_sample_last != 0) && (accel.samples > 0)) {
float interval_us = accel.timestamp_sample - _accel_timestamp_sample_last;
_accel_interval_mean.update(Vector2f{interval_us, interval_us / accel.samples});
if (interval_us > 0.f) {
_accel_interval_mean.update(Vector2f{interval_us, interval_us / accel.samples});
} else {
PX4_ERR("%d - accel %" PRIu32 " timestamp error timestamp_sample: %" PRIu64 ", previous timestamp_sample: %" PRIu64,
_instance, accel.device_id, accel.timestamp_sample, _accel_timestamp_sample_last);
}
}
if (_accel_interval_mean.valid() && (_accel_interval_mean.count() > 100 || !PX4_ISFINITE(_accel_interval_best_variance))
&& ((_accel_interval_mean.variance()(0) < _accel_interval_best_variance) || (_accel_interval_mean.count() > 1000))) {
if (accel.timestamp < accel.timestamp_sample) {
PX4_ERR("%d - accel %" PRIu32 " timestamp (%" PRIu64 ") < timestamp_sample (%" PRIu64 ")",
_instance, accel.device_id, accel.timestamp, accel.timestamp_sample);
}
const int interval_count = _accel_interval_mean.count();
const float interval_variance = _accel_interval_mean.variance()(0);
// check measured interval periodically
if ((_accel_interval_mean.valid() && (interval_count % 10 == 0))
&& (!PX4_ISFINITE(_accel_interval_best_variance)
|| (interval_variance < _accel_interval_best_variance)
|| (interval_count > 1000))) {
const float interval_mean = _accel_interval_mean.mean()(0);
const float interval_mean_fifo = _accel_interval_mean.mean()(1);
// update sample rate if previously invalid or changed
const float interval_delta_us = fabsf(_accel_interval_mean.mean()(0) - _accel_interval_us);
const float interval_delta_us = fabsf(interval_mean - _accel_interval_us);
const float percent_changed = interval_delta_us / _accel_interval_us;
if (!PX4_ISFINITE(_accel_interval_us) || (percent_changed > 0.001f)) {
// update integrator configuration if interval has changed by more than 10%
if (interval_delta_us > 0.1f * _accel_interval_us) {
_update_integrator_config = true;
if (PX4_ISFINITE(interval_mean) && PX4_ISFINITE(interval_mean_fifo) && PX4_ISFINITE(interval_variance)) {
// update integrator configuration if interval has changed by more than 10%
if (interval_delta_us > 0.1f * _accel_interval_us) {
_update_integrator_config = true;
}
_accel_interval_us = interval_mean;
_accel_interval_best_variance = interval_variance;
_status.accel_rate_hz = 1e6f / interval_mean;
_status.accel_raw_rate_hz = 1e6f / interval_mean_fifo; // FIFO
_publish_status = true;
} else {
_accel_interval_mean.reset();
}
_accel_interval_us = _accel_interval_mean.mean()(0);
_accel_interval_best_variance = _accel_interval_mean.variance()(0);
_status.accel_rate_hz = 1e6f / _accel_interval_mean.mean()(0);
_status.accel_raw_rate_hz = 1e6f / _accel_interval_mean.mean()(1); // FIFO
_publish_status = true;
}
}
if (_accel_interval_mean.count() > 10000) {
// reset periodically to prevent numerical issues
_accel_interval_mean.reset();
}
if (interval_count > 10000) {
// reset periodically to prevent numerical issues
_accel_interval_mean.reset();
}
}
@@ -350,35 +377,62 @@ bool VehicleIMU::UpdateGyro()
} else {
// collect sample interval average for filters
if (_gyro_timestamp_sample_last != 0) {
float interval_us = gyro.timestamp_sample - _gyro_timestamp_sample_last;
_gyro_interval_mean.update(Vector2f{interval_us, interval_us / gyro.samples});
if ((_gyro_timestamp_sample_last != 0) && (gyro.samples > 0)) {
const float interval_us = gyro.timestamp_sample - _gyro_timestamp_sample_last;
if (interval_us > 0.f) {
_gyro_interval_mean.update(Vector2f{interval_us, interval_us / gyro.samples});
} else {
PX4_ERR("%d - gyro %" PRIu32 " timestamp error timestamp_sample: %" PRIu64 ", previous timestamp_sample: %" PRIu64,
_instance, gyro.device_id, gyro.timestamp_sample, _gyro_timestamp_sample_last);
}
}
if (_gyro_interval_mean.valid() && (_gyro_interval_mean.count() > 100 || !PX4_ISFINITE(_gyro_interval_best_variance))
&& ((_gyro_interval_mean.variance()(0) < _gyro_interval_best_variance) || (_gyro_interval_mean.count() > 1000))) {
if (gyro.timestamp < gyro.timestamp_sample) {
PX4_ERR("%d - gyro %" PRIu32 " timestamp (%" PRIu64 ") < timestamp_sample (%" PRIu64 ")",
_instance, gyro.device_id, gyro.timestamp, gyro.timestamp_sample);
}
const int interval_count = _gyro_interval_mean.count();
const float interval_variance = _gyro_interval_mean.variance()(0);
// check measured interval periodically
if ((_gyro_interval_mean.valid() && (interval_count % 10 == 0))
&& (!PX4_ISFINITE(_gyro_interval_best_variance)
|| (interval_variance < _gyro_interval_best_variance)
|| (interval_count > 1000))) {
const float interval_mean = _gyro_interval_mean.mean()(0);
const float interval_mean_fifo = _gyro_interval_mean.mean()(1);
// update sample rate if previously invalid or changed
const float interval_delta_us = fabsf(_gyro_interval_mean.mean()(0) - _gyro_interval_us);
const float interval_delta_us = fabsf(interval_mean - _gyro_interval_us);
const float percent_changed = interval_delta_us / _gyro_interval_us;
if (!PX4_ISFINITE(_gyro_interval_us) || (percent_changed > 0.001f)) {
// update integrator configuration if interval has changed by more than 10%
if (interval_delta_us > 0.1f * _gyro_interval_us) {
_update_integrator_config = true;
if (PX4_ISFINITE(interval_mean) && PX4_ISFINITE(interval_mean_fifo) && PX4_ISFINITE(interval_variance)) {
// update integrator configuration if interval has changed by more than 10%
if (interval_delta_us > 0.1f * _gyro_interval_us) {
_update_integrator_config = true;
}
_gyro_interval_us = interval_mean;
_gyro_interval_best_variance = interval_variance;
_status.gyro_rate_hz = 1e6f / interval_mean;
_status.gyro_raw_rate_hz = 1e6f / interval_mean_fifo; // FIFO
_publish_status = true;
} else {
_gyro_interval_mean.reset();
}
_gyro_interval_us = _gyro_interval_mean.mean()(0);
_gyro_interval_best_variance = _gyro_interval_mean.variance()(0);
_status.gyro_rate_hz = 1e6f / _gyro_interval_mean.mean()(0);
_status.gyro_raw_rate_hz = 1e6f / _gyro_interval_mean.mean()(1); // FIFO
_publish_status = true;
}
}
if (_gyro_interval_mean.count() > 10000) {
// reset periodically to prevent numerical issues
_gyro_interval_mean.reset();
}
if (interval_count > 10000) {
// reset periodically to prevent numerical issues
_gyro_interval_mean.reset();
}
}
@@ -560,9 +614,10 @@ void VehicleIMU::UpdateGyroVibrationMetrics(const Vector3f &angular_velocity)
void VehicleIMU::PrintStatus()
{
PX4_INFO("%" PRIu8 " - Accel ID: %" PRIu32 ", interval: %.1f us (SD %.1f us), Gyro ID: %" PRIu32
PX4_INFO("%" PRIu8 " - Accel: %" PRIu32 ", interval: %.1f us (SD %.1f us), Gyro: %" PRIu32
", interval: %.1f us (SD %.1f us)",
_instance, _accel_calibration.device_id(), (double)_accel_interval_us, (double)sqrtf(_accel_interval_best_variance),
_instance,
_accel_calibration.device_id(), (double)_accel_interval_us, (double)sqrtf(_accel_interval_best_variance),
_gyro_calibration.device_id(), (double)_gyro_interval_us, (double)sqrtf(_gyro_interval_best_variance));
PX4_DEBUG("gyro update mean sample latency: %.6f s, publish latency %.6f s, gyro interval %.6f s",