diff options
Diffstat (limited to 'apps/position_estimator_inav/acceleration_transform.c')
| -rw-r--r-- | apps/position_estimator_inav/acceleration_transform.c | 84 |
1 files changed, 46 insertions, 38 deletions
diff --git a/apps/position_estimator_inav/acceleration_transform.c b/apps/position_estimator_inav/acceleration_transform.c index bd933cab4..812920878 100644 --- a/apps/position_estimator_inav/acceleration_transform.c +++ b/apps/position_estimator_inav/acceleration_transform.c @@ -17,23 +17,23 @@ * * * * * Calibration * * * * - * Tests - * - * accel_corr_x_p = [ g 0 0 ]^T // positive x - * accel_corr_x_n = [ -g 0 0 ]^T // negative x - * accel_corr_y_p = [ 0 g 0 ]^T // positive y - * accel_corr_y_n = [ 0 -g 0 ]^T // negative y - * accel_corr_z_p = [ 0 0 g ]^T // positive z - * accel_corr_z_n = [ 0 0 -g ]^T // negative z - * - * 6 tests * 3 axes = 18 equations + * 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 = (accel_raw_x_p + accel_raw_x_n + - * accel_raw_y_p + accel_raw_y_n + - * accel_raw_z_p + accel_raw_z_n) / 6 + * accel_offs[i] = (accel_raw_ref[i*2][i] + accel_raw_ref[i*2+1][i]) / 2 * * * Find accel_T @@ -41,9 +41,8 @@ * 9 unknown constants * need 9 equations -> use 3 of 6 measurements -> 3 * 3 = 9 equations * - * accel_corr[i] = accel_T[i][0] * (accel_raw[0] - accel_offs[0]) + - * accel_T[i][1] * (accel_raw[1] - accel_offs[1]) + - * accel_T[i][2] * (accel_raw[2] - accel_offs[2]) + * accel_corr_ref[i*2] = accel_T * (accel_raw_ref[i*2] - accel_offs), i = 0...2 + * * A x = b * * x = [ accel_T[0][0] ] @@ -56,34 +55,35 @@ * | accel_T[2][1] | * [ accel_T[2][2] ] * - * b = [ accel_corr_x_p[0] ] // One measurement per axis is enough - * | accel_corr_x_p[1] | - * | accel_corr_x_p[2] | - * | accel_corr_y_p[0] | - * | accel_corr_y_p[1] | - * | accel_corr_y_p[2] | - * | accel_corr_z_p[0] | - * | accel_corr_z_p[1] | - * [ accel_corr_z_p[2] ] - * - * a_x[i] = accel_raw_x_p[i] - accel_offs[i] // Measurement for X axis - * ... - * A = [ a_x[0] a_x[1] a_x[2] 0 0 0 0 0 0 ] - * | 0 0 0 a_x[0] a_x[1] a_x[2] 0 0 0 | - * | 0 0 0 0 0 0 a_x[0] a_x[1] a_x[2] | - * | a_y[0] a_y[1] a_y[2] 0 0 0 0 0 0 | - * | 0 0 0 a_y[0] a_y[1] a_y[2] 0 0 0 | - * | 0 0 0 0 0 0 a_y[0] a_y[1] a_y[2] | - * | a_z[0] a_z[1] a_z[2] 0 0 0 0 0 0 | - * | 0 0 0 a_z[0] a_z[1] a_z[2] 0 0 0 | - * [ 0 0 0 0 0 0 a_z[0] a_z[1] a_z[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 = A^-1 b */ #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]) { +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++) { @@ -91,3 +91,11 @@ void acceleration_correct(float accel_corr[3], int16_t accel_raw[3], float accel } } } + +void calculate_calibration_values(int16_t accel_raw_ref[6][3], + float accel_T[3][3], int16_t accel_offs[3], float g) { + 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); + } +} |