diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2013-11-03 18:26:39 +0100 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-11-03 18:26:39 +0100 |
| commit | 791695ccd008859f6abe1a12d86b7be2ba811fec (patch) | |
| tree | 579fb3b4e8071f2e6f675aa6bd50ce12f4a9915f | |
| parent | b53d86ed681eb6c9f979bb10d5f487fa9c94d81b (diff) | |
| download | px4-firmware-791695ccd008859f6abe1a12d86b7be2ba811fec.tar.gz px4-firmware-791695ccd008859f6abe1a12d86b7be2ba811fec.tar.bz2 px4-firmware-791695ccd008859f6abe1a12d86b7be2ba811fec.zip | |
Hotfix: Check for out of range accel values
| -rw-r--r-- | src/systemcmds/preflight_check/preflight_check.c | 28 |
1 files changed, 26 insertions, 2 deletions
diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index 1c58a2db6..07581459b 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -109,7 +109,7 @@ int preflight_check_main(int argc, char *argv[]) /* ---- ACCEL ---- */ close(fd); - fd = open(ACCEL_DEVICE_PATH, 0); + fd = open(ACCEL_DEVICE_PATH, O_RDONLY); ret = ioctl(fd, ACCELIOCSELFTEST, 0); if (ret != OK) { @@ -119,6 +119,29 @@ int preflight_check_main(int argc, char *argv[]) goto system_eval; } + /* check measurement result range */ + struct accel_report acc; + ret = read(fd, &acc, sizeof(acc)); + + if (ret == sizeof(acc)) { + /* evaluate values */ + if (sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z) > 30.0f /* m/s^2 */) { + warnx("accel with spurious values"); + mavlink_log_critical(mavlink_fd, "SENSOR FAIL: |ACCEL| > 30 m/s^2"); + /* this is frickin' fatal */ + fail_on_error = true; + system_ok = false; + goto system_eval; + } + } else { + warnx("accel read failed"); + mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACCEL READ"); + /* this is frickin' fatal */ + fail_on_error = true; + system_ok = false; + goto system_eval; + } + /* ---- GYRO ---- */ close(fd); @@ -193,9 +216,10 @@ system_eval: /* stop alarm */ ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE); - /* switch on leds */ + /* switch off leds */ led_on(leds, LED_BLUE); led_on(leds, LED_AMBER); + close(leds); if (fail_on_error) { /* exit with error message */ |