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

Hotfix: Check for out of range accel values

Browse Source
This commit is contained in:
Lorenz Meier
2013-11-03 18:26:39 +01:00
parent b53d86ed68
commit 791695ccd0
1 changed files with 26 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/systemcmds/preflight_check/preflight_check.c
+26 -2
View File
@@ -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 */