fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-22 00:47:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Use common accel calibration. Cleanup.

Browse Source
This commit is contained in:
Anton Babushkin
2013-05-02 17:41:25 +04:00
parent 7e5801381c
commit 7570d9082c
6 changed files with 22 additions and 389 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/position_estimator_inav/Makefile
-5
View File
@@ -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
apps/position_estimator_inav/acceleration_transform.c
-130
View File
@@ -1,130 +0,0 @@
/*
* acceleration_transform.c
*
* Copyright (C) 2013 Anton Babushkin. All rights reserved.
* Author: Anton Babushkin <rk3dov@gmail.com>
*
* 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;
}
apps/position_estimator_inav/acceleration_transform.h
-19
View File
@@ -1,19 +0,0 @@
/*
* acceleration_transform.h
*
* Copyright (C) 2013 Anton Babushkin. All rights reserved.
* Author: Anton Babushkin <rk3dov@gmail.com>
*/
#ifndef ACCELERATION_TRANSFORM_H_
#define ACCELERATION_TRANSFORM_H_
#include <unistd.h>
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_ */
apps/position_estimator_inav/position_estimator_inav_main.c
+22 -163
View File
@@ -62,22 +62,19 @@
#include <poll.h>
#include <systemlib/geo/geo.h>
#include <systemlib/systemlib.h>
#include <systemlib/conversions.h>
#include <drivers/drv_hrt.h>
#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];
apps/position_estimator_inav/position_estimator_inav_params.c
-53
View File
@@ -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;
}
apps/position_estimator_inav/position_estimator_inav_params.h
-19
View File
@@ -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;
};
/**