aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorLorenz Meier <lm@inf.ethz.ch>2013-05-06 07:57:43 -0700
committerLorenz Meier <lm@inf.ethz.ch>2013-05-06 07:57:43 -0700
commit012a0bbc0d8b0880fd4cba7e1c7ee5a208101717 (patch)
treebdc7b8d91997fc534dae10eb72f7083dfc945274
parent1733fce3df71ae3d49d53ff4f62583a8db78f38b (diff)
parent41ec41cf8cc16309cf6f7e949d3ddad78e5f44a2 (diff)
downloadpx4-firmware-012a0bbc0d8b0880fd4cba7e1c7ee5a208101717.tar.gz
px4-firmware-012a0bbc0d8b0880fd4cba7e1c7ee5a208101717.tar.bz2
px4-firmware-012a0bbc0d8b0880fd4cba7e1c7ee5a208101717.zip
Merge pull request #262 from DrTon/accel_calibration_fix
Accelerometer calibration bugfix
-rw-r--r--apps/commander/accelerometer_calibration.c14
-rw-r--r--apps/commander/accelerometer_calibration.h4
2 files changed, 8 insertions, 10 deletions
diff --git a/apps/commander/accelerometer_calibration.c b/apps/commander/accelerometer_calibration.c
index 991145d73..d79dd93dd 100644
--- a/apps/commander/accelerometer_calibration.c
+++ b/apps/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/apps/commander/accelerometer_calibration.h b/apps/commander/accelerometer_calibration.h
index c0169c2a1..a11cf93d3 100644
--- a/apps/commander/accelerometer_calibration.h
+++ b/apps/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_