Reset offsets/scales before calibration and use prescaled values in m/s^2 instead of raw values.

This commit is contained in:
Anton Babushkin 2013-04-28 18:04:54 +04:00
parent 29057cb3bd
commit 4109874fc8
2 changed files with 76 additions and 55 deletions

View File

@ -78,6 +78,13 @@
#include <systemlib/conversions.h>
#include <mavlink/mavlink_log.h>
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;
}

View File

@ -12,8 +12,5 @@
#include <uORB/topics/vehicle_status.h>
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_ */