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

Accelerometer calibration bugfix

This commit is contained in:
Anton Babushkin 2013-05-06 18:21:56 +04:00
parent 1733fce3df
commit 41ec41cf8c
2 changed files with 8 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
apps/commander/accelerometer_calibration.c
View File

@ -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");

4
apps/commander/accelerometer_calibration.h
View File

@ -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_