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

sensors/vehicle_angular_velocity: sensor update loop limit iterations

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

10
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
View File

@ -822,9 +822,12 @@ void VehicleAngularVelocity::Run()
if (_fifo_available) { if (_fifo_available) {
// process all outstanding fifo messages // process all outstanding fifo messages
int sensor_sub_updates = 0;
sensor_gyro_fifo_s sensor_fifo_data; sensor_gyro_fifo_s sensor_fifo_data;
while (_sensor_gyro_fifo_sub.update(&sensor_fifo_data)) { while ((sensor_sub_updates < sensor_gyro_fifo_s::ORB_QUEUE_LENGTH) && _sensor_gyro_fifo_sub.update(&sensor_fifo_data)) {
sensor_sub_updates++;
const float inverse_dt_s = 1e6f / sensor_fifo_data.dt; const float inverse_dt_s = 1e6f / sensor_fifo_data.dt;
const int N = sensor_fifo_data.samples; const int N = sensor_fifo_data.samples;
static constexpr int FIFO_SIZE_MAX = sizeof(sensor_fifo_data.x) / sizeof(sensor_fifo_data.x[0]); static constexpr int FIFO_SIZE_MAX = sizeof(sensor_fifo_data.x) / sizeof(sensor_fifo_data.x[0]);
@ -863,9 +866,12 @@ void VehicleAngularVelocity::Run()
} else { } else {
// process all outstanding messages // process all outstanding messages
int sensor_sub_updates = 0;
sensor_gyro_s sensor_data; sensor_gyro_s sensor_data;
while (_sensor_sub.update(&sensor_data)) { while ((sensor_sub_updates < sensor_gyro_s::ORB_QUEUE_LENGTH) && _sensor_sub.update(&sensor_data)) {
sensor_sub_updates++;
if (Vector3f(sensor_data.x, sensor_data.y, sensor_data.z).isAllFinite()) { if (Vector3f(sensor_data.x, sensor_data.y, sensor_data.z).isAllFinite()) {
if (_timestamp_sample_last == 0 || (sensor_data.timestamp_sample <= _timestamp_sample_last)) { if (_timestamp_sample_last == 0 || (sensor_data.timestamp_sample <= _timestamp_sample_last)) {