From 7570d9082ca4291b598f3e34be291189678685ec Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 2 May 2013 17:41:25 +0400 Subject: [PATCH] Use common accel calibration. Cleanup. --- apps/position_estimator_inav/Makefile | 5 - .../acceleration_transform.c | 130 ------------ .../acceleration_transform.h | 19 -- .../position_estimator_inav_main.c | 185 +++--------------- .../position_estimator_inav_params.c | 53 ----- .../position_estimator_inav_params.h | 19 -- 6 files changed, 22 insertions(+), 389 deletions(-) delete mode 100644 apps/position_estimator_inav/acceleration_transform.c delete mode 100644 apps/position_estimator_inav/acceleration_transform.h diff --git a/apps/position_estimator_inav/Makefile b/apps/position_estimator_inav/Makefile index 192ec18255..566bb9f431 100644 --- a/apps/position_estimator_inav/Makefile +++ b/apps/position_estimator_inav/Makefile @@ -39,9 +39,4 @@ APPNAME = position_estimator_inav PRIORITY = SCHED_PRIORITY_DEFAULT STACKSIZE = 4096 -CSRCS = position_estimator_inav_main.c \ - position_estimator_inav_params.c \ - kalman_filter_inertial.c \ - acceleration_transform.c - include $(APPDIR)/mk/app.mk diff --git a/apps/position_estimator_inav/acceleration_transform.c b/apps/position_estimator_inav/acceleration_transform.c deleted file mode 100644 index 3aba9b403d..0000000000 --- a/apps/position_estimator_inav/acceleration_transform.c +++ /dev/null @@ -1,130 +0,0 @@ -/* - * acceleration_transform.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 UAV frame - * accel_T[3][3] - accelerometers transform matrix, rotation and scaling transform - * accel_raw[3] - raw acceleration vector - * accel_offs[3] - true acceleration offset vector, don't use sensors offset because - * it's caused not only by real offset but also by sensor rotation - * - * * * * Calibration * * * - * - * Reference vectors - * accel_corr_ref[6][3] = [ g 0 0 ] // positive x - * | -g 0 0 | // negative x - * | 0 g 0 | // positive y - * | 0 -g 0 | // negative y - * | 0 0 g | // positive z - * [ 0 0 -g ] // negative z - * 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.81 - */ - -#include "acceleration_transform.h" - -void acceleration_correct(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/position_estimator_inav/acceleration_transform.h b/apps/position_estimator_inav/acceleration_transform.h deleted file mode 100644 index 4d1299db5d..0000000000 --- a/apps/position_estimator_inav/acceleration_transform.h +++ /dev/null @@ -1,19 +0,0 @@ -/* - * acceleration_transform.h - * - * Copyright (C) 2013 Anton Babushkin. All rights reserved. - * Author: Anton Babushkin - */ - -#ifndef ACCELERATION_TRANSFORM_H_ -#define ACCELERATION_TRANSFORM_H_ - -#include - -void acceleration_correct(float accel_corr[3], int16_t accel_raw[3], - float accel_T[3][3], int16_t accel_offs[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 /* ACCELERATION_TRANSFORM_H_ */ diff --git a/apps/position_estimator_inav/position_estimator_inav_main.c b/apps/position_estimator_inav/position_estimator_inav_main.c index 6e8c0ab5fe..071ec6ad4d 100644 --- a/apps/position_estimator_inav/position_estimator_inav_main.c +++ b/apps/position_estimator_inav/position_estimator_inav_main.c @@ -62,22 +62,19 @@ #include #include #include +#include #include #include "position_estimator_inav_params.h" #include "kalman_filter_inertial.h" -#include "acceleration_transform.h" static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ static int position_estimator_inav_task; /**< Handle of deamon task / thread */ static bool verbose_mode = false; -const static float const_earth_gravity = 9.81f; __EXPORT int position_estimator_inav_main(int argc, char *argv[]); -void do_accelerometer_calibration(); - int position_estimator_inav_thread_main(int argc, char *argv[]); static void usage(const char *reason); @@ -89,18 +86,18 @@ static void usage(const char *reason) { if (reason) fprintf(stderr, "%s\n", reason); fprintf(stderr, - "usage: position_estimator_inav {start|stop|status|calibrate} [-v]\n\n"); + "usage: position_estimator_inav {start|stop|status} [-v]\n\n"); exit(1); } - /** - * The position_estimator_inav_thread only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_create(). - */ +/** + * The position_estimator_inav_thread only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_create(). + */ int position_estimator_inav_main(int argc, char *argv[]) { if (argc < 1) usage("missing command"); @@ -136,136 +133,10 @@ int position_estimator_inav_main(int argc, char *argv[]) { exit(0); } - if (!strcmp(argv[1], "calibrate")) { - do_accelerometer_calibration(); - exit(0); - } - usage("unrecognized command"); exit(1); } -void wait_for_input() { - printf("press any key to continue, 'Q' to abort\n"); - while (true ) { - int c = getchar(); - if (c == 'q' || c == 'Q') { - exit(0); - } else { - return; - } - } -} - -void read_accelerometer_raw_avg(int sensor_combined_sub, int16_t accel_avg[3], - int samples) { - printf("[position_estimator_inav] measuring...\n"); - struct pollfd fds[1] = { { .fd = sensor_combined_sub, .events = POLLIN } }; - int count = 0; - int32_t accel_sum[3] = { 0, 0, 0 }; - while (count < samples) { - 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); - accel_sum[0] += sensor.accelerometer_raw[0]; - accel_sum[1] += sensor.accelerometer_raw[1]; - accel_sum[2] += sensor.accelerometer_raw[2]; - count++; - } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - printf("[position_estimator_inav] no accelerometer data for 1s\n"); - exit(1); - } else { - printf("[position_estimator_inav] poll error\n"); - exit(1); - } - } - for (int i = 0; i < 3; i++) { - accel_avg[i] = (accel_sum[i] + count / 2) / count; - } - printf("[position_estimator_inav] raw data: [ %i %i %i ]\n", accel_avg[0], - accel_avg[1], accel_avg[2]); -} - -void do_accelerometer_calibration() { - printf("[position_estimator_inav] calibration started\n"); - const int calibration_samples = 1000; - int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); - int16_t accel_raw_ref[6][3]; // Reference measurements - printf("[position_estimator_inav] place vehicle level, "); - wait_for_input(); - read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[5][0]), - calibration_samples); - printf("[position_estimator_inav] place vehicle on it's back, "); - wait_for_input(); - read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[4][0]), - calibration_samples); - printf("[position_estimator_inav] place vehicle on right side, "); - wait_for_input(); - read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[3][0]), - calibration_samples); - printf("[position_estimator_inav] place vehicle on left side, "); - wait_for_input(); - read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[2][0]), - calibration_samples); - printf("[position_estimator_inav] place vehicle nose down, "); - wait_for_input(); - read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[1][0]), - calibration_samples); - printf("[position_estimator_inav] place vehicle nose up, "); - wait_for_input(); - read_accelerometer_raw_avg(sensor_combined_sub, &(accel_raw_ref[0][0]), - calibration_samples); - close(sensor_combined_sub); - printf("[position_estimator_inav] reference data collection done\n"); - - int16_t accel_offs[3]; - float accel_T[3][3]; - int res = calculate_calibration_values(accel_raw_ref, accel_T, accel_offs, - const_earth_gravity); - if (res != 0) { - printf( - "[position_estimator_inav] calibration parameters calculation error\n"); - exit(1); - - } - printf( - "[position_estimator_inav] accelerometers raw offsets: [ %i %i %i ]\n", - accel_offs[0], accel_offs[1], accel_offs[2]); - printf( - "[position_estimator_inav] accelerometers transform matrix:\n [ %0.6f %0.6f %0.6f ]\n | %0.6f %0.6f %0.6f |\n [ %0.6f %0.6f %0.6f ]\n", - accel_T[0][0], accel_T[0][1], accel_T[0][2], accel_T[1][0], - accel_T[1][1], accel_T[1][2], accel_T[2][0], accel_T[2][1], - accel_T[2][2]); - int32_t accel_offs_int32[3] = - { accel_offs[0], accel_offs[1], accel_offs[2] }; - - if (param_set(param_find("INAV_ACC_OFFS_0"), &(accel_offs_int32[0])) - || param_set(param_find("INAV_ACC_OFFS_1"), &(accel_offs_int32[1])) - || param_set(param_find("INAV_ACC_OFFS_2"), &(accel_offs_int32[2])) - || param_set(param_find("INAV_ACC_T_00"), &(accel_T[0][0])) - || param_set(param_find("INAV_ACC_T_01"), &(accel_T[0][1])) - || param_set(param_find("INAV_ACC_T_02"), &(accel_T[0][2])) - || param_set(param_find("INAV_ACC_T_10"), &(accel_T[1][0])) - || param_set(param_find("INAV_ACC_T_11"), &(accel_T[1][1])) - || param_set(param_find("INAV_ACC_T_12"), &(accel_T[1][2])) - || param_set(param_find("INAV_ACC_T_20"), &(accel_T[2][0])) - || param_set(param_find("INAV_ACC_T_21"), &(accel_T[2][1])) - || param_set(param_find("INAV_ACC_T_22"), &(accel_T[2][2]))) { - printf("[position_estimator_inav] setting parameters failed\n"); - exit(1); - } - /* auto-save to EEPROM */ - int save_ret = param_save_default(); - if (save_ret != 0) { - printf( - "[position_estimator_inav] auto-save of parameters to storage failed\n"); - exit(1); - } - printf("[position_estimator_inav] calibration done\n"); -} - /**************************************************************************** * main ****************************************************************************/ @@ -277,18 +148,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { mavlink_log_info(mavlink_fd, "[position_estimator_inav] started"); /* initialize values */ - static float x_est[3] = { 0.0f, 0.0f, 0.0f }; - static float y_est[3] = { 0.0f, 0.0f, 0.0f }; - static float z_est[3] = { 0.0f, 0.0f, 0.0f }; - float accel_offs_est[3] = { 0.0f, 0.0f, 0.0f }; + float x_est[3] = { 0.0f, 0.0f, 0.0f }; + float y_est[3] = { 0.0f, 0.0f, 0.0f }; + float z_est[3] = { 0.0f, 0.0f, 0.0f }; int baro_loop_cnt = 0; int baro_loop_end = 70; /* measurement for 1 second */ - float baro_alt0 = 0.0f; /* to determin while start up */ + float baro_alt0 = 0.0f; /* to determine while start up */ - static double lat_current = 0.0d; //[°]] --> 47.0 - static double lon_current = 0.0d; //[°]] -->8.5 - static double alt_current = 0.0d; //[m] above MSL + static double lat_current = 0.0; //[°]] --> 47.0 + static double lon_current = 0.0; //[°]] -->8.5 + static double alt_current = 0.0; //[m] above MSL /* declare and safely initialize all structs */ struct vehicle_status_s vehicle_status; @@ -473,26 +343,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { float dt = (t - last_time) / 1000000.0; last_time = t; if (att.R_valid) { - /* calculate corrected acceleration vector in UAV frame */ - float accel_corr[3]; - acceleration_correct(accel_corr, sensor.accelerometer_raw, - params.acc_T, params.acc_offs); /* transform acceleration vector from UAV frame to NED frame */ float accel_NED[3]; for (int i = 0; i < 3; i++) { accel_NED[i] = 0.0f; for (int j = 0; j < 3; j++) { - accel_NED[i] += att.R[i][j] * (accel_corr[j] - accel_offs_est[j]); - } - } - accel_NED[2] += const_earth_gravity; - /* accelerometers offset drift correction: rotate acceleration error back to UAV frame and integrate */ - for (int i = 0; i < 3; i++) { - for (int j = 0; j < 3; j++) { - /* the inverse of a rotation matrix is its transpose, just swap i and j */ - accel_offs_est[i] += att.R[j][i] * accel_NED[j] * params.acc_offs_w * dt; + accel_NED[i] += att.R[i][j] * sensor.accelerometer_m_s2[j]; } } + accel_NED[2] += CONSTANTS_ONE_G; /* kalman filter prediction */ kalman_filter_inertial_predict(dt, z_est); /* prepare vectors for kalman filter correction */ @@ -531,9 +390,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { } if (t - pub_last > pub_interval) { pub_last = t; - pos.x = accel_offs_est[0]; - pos.vx = accel_offs_est[1]; - pos.y = accel_offs_est[2]; + pos.x = 0.0f; + pos.vx = 0.0f; + pos.y = 0.0f; pos.vy = 0.0f; pos.z = z_est[0]; pos.vz = z_est[1]; diff --git a/apps/position_estimator_inav/position_estimator_inav_params.c b/apps/position_estimator_inav/position_estimator_inav_params.c index 8cd9844c7d..bb2b014813 100644 --- a/apps/position_estimator_inav/position_estimator_inav_params.c +++ b/apps/position_estimator_inav/position_estimator_inav_params.c @@ -39,7 +39,6 @@ */ #include "position_estimator_inav_params.h" -#include "acceleration_transform.h" /* Kalman Filter covariances */ /* gps measurement noise standard deviation */ @@ -52,22 +51,6 @@ PARAM_DEFINE_FLOAT(INAV_K_ALT_11, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_ALT_20, 0.0f); PARAM_DEFINE_FLOAT(INAV_K_ALT_21, 0.0f); -PARAM_DEFINE_FLOAT(INAV_ACC_OFFS_W, 0.0f); - -PARAM_DEFINE_INT32(INAV_ACC_OFFS_0, 0); -PARAM_DEFINE_INT32(INAV_ACC_OFFS_1, 0); -PARAM_DEFINE_INT32(INAV_ACC_OFFS_2, 0); - -PARAM_DEFINE_FLOAT(INAV_ACC_T_00, 0.0021f); -PARAM_DEFINE_FLOAT(INAV_ACC_T_01, 0.0f); -PARAM_DEFINE_FLOAT(INAV_ACC_T_02, 0.0f); -PARAM_DEFINE_FLOAT(INAV_ACC_T_10, 0.0f); -PARAM_DEFINE_FLOAT(INAV_ACC_T_11, 0.0021f); -PARAM_DEFINE_FLOAT(INAV_ACC_T_12, 0.0f); -PARAM_DEFINE_FLOAT(INAV_ACC_T_20, 0.0f); -PARAM_DEFINE_FLOAT(INAV_ACC_T_21, 0.0f); -PARAM_DEFINE_FLOAT(INAV_ACC_T_22, 0.0021f); - int parameters_init(struct position_estimator_inav_param_handles *h) { h->use_gps = param_find("INAV_USE_GPS"); @@ -78,21 +61,6 @@ int parameters_init(struct position_estimator_inav_param_handles *h) { h->k_alt_20 = param_find("INAV_K_ALT_20"); h->k_alt_21 = param_find("INAV_K_ALT_21"); - h->acc_offs_w = param_find("INAV_ACC_OFFS_W"); - - h->acc_offs_0 = param_find("INAV_ACC_OFFS_0"); - h->acc_offs_1 = param_find("INAV_ACC_OFFS_1"); - h->acc_offs_2 = param_find("INAV_ACC_OFFS_2"); - - h->acc_t_00 = param_find("INAV_ACC_T_00"); - h->acc_t_01 = param_find("INAV_ACC_T_01"); - h->acc_t_02 = param_find("INAV_ACC_T_02"); - h->acc_t_10 = param_find("INAV_ACC_T_10"); - h->acc_t_11 = param_find("INAV_ACC_T_11"); - h->acc_t_12 = param_find("INAV_ACC_T_12"); - h->acc_t_20 = param_find("INAV_ACC_T_20"); - h->acc_t_21 = param_find("INAV_ACC_T_21"); - h->acc_t_22 = param_find("INAV_ACC_T_22"); return OK; } @@ -106,26 +74,5 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str param_get(h->k_alt_20, &(p->k[2][0])); param_get(h->k_alt_21, &(p->k[2][1])); - param_get(h->acc_offs_w, &(p->acc_offs_w)); - - /* read int32 and cast to int16 */ - int32_t t; - param_get(h->acc_offs_0, &t); - p->acc_offs[0] = t; - param_get(h->acc_offs_1, &t); - p->acc_offs[1] = t; - param_get(h->acc_offs_2, &t); - p->acc_offs[2] = t; - - param_get(h->acc_t_00, &(p->acc_T[0][0])); - param_get(h->acc_t_01, &(p->acc_T[0][1])); - param_get(h->acc_t_02, &(p->acc_T[0][2])); - param_get(h->acc_t_10, &(p->acc_T[1][0])); - param_get(h->acc_t_11, &(p->acc_T[1][1])); - param_get(h->acc_t_12, &(p->acc_T[1][2])); - param_get(h->acc_t_20, &(p->acc_T[2][0])); - param_get(h->acc_t_21, &(p->acc_T[2][1])); - param_get(h->acc_t_22, &(p->acc_T[2][2])); - return OK; } diff --git a/apps/position_estimator_inav/position_estimator_inav_params.h b/apps/position_estimator_inav/position_estimator_inav_params.h index 2c6fb3df2e..eaa1bbc953 100644 --- a/apps/position_estimator_inav/position_estimator_inav_params.h +++ b/apps/position_estimator_inav/position_estimator_inav_params.h @@ -43,9 +43,6 @@ struct position_estimator_inav_params { int use_gps; float k[3][2]; - float acc_offs_w; - int16_t acc_offs[3]; - float acc_T[3][3]; }; struct position_estimator_inav_param_handles { @@ -57,22 +54,6 @@ struct position_estimator_inav_param_handles { param_t k_alt_11; param_t k_alt_20; param_t k_alt_21; - - param_t acc_offs_w; - - param_t acc_offs_0; - param_t acc_offs_1; - param_t acc_offs_2; - - param_t acc_t_00; - param_t acc_t_01; - param_t acc_t_02; - param_t acc_t_10; - param_t acc_t_11; - param_t acc_t_12; - param_t acc_t_20; - param_t acc_t_21; - param_t acc_t_22; }; /**