fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-28 09:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Sensors: Do not fail over to non-existent gyro sub. Do not fail over on POSIX / sim

Browse Source
This commit is contained in:
Lorenz Meier
2016-02-05 00:52:12 +01:00
parent fdcfb7c7c6
commit 4990272b19
1 changed files with 8 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/sensors/sensors.cpp
+8 -4
View File
@@ -2199,7 +2199,7 @@ Sensors::task_main()
/* this is undesirable but not much we can do - might want to flag unhappy status */ /* this is undesirable but not much we can do - might want to flag unhappy status */
if (pret < 0) { if (pret < 0) {
warnx("poll error %d, %d", pret, errno); warnx("sens: poll error %d, %d", pret, errno);
continue; continue;
} }
@@ -2215,17 +2215,21 @@ Sensors::task_main()
mag_poll(raw); mag_poll(raw);
baro_poll(raw); baro_poll(raw);
#ifndef __PX4_POSIX
/* work out if main gyro timed out and fail over to alternate gyro */ /* 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 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]; 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]; fds[0].fd = _gyro_sub[1];
warnx("failing over to second gyro");
} }
} }
#endif
/* check battery voltage */ /* check battery voltage */
adc_poll(raw); adc_poll(raw);