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

sensors/vehicle_imu: continue integrating queued raw data if falling behind

This commit is contained in:
Daniel Agar 2020-10-10 12:30:26 -04:00 committed by GitHub
parent f26d4f2b6b
commit dfbaf6607a
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 8 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
src/modules/sensors/vehicle_imu/VehicleIMU.cpp
View File

@ -133,6 +133,8 @@ void VehicleIMU::ParametersUpdate(bool force)
_param_imu_integ_rate.commit_no_notification();
}
_imu_integration_interval_us = 1000000 / imu_integration_rate_hz;
if (_param_imu_integ_rate.get() != imu_integ_rate_prev) {
// force update
UpdateIntegratorConfiguration();
@ -245,7 +247,10 @@ void VehicleIMU::Run()
_gyro_integrator.put(gyro.timestamp_sample, gyro_raw);
_last_timestamp_sample_gyro = gyro.timestamp_sample;
if (!sensor_data_gap && _intervals_configured && _gyro_integrator.integral_ready()) {
// break if interval is configured and we haven't fallen behind
if (_intervals_configured && _gyro_integrator.integral_ready()
&& (hrt_elapsed_time(&gyro.timestamp) < _imu_integration_interval_us) && !sensor_data_gap) {
break;
}
}

2
src/modules/sensors/vehicle_imu/VehicleIMU.hpp
View File

@ -100,6 +100,8 @@ private:
hrt_abstime _last_timestamp_sample_accel{0};
hrt_abstime _last_timestamp_sample_gyro{0};
uint32_t _imu_integration_interval_us{4000};
IntervalAverage _accel_interval{};
IntervalAverage _gyro_interval{};