fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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);