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

sensors/vehicle_optical_flow: limit gyro updates and generation lost error per cycle 

- in the worst case scenario printing the error message can take longer than the next gyro publication, so combined with an infinite loop you could get stuck here
This commit is contained in:
Daniel Agar 2022-10-17 13:45:04 -04:00 committed by GitHub
parent 8545164869
commit 677f3e9294
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 9 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/modules/sensors/vehicle_optical_flow/VehicleOpticalFlow.cpp
View File

@ -435,15 +435,21 @@ void VehicleOpticalFlow::UpdateSensorGyro()
}
// buffer
while (_sensor_gyro_sub.updated()) {
const unsigned last_generation = _sensor_gyro_sub.get_last_generation();
bool sensor_gyro_lost_printed = false;
int gyro_updates = 0;
while (_sensor_gyro_sub.updated() && (gyro_updates < sensor_gyro_s::ORB_QUEUE_LENGTH)) {
gyro_updates++;
const unsigned last_generation = _sensor_gyro_sub.get_last_generation();
sensor_gyro_s sensor_gyro;
if (_sensor_gyro_sub.copy(&sensor_gyro)) {
if (_sensor_gyro_sub.get_last_generation() != last_generation + 1) {
PX4_ERR("sensor_gyro lost, generation %u -> %u", last_generation, _sensor_gyro_sub.get_last_generation());
if (!sensor_gyro_lost_printed) {
PX4_ERR("sensor_gyro lost, generation %u -> %u", last_generation, _sensor_gyro_sub.get_last_generation());
sensor_gyro_lost_printed = true;
}
}
_gyro_calibration.set_device_id(sensor_gyro.device_id);