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

sensors app: logic fixed and cleanup 

- do not exit sensors app if sensor init failed
- do not spam console if we fail over first/second gyro

Signed-off-by: tumbili <roman@px4.io>
This commit is contained in:
tumbili 2016-06-27 14:06:01 +02:00 committed by Lorenz Meier
parent ee58188162
commit 7e282f579b
1 changed files with 6 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -2096,12 +2096,7 @@ Sensors::task_main()
#endif
if (ret) {
warnx("sensor initialization failed");
_sensors_task = -1;
DevMgr::releaseHandle(_h_adc);
return;
PX4_ERR("sensor initialization failed");
}
struct sensor_combined_s raw = {};
@ -2225,17 +2220,19 @@ Sensors::task_main()
/* Work out if main gyro timed out and fail over to alternate gyro.
* However, don't do this if the secondary is not available. */
if (hrt_elapsed_time(&raw.gyro_timestamp[0]) > 20 * 1000 && _gyro_sub[1] >= 0) {
warnx("gyro has timed out");
if (fds[0].fd == _gyro_sub[0]) {
PX4_WARN("gyro0 has timed out");
}
/* If the secondary failed as well, go to the tertiary, also only if available. */
if (hrt_elapsed_time(&raw.gyro_timestamp[1]) > 20 * 1000 && _gyro_sub[2] >= 0) {
if (hrt_elapsed_time(&raw.gyro_timestamp[1]) > 20 * 1000 && _gyro_sub[2] >= 0 && (fds[0].fd != _gyro_sub[2])) {
fds[0].fd = _gyro_sub[2];
if (!_hil_enabled) {
warnx("failing over to third gyro");
}
} else if (_gyro_sub[1] >= 0) {
} else if (_gyro_sub[1] >= 0 && (fds[0].fd != _gyro_sub[1])) {
fds[0].fd = _gyro_sub[1];
if (!_hil_enabled) {