diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2013-05-06 23:50:23 +0200 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-05-06 23:50:23 +0200 |
| commit | 44df8db984b0fcedb246aec0c272b62babefc31f (patch) | |
| tree | 1112e9be17ada4c4b1a371480e35a598c6c0f3c1 /src/modules | |
| parent | eac9e10a83ab2f897e4d0c2c6c8cd9f9f55b29cb (diff) | |
| parent | 078ae23cfa00e2128d08d87dc015a3ca116f342e (diff) | |
| download | px4-firmware-44df8db984b0fcedb246aec0c272b62babefc31f.tar.gz px4-firmware-44df8db984b0fcedb246aec0c272b62babefc31f.tar.bz2 px4-firmware-44df8db984b0fcedb246aec0c272b62babefc31f.zip | |
Merge branch 'master' of github.com:PX4/Firmware into export-build
Diffstat (limited to 'src/modules')
| -rw-r--r-- | src/modules/commander/accelerometer_calibration.c | 14 | ||||
| -rw-r--r-- | src/modules/commander/accelerometer_calibration.h | 4 |
2 files changed, 8 insertions, 10 deletions
diff --git a/src/modules/commander/accelerometer_calibration.c b/src/modules/commander/accelerometer_calibration.c index 991145d73..d79dd93dd 100644 --- a/src/modules/commander/accelerometer_calibration.c +++ b/src/modules/commander/accelerometer_calibration.c @@ -232,7 +232,6 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { float accel_ema[3] = { 0.0f, 0.0f, 0.0f }; /* max-hold dispersion of accel */ float accel_disp[3] = { 0.0f, 0.0f, 0.0f }; - float accel_len2 = 0.0f; /* EMA time constant in seconds*/ float ema_len = 0.2f; /* set "still" threshold to 0.1 m/s^2 */ @@ -304,30 +303,29 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { } } - float accel_len = sqrt(accel_len2); - if ( fabs(accel_ema[0] - accel_len) < accel_err_thr && + if ( fabs(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr && fabs(accel_ema[1]) < accel_err_thr && fabs(accel_ema[2]) < accel_err_thr ) return 0; // [ g, 0, 0 ] - if ( fabs(accel_ema[0] + accel_len) < accel_err_thr && + if ( fabs(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr && fabs(accel_ema[1]) < accel_err_thr && fabs(accel_ema[2]) < accel_err_thr ) return 1; // [ -g, 0, 0 ] if ( fabs(accel_ema[0]) < accel_err_thr && - fabs(accel_ema[1] - accel_len) < accel_err_thr && + fabs(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr && fabs(accel_ema[2]) < accel_err_thr ) return 2; // [ 0, g, 0 ] if ( fabs(accel_ema[0]) < accel_err_thr && - fabs(accel_ema[1] + accel_len) < accel_err_thr && + fabs(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr && fabs(accel_ema[2]) < accel_err_thr ) return 3; // [ 0, -g, 0 ] if ( fabs(accel_ema[0]) < accel_err_thr && fabs(accel_ema[1]) < accel_err_thr && - fabs(accel_ema[2] - accel_len) < accel_err_thr ) + fabs(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr ) return 4; // [ 0, 0, g ] if ( fabs(accel_ema[0]) < accel_err_thr && fabs(accel_ema[1]) < accel_err_thr && - fabs(accel_ema[2] + accel_len) < accel_err_thr ) + fabs(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr ) return 5; // [ 0, 0, -g ] mavlink_log_info(mavlink_fd, "ERROR: invalid orientation"); diff --git a/src/modules/commander/accelerometer_calibration.h b/src/modules/commander/accelerometer_calibration.h index c0169c2a1..a11cf93d3 100644 --- a/src/modules/commander/accelerometer_calibration.h +++ b/src/modules/commander/accelerometer_calibration.h @@ -1,8 +1,8 @@ /* * accelerometer_calibration.h * - * Created on: 25.04.2013 - * Author: ton + * Copyright (C) 2013 Anton Babushkin. All rights reserved. + * Author: Anton Babushkin <rk3dov@gmail.com> */ #ifndef ACCELEROMETER_CALIBRATION_H_ |