diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2013-05-05 15:51:16 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2013-05-05 15:51:16 +0400 |
commit | 1f800edc7676a6ea13127746ce38787a1e98b935 (patch) | |
tree | 5a3fa8eb618cc1d8bf813a3517d60d134c277ab0 /apps/commander | |
parent | 4109874fc84339e3ee8a794b17d8bdd131313c51 (diff) | |
download | px4-firmware-1f800edc7676a6ea13127746ce38787a1e98b935.tar.gz px4-firmware-1f800edc7676a6ea13127746ce38787a1e98b935.tar.bz2 px4-firmware-1f800edc7676a6ea13127746ce38787a1e98b935.zip |
Still threshold increased to 0.1m/s^2, and orientation error threshold to 5m/s^2. Timeout increased to 30s.
Diffstat (limited to 'apps/commander')
-rw-r--r-- | apps/commander/accelerometer_calibration.c | 66 |
1 files changed, 32 insertions, 34 deletions
diff --git a/apps/commander/accelerometer_calibration.c b/apps/commander/accelerometer_calibration.c index 180736908..991145d73 100644 --- a/apps/commander/accelerometer_calibration.c +++ b/apps/commander/accelerometer_calibration.c @@ -224,7 +224,7 @@ int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs[3], float * Wait for vehicle become still and detect it's orientation. * * @return 0..5 according to orientation when vehicle is still and ready for measurements, - * ERROR if vehicle is not still after 10s or orientation error is more than 20% + * ERROR if vehicle is not still after 30s or orientation error is more than 5m/s^2 */ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { struct sensor_combined_s sensor; @@ -235,17 +235,17 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { float accel_len2 = 0.0f; /* EMA time constant in seconds*/ float ema_len = 0.2f; - /* set "still" threshold to 0.005 m/s^2 */ - float still_thr2 = pow(0.05f / CONSTANTS_ONE_G, 2); - /* set accel error threshold to 30% of accel vector length */ - float accel_err_thr = 0.3f; + /* set "still" threshold to 0.1 m/s^2 */ + float still_thr2 = pow(0.1f, 2); + /* set accel error threshold to 5m/s^2 */ + float accel_err_thr = 5.0f; /* still time required in us */ int64_t still_time = 2000000; struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; hrt_abstime t_start = hrt_absolute_time(); - /* set deadline to 20s */ - hrt_abstime timeout = 20000000; + /* set timeout to 30s */ + hrt_abstime timeout = 30000000; hrt_abstime t_timeout = t_start + timeout; hrt_abstime t = t_start; hrt_abstime t_prev = t_start; @@ -267,11 +267,10 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { if (d > accel_disp[i]) accel_disp[i] = d; } - accel_len2 = accel_ema[0] * accel_ema[0] + accel_ema[1] * accel_ema[1] + accel_ema[2] * accel_ema[2]; - float still_thr_raw2 = still_thr2 * accel_len2; - if ( accel_disp[0] < still_thr_raw2 && - accel_disp[1] < still_thr_raw2 && - accel_disp[2] < still_thr_raw2 ) { + /* still detector with hysteresis */ + if ( accel_disp[0] < still_thr2 && + accel_disp[1] < still_thr2 && + accel_disp[2] < still_thr2 ) { /* is still now */ if (t_still == 0) { /* first time */ @@ -285,9 +284,9 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { break; } } - } else if ( accel_disp[0] > still_thr_raw2 * 2.0f || - accel_disp[1] > still_thr_raw2 * 2.0f || - accel_disp[2] > still_thr_raw2 * 2.0f) { + } else if ( accel_disp[0] > still_thr2 * 2.0f || + accel_disp[1] > still_thr2 * 2.0f || + accel_disp[2] > still_thr2 * 2.0f) { /* not still, reset still start time */ if (t_still != 0) { mavlink_log_info(mavlink_fd, "moving..."); @@ -306,30 +305,29 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { } float accel_len = sqrt(accel_len2); - float accel_err_thr_raw = accel_len * accel_err_thr; - if ( fabs(accel_ema[0] - accel_len) < accel_err_thr_raw && - fabs(accel_ema[1]) < accel_err_thr_raw && - fabs(accel_ema[2]) < accel_err_thr_raw ) + if ( fabs(accel_ema[0] - accel_len) < 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_raw && - fabs(accel_ema[1]) < accel_err_thr_raw && - fabs(accel_ema[2]) < accel_err_thr_raw ) + if ( fabs(accel_ema[0] + accel_len) < 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_raw && - fabs(accel_ema[1] - accel_len) < accel_err_thr_raw && - fabs(accel_ema[2]) < accel_err_thr_raw ) + if ( fabs(accel_ema[0]) < accel_err_thr && + fabs(accel_ema[1] - accel_len) < accel_err_thr && + fabs(accel_ema[2]) < accel_err_thr ) return 2; // [ 0, g, 0 ] - if ( fabs(accel_ema[0]) < accel_err_thr_raw && - fabs(accel_ema[1] + accel_len) < accel_err_thr_raw && - fabs(accel_ema[2]) < accel_err_thr_raw ) + if ( fabs(accel_ema[0]) < accel_err_thr && + fabs(accel_ema[1] + accel_len) < accel_err_thr && + fabs(accel_ema[2]) < accel_err_thr ) return 3; // [ 0, -g, 0 ] - if ( fabs(accel_ema[0]) < accel_err_thr_raw && - fabs(accel_ema[1]) < accel_err_thr_raw && - fabs(accel_ema[2] - accel_len) < accel_err_thr_raw ) + if ( fabs(accel_ema[0]) < accel_err_thr && + fabs(accel_ema[1]) < accel_err_thr && + fabs(accel_ema[2] - accel_len) < accel_err_thr ) return 4; // [ 0, 0, g ] - if ( fabs(accel_ema[0]) < accel_err_thr_raw && - fabs(accel_ema[1]) < accel_err_thr_raw && - fabs(accel_ema[2] + accel_len) < accel_err_thr_raw ) + if ( fabs(accel_ema[0]) < accel_err_thr && + fabs(accel_ema[1]) < accel_err_thr && + fabs(accel_ema[2] + accel_len) < accel_err_thr ) return 5; // [ 0, 0, -g ] mavlink_log_info(mavlink_fd, "ERROR: invalid orientation"); |