forked from Archive/PX4-Autopilot
Reset offsets/scales before calibration and use prescaled values in m/s^2 instead of raw values.
This commit is contained in:
Anton Babushkin
parent
29057cb3bd
commit
4109874fc8
|
|
@ -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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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_ */
|
||||
|
|
|
|||
Loading…
Reference in New Issue