From 4990272b19e3569e6e3ed4fbe9f2f6eea4bf080f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 5 Feb 2016 00:52:12 +0100 Subject: [PATCH] Sensors: Do not fail over to non-existent gyro sub. Do not fail over on POSIX / sim --- src/modules/sensors/sensors.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) 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);