zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Complete calibration implemented

sbg
Anton 12 years ago
parent
1be74a4716
commit
d536e3d5f9
3 changed files with 122 additions and 65 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 93
      apps/position_estimator_inav/acceleration_transform.c
  2. 5
      apps/position_estimator_inav/acceleration_transform.h
  3. 89
      apps/position_estimator_inav/position_estimator_inav_main.c

93
apps/position_estimator_inav/acceleration_transform.c
Unescape View File

@ -43,41 +43,31 @@ @@ -43,41 +43,31 @@
*
* 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][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] ]
*
* b = [ accel_corr_ref[0][0] ] // One measurement per axis is enough
* | accel_corr_ref[0][1] |
* | accel_corr_ref[0][2] |
* | accel_corr_ref[2][0] |
* | accel_corr_ref[2][1] |
* | accel_corr_ref[2][2] |
* | accel_corr_ref[4][0] |
* | accel_corr_ref[4][1] |
* [ accel_corr_ref[4][2] ]
*
* a[i][j] = accel_raw_ref[i][j] - accel_offs[j], i = 0...5, j = 0...2
*
* A = [ a[0][0] a[0][1] a[0][2] 0 0 0 0 0 0 ]
* | 0 0 0 a[0][0] a[0][1] a[0][2] 0 0 0 |
* | 0 0 0 0 0 0 a[0][0] a[0][1] a[0][2] |
* | a[1][0] a[1][1] a[1][2] 0 0 0 0 0 0 |
* | 0 0 0 a[1][0] a[1][1] a[1][2] 0 0 0 |
* | 0 0 0 0 0 0 a[1][0] a[1][1] a[1][2] |
* | a[2][0] a[2][1] a[2][2] 0 0 0 0 0 0 |
* | 0 0 0 a[2][0] a[2][1] a[2][2] 0 0 0 |
* [ 0 0 0 0 0 0 a[2][0] a[2][1] a[2][2] ]
* 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"
@ -92,10 +82,49 @@ void acceleration_correct(float accel_corr[3], int16_t accel_raw[3], @@ -92,10 +82,49 @@ void acceleration_correct(float accel_corr[3], int16_t accel_raw[3],
}
}
void calculate_calibration_values(int16_t accel_raw_ref[6][3],
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;
}

5
apps/position_estimator_inav/acceleration_transform.h
Unescape View File

@ -10,9 +10,10 @@ @@ -10,9 +10,10 @@
#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]);
void acceleration_correct(float accel_corr[3], int16_t accel_raw[3],
float accel_T[3][3], int16_t accel_offs[3]);
void calculate_calibration_values(int16_t accel_raw_ref[6][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_ */

89
apps/position_estimator_inav/position_estimator_inav_main.c
Unescape View File

@ -76,12 +76,15 @@ const static float const_earth_gravity = 9.81f; @@ -76,12 +76,15 @@ 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);
/**
* Print the correct usage.
*/
static void usage(const char *reason);
static void usage(const char *reason) {
if (reason)
fprintf(stderr, "%s\n", reason);
@ -143,8 +146,7 @@ int position_estimator_inav_main(int argc, char *argv[]) { @@ -143,8 +146,7 @@ int position_estimator_inav_main(int argc, char *argv[]) {
}
void wait_for_input() {
printf(
"press any key to continue, 'Q' to abort\n");
printf("press any key to continue, 'Q' to abort\n");
while (true ) {
int c = getchar();
if (c == 'q' || c == 'Q') {
@ -179,9 +181,11 @@ void read_accelerometer_raw_avg(int sensor_combined_sub, int16_t accel_avg[3], @@ -179,9 +181,11 @@ void read_accelerometer_raw_avg(int sensor_combined_sub, int16_t accel_avg[3],
exit(1);
}
}
for (int i = 0; i < 3; i++)
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]);
}
printf("[position_estimator_inav] raw data: [ %i %i %i ]\n", accel_avg[0],
accel_avg[1], accel_avg[2]);
}
void do_accelerometer_calibration() {
@ -191,49 +195,72 @@ void do_accelerometer_calibration() { @@ -191,49 +195,72 @@ void do_accelerometer_calibration() {
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[4][0]),
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[5][0]),
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[2][0]),
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[3][0]),
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[0][0]),
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[1][0]),
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];
calculate_calibration_values(accel_raw_ref, accel_T, accel_offs,
int res = calculate_calibration_values(accel_raw_ref, accel_T, accel_offs,
const_earth_gravity);
printf("[position_estimator_inav] accelerometers raw offsets: [ %i %i %i ]\n",
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]);
int32_t accel_offs_int32[3] = { 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_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");
printf(
"[position_estimator_inav] auto-save of parameters to storage failed\n");
exit(1);
}
printf("[position_estimator_inav] calibration done\n");
@ -253,8 +280,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { @@ -253,8 +280,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
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 };
const static float dT_const_120 = 1.0f / 120.0f;
const static float dT2_const_120 = 1.0f / 120.0f / 120.0f / 2.0f;
bool use_gps = false;
int baro_loop_cnt = 0;
@ -308,12 +333,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { @@ -308,12 +333,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
.events = POLLIN } };
while (gps.fix_type < 3) {
/* wait for GPS updates, BUT READ VEHICLE STATUS (!)
* this choice is critical, since the vehicle status might not
* actually change, if this app is started after GPS lock was
* aquired.
*/
if (poll(fds_init, 1, 5000)) { /* poll only two first subscriptions */
if (fds_init[0].revents & POLLIN) {
/* Wait for the GPS update to propagate (we have some time) */
@ -345,15 +364,16 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { @@ -345,15 +364,16 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
hrt_abstime last_time = 0;
thread_running = true;
static int printatt_counter = 0;
uint32_t accelerometer_counter = 0;
uint32_t baro_counter = 0;
uint16_t accelerometer_updates = 0;
uint16_t baro_updates = 0;
uint16_t gps_updates = 0;
uint16_t attitude_updates = 0;
hrt_abstime updates_counter_start = hrt_absolute_time();
uint32_t updates_counter_len = 1000000;
hrt_abstime pub_last = hrt_absolute_time();
uint32_t pub_interval = 5000; // limit publish rate to 200 Hz
uint32_t pub_interval = 4000; // limit publish rate to 250 Hz
/* main loop */
struct pollfd fds[5] = { { .fd = parameter_update_sub, .events = POLLIN }, {
@ -368,8 +388,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { @@ -368,8 +388,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
int ret = poll(fds, 5, 10); // wait maximal this 10 ms = 100 Hz minimum rate
if (ret < 0) {
/* poll error */
if (verbose_mode)
printf("[position_estimator_inav] subscriptions poll error\n");
printf("[position_estimator_inav] subscriptions poll error\n");
thread_should_exit = true;
continue;
} else if (ret > 0) {
/* parameter update */
if (fds[0].revents & POLLIN) {
@ -388,6 +409,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { @@ -388,6 +409,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
/* vehicle attitude */
if (fds[2].revents & POLLIN) {
orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att);
attitude_updates++;
}
/* sensor combined */
if (fds[3].revents & POLLIN) {
@ -432,6 +454,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { @@ -432,6 +454,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
((double) (gps.lon)) * 1e-7, &(local_pos_gps[0]),
&(local_pos_gps[1]));
local_pos_gps[2] = (float) (gps.alt * 1e-3);
gps_updates++;
}
}
@ -476,12 +499,16 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) { @@ -476,12 +499,16 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) {
if (t - updates_counter_start > updates_counter_len) {
float updates_dt = (t - updates_counter_start) * 0.000001f;
printf(
"[position_estimator_inav] accelerometer_updates_rate = %.1/s, baro_updates_rate = %.1f/s\n",
"[position_estimator_inav] updates rate: accelerometer = %.1f/s, baro = %.1f/s, gps = %.1f/s, attitude = %.1f/s\n",
accelerometer_updates / updates_dt,
baro_updates / updates_dt);
baro_updates / updates_dt,
gps_updates / updates_dt,
attitude_updates / updates_dt);
updates_counter_start = t;
accelerometer_updates = 0;
baro_updates = 0;
gps_updates = 0;
attitude_updates = 0;
}
}
if (t - pub_last > pub_interval) {

Write Preview
Loading…
Cancel
Save