From e37f471ac4ce52e724b0d89714d9db6e1d16ee8e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 25 Apr 2013 23:59:46 +0400 Subject: [PATCH 1/5] 6-point accelerometer calibration implemented --- apps/commander/accelerometer_calibration.c | 414 +++++++++++++++++++++ apps/commander/accelerometer_calibration.h | 19 + apps/commander/commander.c | 125 +------ 3 files changed, 435 insertions(+), 123 deletions(-) create mode 100644 apps/commander/accelerometer_calibration.c create mode 100644 apps/commander/accelerometer_calibration.h diff --git a/apps/commander/accelerometer_calibration.c b/apps/commander/accelerometer_calibration.c new file mode 100644 index 0000000000..f950398815 --- /dev/null +++ b/apps/commander/accelerometer_calibration.c @@ -0,0 +1,414 @@ +/* + * accelerometer_calibration.c + * + * Copyright (C) 2013 Anton Babushkin. All rights reserved. + * Author: Anton Babushkin + * + * Transform acceleration vector to true orientation and scale + * + * * * * Model * * * + * accel_corr = accel_T * (accel_raw - accel_offs) + * + * accel_corr[3] - fully corrected acceleration vector in body frame + * accel_T[3][3] - accelerometers transform matrix, rotation and scaling transform + * accel_raw[3] - raw acceleration vector + * accel_offs[3] - acceleration offset vector + * + * * * * Calibration * * * + * + * Reference vectors + * accel_corr_ref[6][3] = [ g 0 0 ] // nose up + * | -g 0 0 | // nose down + * | 0 g 0 | // left side down + * | 0 -g 0 | // right side down + * | 0 0 g | // on back + * [ 0 0 -g ] // level + * accel_raw_ref[6][3] + * + * accel_corr_ref[i] = accel_T * (accel_raw_ref[i] - accel_offs), i = 0...5 + * + * 6 reference vectors * 3 axes = 18 equations + * 9 (accel_T) + 3 (accel_offs) = 12 unknown constants + * + * Find accel_offs + * + * accel_offs[i] = (accel_raw_ref[i*2][i] + accel_raw_ref[i*2+1][i]) / 2 + * + * + * Find accel_T + * + * 9 unknown constants + * need 9 equations -> use 3 of 6 measurements -> 3 * 3 = 9 equations + * + * accel_corr_ref[i*2] = accel_T * (accel_raw_ref[i*2] - accel_offs), i = 0...2 + * + * Solve separate system for each row of accel_T: + * + * accel_corr_ref[j*2][i] = accel_T[i] * (accel_raw_ref[j*2] - accel_offs), j = 0...2 + * + * A * x = b + * + * x = [ accel_T[0][i] ] + * | accel_T[1][i] | + * [ accel_T[2][i] ] + * + * b = [ accel_corr_ref[0][i] ] // One measurement per axis is enough + * | accel_corr_ref[2][i] | + * [ accel_corr_ref[4][i] ] + * + * a[i][j] = accel_raw_ref[i][j] - accel_offs[j], i = 0;2;4, j = 0...2 + * + * Matrix A is common for all three systems: + * A = [ a[0][0] a[0][1] a[0][2] ] + * | a[2][0] a[2][1] a[2][2] | + * [ a[4][0] a[4][1] a[4][2] ] + * + * x = A^-1 * b + * + * accel_T = A^-1 * g + * g = 9.80665 + */ + +#include "accelerometer_calibration.h" + +#include +#include +#include +#include +#include +#include + +void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd) { + /* announce change */ + mavlink_log_info(mavlink_fd, "accelerometer calibration started"); + /* set to accel calibration mode */ + status->flag_preflight_accel_calibration = true; + state_machine_publish(status_pub, status, mavlink_fd); + + float accel_offs_scaled[3]; + float accel_scale[3]; + int res = do_accel_calibration_mesurements(mavlink_fd, accel_offs_scaled, accel_scale); + if (res == OK) { + /* measurements complete successfully, set parameters */ + if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offs_scaled[0])) + || param_set(param_find("SENS_ACC_YOFF"), &(accel_offs_scaled[1])) + || param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs_scaled[2])) + || param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale[0])) + || param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale[1])) + || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale[2]))) { + mavlink_log_critical(mavlink_fd, "Setting offs or scale failed!"); + } + + int fd = open(ACCEL_DEVICE_PATH, 0); + struct accel_scale ascale = { + accel_offs_scaled[0], + accel_scale[0], + accel_offs_scaled[1], + accel_scale[1], + accel_offs_scaled[2], + accel_scale[2], + }; + + if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale)) + warn("WARNING: failed to set scale / offsets for accel"); + + close(fd); + + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + warn("WARNING: auto-save of params to storage failed"); + } + + mavlink_log_info(mavlink_fd, "accel calibration done"); + + tune_confirm(); + sleep(2); + tune_confirm(); + sleep(2); + /* third beep by cal end routine */ + } else { + /* measurements error */ + mavlink_log_info(mavlink_fd, "accel calibration aborted"); + tune_error(); + sleep(2); + } + + /* exit accel calibration mode */ + status->flag_preflight_accel_calibration = false; + state_machine_publish(status_pub, status, mavlink_fd); +} + +int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs_scaled[3], float accel_scale[3]) { + const int samples_num = 2500; + + int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); + + int16_t accel_raw_ref[6][3]; + bool data_collected[6] = { false, false, false, false, false, false }; + const char *orientation_strs[6] = { "x+", "x-", "y+", "y-", "z+", "z-" }; + while (true) { + bool done = true; + char str[80]; + int str_ptr; + str_ptr = sprintf(str, "keep vehicle still:"); + for (int i = 0; i < 6; i++) { + if (!data_collected[i]) { + str_ptr += sprintf(&(str[str_ptr]), " %s", orientation_strs[i]); + done = false; + } + } + if (done) { + mavlink_log_info(mavlink_fd, "all accel measurements complete"); + break; + } else { + mavlink_log_info(mavlink_fd, str); + int orient = detect_orientation(mavlink_fd, sensor_combined_sub); + if (orient < 0) { + sprintf(str, "orientation detection error: %i", orient); + mavlink_log_info(mavlink_fd, str); + return ERROR; + } + mavlink_log_info(mavlink_fd, "accel measurement started"); + read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[orient][0]), samples_num); + //mavlink_log_info(mavlink_fd, "accel measurement complete"); + str_ptr = sprintf(str, "complete: %i [ %i %i %i ]", orient, accel_raw_ref[orient][0], accel_raw_ref[orient][1], accel_raw_ref[orient][2]); + mavlink_log_info(mavlink_fd, str); + data_collected[orient] = true; + tune_confirm(); + } + } + close(sensor_combined_sub); + + /* calculate offsets and rotation+scale matrix */ + int16_t accel_offs[3]; + float accel_T[3][3]; + int res = calculate_calibration_values(accel_raw_ref, accel_T, accel_offs, CONSTANTS_ONE_G); + if (res != 0) { + mavlink_log_info(mavlink_fd, "calibration values calculation error"); + return ERROR; + } + + char str[80]; + sprintf(str, "accel offsets: [ %i %i %i ]", accel_offs[0], accel_offs[1], accel_offs[2]); + mavlink_log_info(mavlink_fd, str); + //mavlink_log_info(mavlink_fd, "accel transform matrix:"); + for (int i = 0; i < 3; i++) { + //sprintf(str, "\t[ %0.6f %0.6f %0.6f ]", accel_T[i][0], accel_T[i][1], accel_T[i][2]); + //mavlink_log_info(mavlink_fd, str); + } + + /* convert raw accel offset to scaled and transform matrix to scales + * rotation part of transform matrix is not used by now */ + for (int i = 0; i < 3; i++) { + accel_offs_scaled[i] = accel_offs[i] * accel_T[i][i]; + accel_scale[i] = accel_T[i][i]; + } + + return OK; +} + +/* + * 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% + */ +int detect_orientation(int mavlink_fd, int sub_sensor_combined) { + struct sensor_combined_s sensor; + /* exponential moving average of accel */ + 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.005 m/s^2 */ + float still_thr2 = pow(0.05f / CONSTANTS_ONE_G, 2); + /* set accel error threshold to 20% of accel vector length */ + float accel_err_thr = 0.2f; + /* 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; + hrt_abstime t_timeout = t_start + timeout; + hrt_abstime t = t_start; + hrt_abstime t_prev = t_start; + hrt_abstime t_still = 0; + while (true) { + /* wait blocking for new data */ + int poll_ret = poll(fds, 1, 1000); + if (poll_ret) { + orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &sensor); + t = hrt_absolute_time(); + float dt = (t - t_prev) / 1000000.0f; + t_prev = t; + float w = dt / ema_len; + for (int i = 0; i < 3; i++) { + accel_ema[i] = accel_ema[i] * (1.0f - w) + sensor.accelerometer_raw[i] * w; + float d = (float) sensor.accelerometer_raw[i] - accel_ema[i]; + d = d * d; + accel_disp[i] = accel_disp[i] * (1.0f - w); + 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 ) { + /* is still now */ + if (t_still == 0) { + /* first time */ + mavlink_log_info(mavlink_fd, "still"); + t_still = t; + t_timeout = t + timeout; + } else { + /* still since t_still */ + if ((int64_t) t - (int64_t) t_still > still_time) { + /* vehicle is still, exit from the loop to detection of its orientation */ + break; + } + } + } else { + /* not still, reset still start time */ + if (t_still != 0) { + mavlink_log_info(mavlink_fd, "moving"); + t_still = 0; + } + } + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_info(mavlink_fd, "ERROR: poll failure"); + return -3; + } + if (t > t_timeout) { + mavlink_log_info(mavlink_fd, "ERROR: timeout"); + return -1; + } + } + float accel_len = sqrt(accel_len2); + float accel_err_thr_raw = accel_len * accel_err_thr; + char str[80]; + sprintf(str, "ema: [ %.1f %.1f %.1f ]", accel_ema[0], accel_ema[1], accel_ema[2]); + mavlink_log_info(mavlink_fd, str); + sprintf(str, "disp: [ %.1f %.1f %.1f ]", accel_disp[0], accel_disp[1], accel_disp[2]); + mavlink_log_info(mavlink_fd, str); + 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 ) + 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 ) + 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 ) + 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 ) + return 3; // [ 0, -g, 0 ] + if ( abs(accel_ema[0]) < accel_err_thr_raw && + abs(accel_ema[1]) < accel_err_thr_raw && + abs(accel_ema[2] - accel_len) < accel_err_thr_raw ) + return 4; // [ 0, 0, g ] + if ( abs(accel_ema[0]) < accel_err_thr_raw && + abs(accel_ema[1]) < accel_err_thr_raw && + abs(accel_ema[2] + accel_len) < accel_err_thr_raw ) + return 5; // [ 0, 0, -g ] + mavlink_log_info(mavlink_fd, "ERROR: invalid orientation"); + return -2; // Can't detect orientation +} + +/* + * Read specified number of accelerometer samples, calculate average and dispersion. + */ +int read_accelerometer_raw_avg(int sensor_combined_sub, int16_t accel_avg[3], int samples_num) { + struct pollfd fds[1] = { { .fd = sensor_combined_sub, .events = POLLIN } }; + int count = 0; + int32_t accel_sum[3] = { 0, 0, 0 }; + while (count < samples_num) { + int poll_ret = poll(fds, 1, 1000); + if (poll_ret == 1) { + struct sensor_combined_s sensor; + orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); + for (int i = 0; i < 3; i++) { + accel_sum[i] += sensor.accelerometer_raw[i]; + } + count++; + } else { + return ERROR; + } + } + for (int i = 0; i < 3; i++) { + accel_avg[i] = (accel_sum[i] + count / 2) / count; + } + /* calculate dispersion */ + return OK; +} + +/* + * Convert raw values from accelerometers to m/s^2. + */ +void acceleration_raw_to_m_s2(float accel_corr[3], int16_t accel_raw[3], + float accel_T[3][3], int16_t accel_offs[3]) { + for (int i = 0; i < 3; i++) { + accel_corr[i] = 0.0f; + for (int j = 0; j < 3; j++) { + accel_corr[i] += accel_T[i][j] * (accel_raw[j] - accel_offs[j]); + } + } +} + +int mat_invert3(float src[3][3], float dst[3][3]) { + float det = src[0][0] * (src[1][1] * src[2][2] - src[1][2] * src[2][1]) - + src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) + + src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]); + if (det == 0.0) + return -1; // Singular matrix + dst[0][0] = (src[1][1] * src[2][2] - src[1][2] * src[2][1]) / det; + dst[1][0] = (src[1][2] * src[2][0] - src[1][0] * src[2][2]) / det; + dst[2][0] = (src[1][0] * src[2][1] - src[1][1] * src[2][0]) / det; + dst[0][1] = (src[0][2] * src[2][1] - src[0][1] * src[2][2]) / det; + dst[1][1] = (src[0][0] * src[2][2] - src[0][2] * src[2][0]) / det; + dst[2][1] = (src[0][1] * src[2][0] - src[0][0] * src[2][1]) / det; + dst[0][2] = (src[0][1] * src[1][2] - src[0][2] * src[1][1]) / det; + dst[1][2] = (src[0][2] * src[1][0] - src[0][0] * src[1][2]) / det; + dst[2][2] = (src[0][0] * src[1][1] - src[0][1] * src[1][0]) / det; + return 0; +} + +int calculate_calibration_values(int16_t accel_raw_ref[6][3], + float accel_T[3][3], int16_t accel_offs[3], float g) { + /* calculate raw offsets */ + for (int i = 0; i < 3; i++) { + accel_offs[i] = (int16_t) (((int32_t) (accel_raw_ref[i * 2][i]) + + (int32_t) (accel_raw_ref[i * 2 + 1][i])) / 2); + } + /* fill matrix A for linear equations system*/ + float mat_A[3][3]; + memset(mat_A, 0, sizeof(mat_A)); + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + float a = (accel_raw_ref[i * 2][j] - accel_offs[j]); + mat_A[i][j] = a; + } + } + /* calculate inverse matrix for A */ + float mat_A_inv[3][3]; + mat_invert3(mat_A, mat_A_inv); + for (int i = 0; i < 3; i++) { + /* copy results to accel_T */ + for (int j = 0; j < 3; j++) { + /* simplify matrices mult because b has only one non-zero element == g at index i */ + accel_T[j][i] = mat_A_inv[j][i] * g; + } + } + return 0; +} diff --git a/apps/commander/accelerometer_calibration.h b/apps/commander/accelerometer_calibration.h new file mode 100644 index 0000000000..acf45b6b6e --- /dev/null +++ b/apps/commander/accelerometer_calibration.h @@ -0,0 +1,19 @@ +/* + * accelerometer_calibration.h + * + * Created on: 25.04.2013 + * Author: ton + */ + +#ifndef ACCELEROMETER_CALIBRATION_H_ +#define ACCELEROMETER_CALIBRATION_H_ + +#include +#include + +void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd); +int detect_orientation(int mavlink_fd, int sub_sensor_combined); +int mat_invert3(float src[3][3], float dst[3][3]); +int calculate_calibration_values(int16_t accel_raw_ref[6][3], float accel_T[3][3], int16_t accel_offs[3], float g); + +#endif /* ACCELEROMETER_CALIBRATION_H_ */ diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 7c0a25f862..0f18d6cef5 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -94,7 +94,7 @@ #include #include "calibration_routines.h" - +#include "accelerometer_calibration.h" PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0); /**< Go into low-level failsafe after 0 ms */ //PARAM_DEFINE_INT32(SYS_FAILSAVE_HL, 0); /**< Go into high-level failsafe after 0 ms */ @@ -158,7 +158,6 @@ static int led_off(int led); static void do_gyro_calibration(int status_pub, struct vehicle_status_s *status); static void do_mag_calibration(int status_pub, struct vehicle_status_s *status); static void do_rc_calibration(int status_pub, struct vehicle_status_s *status); -static void do_accel_calibration(int status_pub, struct vehicle_status_s *status); static void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd); int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state); @@ -666,126 +665,6 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) close(sub_sensor_combined); } -void do_accel_calibration(int status_pub, struct vehicle_status_s *status) -{ - /* announce change */ - - mavlink_log_info(mavlink_fd, "keep it level and still"); - /* set to accel calibration mode */ - status->flag_preflight_accel_calibration = true; - state_machine_publish(status_pub, status, mavlink_fd); - - const int calibration_count = 2500; - - int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined)); - struct sensor_combined_s raw; - - int calibration_counter = 0; - float accel_offset[3] = {0.0f, 0.0f, 0.0f}; - - int fd = open(ACCEL_DEVICE_PATH, 0); - struct accel_scale ascale_null = { - 0.0f, - 1.0f, - 0.0f, - 1.0f, - 0.0f, - 1.0f, - }; - - if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale_null)) - warn("WARNING: failed to set scale / offsets for accel"); - - close(fd); - - while (calibration_counter < calibration_count) { - - /* wait blocking for new data */ - struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; - - int poll_ret = poll(fds, 1, 1000); - - if (poll_ret) { - orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); - accel_offset[0] += raw.accelerometer_m_s2[0]; - accel_offset[1] += raw.accelerometer_m_s2[1]; - accel_offset[2] += raw.accelerometer_m_s2[2]; - calibration_counter++; - - } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "acceleration calibration aborted"); - return; - } - } - - accel_offset[0] = accel_offset[0] / calibration_count; - accel_offset[1] = accel_offset[1] / calibration_count; - accel_offset[2] = accel_offset[2] / calibration_count; - - if (isfinite(accel_offset[0]) && isfinite(accel_offset[1]) && isfinite(accel_offset[2])) { - - /* add the removed length from x / y to z, since we induce a scaling issue else */ - float total_len = sqrtf(accel_offset[0] * accel_offset[0] + accel_offset[1] * accel_offset[1] + accel_offset[2] * accel_offset[2]); - - /* if length is correct, zero results here */ - accel_offset[2] = accel_offset[2] + total_len; - - float scale = 9.80665f / total_len; - - if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offset[0])) - || param_set(param_find("SENS_ACC_YOFF"), &(accel_offset[1])) - || param_set(param_find("SENS_ACC_ZOFF"), &(accel_offset[2])) - || param_set(param_find("SENS_ACC_XSCALE"), &(scale)) - || param_set(param_find("SENS_ACC_YSCALE"), &(scale)) - || param_set(param_find("SENS_ACC_ZSCALE"), &(scale))) { - mavlink_log_critical(mavlink_fd, "Setting offs or scale failed!"); - } - - fd = open(ACCEL_DEVICE_PATH, 0); - struct accel_scale ascale = { - accel_offset[0], - scale, - accel_offset[1], - scale, - accel_offset[2], - scale, - }; - - if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale)) - warn("WARNING: failed to set scale / offsets for accel"); - - close(fd); - - /* auto-save to EEPROM */ - int save_ret = param_save_default(); - - if (save_ret != 0) { - warn("WARNING: auto-save of params to storage failed"); - } - - //char buf[50]; - //sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]); - //mavlink_log_info(mavlink_fd, buf); - mavlink_log_info(mavlink_fd, "accel calibration done"); - - tune_confirm(); - sleep(2); - tune_confirm(); - sleep(2); - /* third beep by cal end routine */ - - } else { - mavlink_log_info(mavlink_fd, "accel calibration FAILED (NaN)"); - } - - /* exit accel calibration mode */ - status->flag_preflight_accel_calibration = false; - state_machine_publish(status_pub, status, mavlink_fd); - - close(sub_sensor_combined); -} - void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status) { /* announce change */ @@ -1040,7 +919,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { mavlink_log_info(mavlink_fd, "CMD starting accel cal"); tune_confirm(); - do_accel_calibration(status_pub, ¤t_status); + do_accel_calibration(status_pub, ¤t_status, mavlink_fd); tune_confirm(); mavlink_log_info(mavlink_fd, "CMD finished accel cal"); do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); From 593e3252dd90b2437862aba1c8a54b5c6edb5600 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 26 Apr 2013 13:52:25 +0400 Subject: [PATCH 2/5] Added hysteresis to still detector --- apps/commander/accelerometer_calibration.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/apps/commander/accelerometer_calibration.c b/apps/commander/accelerometer_calibration.c index f950398815..9e7a3b99e0 100644 --- a/apps/commander/accelerometer_calibration.c +++ b/apps/commander/accelerometer_calibration.c @@ -274,7 +274,9 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { break; } } - } else { + } 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) { /* not still, reset still start time */ if (t_still != 0) { mavlink_log_info(mavlink_fd, "moving"); From 26f9a1d42c6402a283a679e66236f247fd274cd2 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 26 Apr 2013 15:25:17 +0400 Subject: [PATCH 3/5] abs/fabs bugfix, logging cleanup --- apps/commander/accelerometer_calibration.c | 80 ++++++++-------------- 1 file changed, 29 insertions(+), 51 deletions(-) diff --git a/apps/commander/accelerometer_calibration.c b/apps/commander/accelerometer_calibration.c index 9e7a3b99e0..f607b6da69 100644 --- a/apps/commander/accelerometer_calibration.c +++ b/apps/commander/accelerometer_calibration.c @@ -80,7 +80,7 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd) { /* announce change */ - mavlink_log_info(mavlink_fd, "accelerometer calibration started"); + mavlink_log_info(mavlink_fd, "accel calibration started"); /* set to accel calibration mode */ status->flag_preflight_accel_calibration = true; state_machine_publish(status_pub, status, mavlink_fd); @@ -96,7 +96,7 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int m || param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale[0])) || param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale[1])) || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale[2]))) { - mavlink_log_critical(mavlink_fd, "Setting offs or scale failed!"); + mavlink_log_critical(mavlink_fd, "ERROR: setting offs or scale failed"); } int fd = open(ACCEL_DEVICE_PATH, 0); @@ -122,7 +122,6 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int m } mavlink_log_info(mavlink_fd, "accel calibration done"); - tune_confirm(); sleep(2); tune_confirm(); @@ -142,12 +141,11 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int m int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs_scaled[3], float accel_scale[3]) { const int samples_num = 2500; - - int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); - int16_t accel_raw_ref[6][3]; bool data_collected[6] = { false, false, false, false, false, false }; const char *orientation_strs[6] = { "x+", "x-", "y+", "y-", "z+", "z-" }; + + int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); while (true) { bool done = true; char str[80]; @@ -159,25 +157,21 @@ int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs_scaled[3], done = false; } } - if (done) { - mavlink_log_info(mavlink_fd, "all accel measurements complete"); + if (done) break; - } else { - mavlink_log_info(mavlink_fd, str); - int orient = detect_orientation(mavlink_fd, sensor_combined_sub); - if (orient < 0) { - sprintf(str, "orientation detection error: %i", orient); - mavlink_log_info(mavlink_fd, str); - return ERROR; - } - mavlink_log_info(mavlink_fd, "accel measurement started"); - read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[orient][0]), samples_num); - //mavlink_log_info(mavlink_fd, "accel measurement complete"); - str_ptr = sprintf(str, "complete: %i [ %i %i %i ]", orient, accel_raw_ref[orient][0], accel_raw_ref[orient][1], accel_raw_ref[orient][2]); - mavlink_log_info(mavlink_fd, str); - data_collected[orient] = true; - tune_confirm(); - } + mavlink_log_info(mavlink_fd, str); + + int orient = detect_orientation(mavlink_fd, sensor_combined_sub); + if (orient < 0) + return ERROR; + + sprintf(str, "meas started: %s", orientation_strs[orient]); + mavlink_log_info(mavlink_fd, str); + read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[orient][0]), samples_num); + str_ptr = sprintf(str, "meas result for %s: [ %i %i %i ]", orientation_strs[orient], accel_raw_ref[orient][0], accel_raw_ref[orient][1], accel_raw_ref[orient][2]); + mavlink_log_info(mavlink_fd, str); + data_collected[orient] = true; + tune_confirm(); } close(sensor_combined_sub); @@ -186,26 +180,16 @@ int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs_scaled[3], float accel_T[3][3]; int res = calculate_calibration_values(accel_raw_ref, accel_T, accel_offs, CONSTANTS_ONE_G); if (res != 0) { - mavlink_log_info(mavlink_fd, "calibration values calculation error"); + mavlink_log_info(mavlink_fd, "ERROR: calibration values calc error"); return ERROR; } - char str[80]; - sprintf(str, "accel offsets: [ %i %i %i ]", accel_offs[0], accel_offs[1], accel_offs[2]); - mavlink_log_info(mavlink_fd, str); - //mavlink_log_info(mavlink_fd, "accel transform matrix:"); - for (int i = 0; i < 3; i++) { - //sprintf(str, "\t[ %0.6f %0.6f %0.6f ]", accel_T[i][0], accel_T[i][1], accel_T[i][2]); - //mavlink_log_info(mavlink_fd, str); - } - /* convert raw accel offset to scaled and transform matrix to scales * rotation part of transform matrix is not used by now */ for (int i = 0; i < 3; i++) { accel_offs_scaled[i] = accel_offs[i] * accel_T[i][i]; accel_scale[i] = accel_T[i][i]; } - return OK; } @@ -226,8 +210,8 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { 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 20% of accel vector length */ - float accel_err_thr = 0.2f; + /* set accel error threshold to 30% of accel vector length */ + float accel_err_thr = 0.3f; /* still time required in us */ int64_t still_time = 2000000; struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; @@ -264,7 +248,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { /* is still now */ if (t_still == 0) { /* first time */ - mavlink_log_info(mavlink_fd, "still"); + mavlink_log_info(mavlink_fd, "still..."); t_still = t; t_timeout = t + timeout; } else { @@ -279,7 +263,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { accel_disp[2] > still_thr_raw2 * 2.0f) { /* not still, reset still start time */ if (t_still != 0) { - mavlink_log_info(mavlink_fd, "moving"); + mavlink_log_info(mavlink_fd, "moving..."); t_still = 0; } } @@ -289,17 +273,11 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { return -3; } if (t > t_timeout) { - mavlink_log_info(mavlink_fd, "ERROR: timeout"); return -1; } } float accel_len = sqrt(accel_len2); float accel_err_thr_raw = accel_len * accel_err_thr; - char str[80]; - sprintf(str, "ema: [ %.1f %.1f %.1f ]", accel_ema[0], accel_ema[1], accel_ema[2]); - mavlink_log_info(mavlink_fd, str); - sprintf(str, "disp: [ %.1f %.1f %.1f ]", accel_disp[0], accel_disp[1], accel_disp[2]); - mavlink_log_info(mavlink_fd, str); 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 ) @@ -316,13 +294,13 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { fabs(accel_ema[1] + accel_len) < accel_err_thr_raw && fabs(accel_ema[2]) < accel_err_thr_raw ) return 3; // [ 0, -g, 0 ] - if ( abs(accel_ema[0]) < accel_err_thr_raw && - abs(accel_ema[1]) < accel_err_thr_raw && - abs(accel_ema[2] - accel_len) < accel_err_thr_raw ) + 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 ) return 4; // [ 0, 0, g ] - if ( abs(accel_ema[0]) < accel_err_thr_raw && - abs(accel_ema[1]) < accel_err_thr_raw && - abs(accel_ema[2] + accel_len) < accel_err_thr_raw ) + 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 ) return 5; // [ 0, 0, -g ] mavlink_log_info(mavlink_fd, "ERROR: invalid orientation"); return -2; // Can't detect orientation From 4109874fc84339e3ee8a794b17d8bdd131313c51 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 28 Apr 2013 18:04:54 +0400 Subject: [PATCH 4/5] Reset offsets/scales before calibration and use prescaled values in m/s^2 instead of raw values. --- apps/commander/accelerometer_calibration.c | 128 ++++++++++++--------- apps/commander/accelerometer_calibration.h | 3 - 2 files changed, 76 insertions(+), 55 deletions(-) diff --git a/apps/commander/accelerometer_calibration.c b/apps/commander/accelerometer_calibration.c index f607b6da69..1807369080 100644 --- a/apps/commander/accelerometer_calibration.c +++ b/apps/commander/accelerometer_calibration.c @@ -78,6 +78,13 @@ #include #include +void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd); +int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]); +int detect_orientation(int mavlink_fd, int sub_sensor_combined); +int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num); +int mat_invert3(float src[3][3], float dst[3][3]); +int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g); + void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd) { /* announce change */ mavlink_log_info(mavlink_fd, "accel calibration started"); @@ -85,14 +92,16 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int m status->flag_preflight_accel_calibration = true; state_machine_publish(status_pub, status, mavlink_fd); - float accel_offs_scaled[3]; + /* measure and calculate offsets & scales */ + float accel_offs[3]; float accel_scale[3]; - int res = do_accel_calibration_mesurements(mavlink_fd, accel_offs_scaled, accel_scale); + int res = do_accel_calibration_mesurements(mavlink_fd, accel_offs, accel_scale); + if (res == OK) { /* measurements complete successfully, set parameters */ - if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offs_scaled[0])) - || param_set(param_find("SENS_ACC_YOFF"), &(accel_offs_scaled[1])) - || param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs_scaled[2])) + if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offs[0])) + || param_set(param_find("SENS_ACC_YOFF"), &(accel_offs[1])) + || param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs[2])) || param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale[0])) || param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale[1])) || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale[2]))) { @@ -101,11 +110,11 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int m int fd = open(ACCEL_DEVICE_PATH, 0); struct accel_scale ascale = { - accel_offs_scaled[0], + accel_offs[0], accel_scale[0], - accel_offs_scaled[1], + accel_offs[1], accel_scale[1], - accel_offs_scaled[2], + accel_offs[2], accel_scale[2], }; @@ -139,12 +148,30 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int m state_machine_publish(status_pub, status, mavlink_fd); } -int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs_scaled[3], float accel_scale[3]) { +int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]) { const int samples_num = 2500; - int16_t accel_raw_ref[6][3]; + float accel_ref[6][3]; bool data_collected[6] = { false, false, false, false, false, false }; const char *orientation_strs[6] = { "x+", "x-", "y+", "y-", "z+", "z-" }; + /* reset existing calibration */ + int fd = open(ACCEL_DEVICE_PATH, 0); + struct accel_scale ascale_null = { + 0.0f, + 1.0f, + 0.0f, + 1.0f, + 0.0f, + 1.0f, + }; + int ioctl_res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale_null); + close(fd); + + if (OK != ioctl_res) { + warn("ERROR: failed to set scale / offsets for accel"); + return ERROR; + } + int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); while (true) { bool done = true; @@ -167,8 +194,8 @@ int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs_scaled[3], sprintf(str, "meas started: %s", orientation_strs[orient]); mavlink_log_info(mavlink_fd, str); - read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[orient][0]), samples_num); - str_ptr = sprintf(str, "meas result for %s: [ %i %i %i ]", orientation_strs[orient], accel_raw_ref[orient][0], accel_raw_ref[orient][1], accel_raw_ref[orient][2]); + read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num); + str_ptr = sprintf(str, "meas result for %s: [ %.2f %.2f %.2f ]", orientation_strs[orient], accel_ref[orient][0], accel_ref[orient][1], accel_ref[orient][2]); mavlink_log_info(mavlink_fd, str); data_collected[orient] = true; tune_confirm(); @@ -176,20 +203,20 @@ int do_accel_calibration_mesurements(int mavlink_fd, float accel_offs_scaled[3], close(sensor_combined_sub); /* calculate offsets and rotation+scale matrix */ - int16_t accel_offs[3]; float accel_T[3][3]; - int res = calculate_calibration_values(accel_raw_ref, accel_T, accel_offs, CONSTANTS_ONE_G); + int res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G); if (res != 0) { mavlink_log_info(mavlink_fd, "ERROR: calibration values calc error"); return ERROR; } - /* convert raw accel offset to scaled and transform matrix to scales - * rotation part of transform matrix is not used by now */ + /* convert accel transform matrix to scales, + * rotation part of transform matrix is not used by now + */ for (int i = 0; i < 3; i++) { - accel_offs_scaled[i] = accel_offs[i] * accel_T[i][i]; accel_scale[i] = accel_T[i][i]; } + return OK; } @@ -233,8 +260,8 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { t_prev = t; float w = dt / ema_len; for (int i = 0; i < 3; i++) { - accel_ema[i] = accel_ema[i] * (1.0f - w) + sensor.accelerometer_raw[i] * w; - float d = (float) sensor.accelerometer_raw[i] - accel_ema[i]; + accel_ema[i] = accel_ema[i] * (1.0f - w) + sensor.accelerometer_m_s2[i] * w; + float d = (float) sensor.accelerometer_m_s2[i] - accel_ema[i]; d = d * d; accel_disp[i] = accel_disp[i] * (1.0f - w); if (d > accel_disp[i]) @@ -273,9 +300,11 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { return -3; } if (t > t_timeout) { + mavlink_log_info(mavlink_fd, "ERROR: timeout"); return -1; } } + 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 && @@ -302,56 +331,47 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { fabs(accel_ema[1]) < accel_err_thr_raw && fabs(accel_ema[2] + accel_len) < accel_err_thr_raw ) return 5; // [ 0, 0, -g ] + mavlink_log_info(mavlink_fd, "ERROR: invalid orientation"); + return -2; // Can't detect orientation } /* * Read specified number of accelerometer samples, calculate average and dispersion. */ -int read_accelerometer_raw_avg(int sensor_combined_sub, int16_t accel_avg[3], int samples_num) { +int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num) { struct pollfd fds[1] = { { .fd = sensor_combined_sub, .events = POLLIN } }; int count = 0; - int32_t accel_sum[3] = { 0, 0, 0 }; + float accel_sum[3] = { 0.0f, 0.0f, 0.0f }; + while (count < samples_num) { int poll_ret = poll(fds, 1, 1000); if (poll_ret == 1) { struct sensor_combined_s sensor; orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor); - for (int i = 0; i < 3; i++) { - accel_sum[i] += sensor.accelerometer_raw[i]; - } + for (int i = 0; i < 3; i++) + accel_sum[i] += sensor.accelerometer_m_s2[i]; count++; } else { return ERROR; } } - for (int i = 0; i < 3; i++) { - accel_avg[i] = (accel_sum[i] + count / 2) / count; - } - /* calculate dispersion */ - return OK; -} -/* - * Convert raw values from accelerometers to m/s^2. - */ -void acceleration_raw_to_m_s2(float accel_corr[3], int16_t accel_raw[3], - float accel_T[3][3], int16_t accel_offs[3]) { for (int i = 0; i < 3; i++) { - accel_corr[i] = 0.0f; - for (int j = 0; j < 3; j++) { - accel_corr[i] += accel_T[i][j] * (accel_raw[j] - accel_offs[j]); - } + accel_avg[i] = accel_sum[i] / count; } + + return OK; } int mat_invert3(float src[3][3], float dst[3][3]) { float det = src[0][0] * (src[1][1] * src[2][2] - src[1][2] * src[2][1]) - - src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) + - src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]); + src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) + + src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]); if (det == 0.0) - return -1; // Singular matrix + return ERROR; // Singular matrix + dst[0][0] = (src[1][1] * src[2][2] - src[1][2] * src[2][1]) / det; dst[1][0] = (src[1][2] * src[2][0] - src[1][0] * src[2][2]) / det; dst[2][0] = (src[1][0] * src[2][1] - src[1][1] * src[2][0]) / det; @@ -361,34 +381,38 @@ int mat_invert3(float src[3][3], float dst[3][3]) { dst[0][2] = (src[0][1] * src[1][2] - src[0][2] * src[1][1]) / det; dst[1][2] = (src[0][2] * src[1][0] - src[0][0] * src[1][2]) / det; dst[2][2] = (src[0][0] * src[1][1] - src[0][1] * src[1][0]) / det; - return 0; + + return OK; } -int calculate_calibration_values(int16_t accel_raw_ref[6][3], - float accel_T[3][3], int16_t accel_offs[3], float g) { - /* calculate raw offsets */ +int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g) { + /* calculate offsets */ for (int i = 0; i < 3; i++) { - accel_offs[i] = (int16_t) (((int32_t) (accel_raw_ref[i * 2][i]) - + (int32_t) (accel_raw_ref[i * 2 + 1][i])) / 2); + accel_offs[i] = (accel_ref[i * 2][i] + accel_ref[i * 2 + 1][i]) / 2; } + /* fill matrix A for linear equations system*/ float mat_A[3][3]; memset(mat_A, 0, sizeof(mat_A)); for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { - float a = (accel_raw_ref[i * 2][j] - accel_offs[j]); + float a = accel_ref[i * 2][j] - accel_offs[j]; mat_A[i][j] = a; } } + /* calculate inverse matrix for A */ float mat_A_inv[3][3]; - mat_invert3(mat_A, mat_A_inv); + if (mat_invert3(mat_A, mat_A_inv) != OK) + return ERROR; + + /* copy results to accel_T */ for (int i = 0; i < 3; i++) { - /* copy results to accel_T */ for (int j = 0; j < 3; j++) { /* simplify matrices mult because b has only one non-zero element == g at index i */ accel_T[j][i] = mat_A_inv[j][i] * g; } } - return 0; + + return OK; } diff --git a/apps/commander/accelerometer_calibration.h b/apps/commander/accelerometer_calibration.h index acf45b6b6e..c0169c2a13 100644 --- a/apps/commander/accelerometer_calibration.h +++ b/apps/commander/accelerometer_calibration.h @@ -12,8 +12,5 @@ #include void do_accel_calibration(int status_pub, struct vehicle_status_s *status, int mavlink_fd); -int detect_orientation(int mavlink_fd, int sub_sensor_combined); -int mat_invert3(float src[3][3], float dst[3][3]); -int calculate_calibration_values(int16_t accel_raw_ref[6][3], float accel_T[3][3], int16_t accel_offs[3], float g); #endif /* ACCELEROMETER_CALIBRATION_H_ */ From 1f800edc7676a6ea13127746ce38787a1e98b935 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 5 May 2013 15:51:16 +0400 Subject: [PATCH 5/5] Still threshold increased to 0.1m/s^2, and orientation error threshold to 5m/s^2. Timeout increased to 30s. --- apps/commander/accelerometer_calibration.c | 66 +++++++++++----------- 1 file changed, 32 insertions(+), 34 deletions(-) diff --git a/apps/commander/accelerometer_calibration.c b/apps/commander/accelerometer_calibration.c index 1807369080..991145d73c 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");