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

sensors/vehicle_imu: fix timestamp_sample increasing check

Browse Source
This commit is contained in:
Daniel Agar
2021-10-13 19:49:49 -04:00
parent 4535b18a80
commit ef4d4c3093
1 changed files with 14 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/vehicle_imu/VehicleIMU.cpp
+14 -16
View File
@@ -238,16 +238,15 @@ bool VehicleIMU::UpdateAccel()
} else {
// collect sample interval average for filters
if ((_accel_timestamp_sample_last != 0) && (accel.samples > 0)) {
float interval_us = accel.timestamp_sample - _accel_timestamp_sample_last;
if (interval_us > 0.f) {
if (accel.timestamp_sample > _accel_timestamp_sample_last) {
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});
} 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);
}
} 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.timestamp < accel.timestamp_sample) {
@@ -384,16 +383,15 @@ bool VehicleIMU::UpdateGyro()
} else {
// collect sample interval average for filters
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) {
if (gyro.timestamp_sample > _gyro_timestamp_sample_last) {
if ((_gyro_timestamp_sample_last != 0) && (gyro.samples > 0)) {
float interval_us = gyro.timestamp_sample - _gyro_timestamp_sample_last;
_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);
}
} 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.timestamp < gyro.timestamp_sample) {