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

sensors/vehicle_imu: sensor update loop limit iterations

This commit is contained in:
Daniel Agar 2024-03-04 13:07:40 -05:00
parent 8b6c70e0f2
commit 1c741836c0
1 changed files with 11 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -194,7 +194,12 @@ void VehicleIMU::Run()
// reset data gap monitor // reset data gap monitor
_data_gap = false; _data_gap = false;
while (_sensor_gyro_sub.updated() || _sensor_accel_sub.updated()) { int sensor_sub_updates = 0;
while ((_sensor_gyro_sub.updated() || _sensor_accel_sub.updated())
&& (sensor_sub_updates < math::max(sensor_accel_s::ORB_QUEUE_LENGTH, sensor_gyro_s::ORB_QUEUE_LENGTH))) {
sensor_sub_updates++;
bool updated = false; bool updated = false;
bool consume_all_gyro = !_intervals_configured || _data_gap; bool consume_all_gyro = !_intervals_configured || _data_gap;
@ -222,11 +227,16 @@ void VehicleIMU::Run()
// update accel until integrator ready and caught up to gyro // update accel until integrator ready and caught up to gyro
int sensor_accel_sub_updates = 0;
while (_sensor_accel_sub.updated() while (_sensor_accel_sub.updated()
&& (sensor_accel_sub_updates < sensor_accel_s::ORB_QUEUE_LENGTH)
&& (!_accel_integrator.integral_ready() || !_intervals_configured || _data_gap && (!_accel_integrator.integral_ready() || !_intervals_configured || _data_gap
|| (_accel_timestamp_sample_last < (_gyro_timestamp_sample_last - 0.5f * _accel_interval_us))) || (_accel_timestamp_sample_last < (_gyro_timestamp_sample_last - 0.5f * _accel_interval_us)))
) { ) {
sensor_accel_sub_updates++;
if (UpdateAccel()) { if (UpdateAccel()) {
updated = true; updated = true;
} }