diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 60c4fa5e6e..f135529e02 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -2199,7 +2199,7 @@ Sensors::task_main() /* this is undesirable but not much we can do - might want to flag unhappy status */ if (pret < 0) { - warnx("poll error %d, %d", pret, errno); + warnx("sens: poll error %d, %d", pret, errno); continue; } @@ -2215,17 +2215,21 @@ Sensors::task_main() mag_poll(raw); baro_poll(raw); +#ifndef __PX4_POSIX /* work out if main gyro timed out and fail over to alternate gyro */ - if (hrt_elapsed_time(&raw.gyro_timestamp[0]) > 20 * 1000) { + if (raw.gyro_timestamp[0] > 0 && hrt_elapsed_time(&raw.gyro_timestamp[0]) > 20 * 1000) { /* if the secondary failed as well, go to the tertiary */ - if (hrt_elapsed_time(&raw.gyro_timestamp[1]) > 20 * 1000) { + if (_gyro_sub[2] >= 0 && (hrt_elapsed_time(&raw.gyro_timestamp[1]) > 20 * 1000)) { fds[0].fd = _gyro_sub[2]; + warnx("failing over to third gyro"); - } else { + } else if (_gyro_sub[1] >= 0) { fds[0].fd = _gyro_sub[1]; + warnx("failing over to second gyro"); } } +#endif /* check battery voltage */ adc_poll(raw);