valeria toffoli / Mbed OS COG4050_adxl355_adxl357

base program for tilt measurement

Dependencies:   COG4050_ADT7420 ADXL362

Fork of COG4050_adxl355_adxl357-ver2 by ADI_CAC

Calibration/CALIBRATION.cpp@8:9e6ead2ee8d7, 2018-08-21 (annotated)

Committer:
vtoffoli
Date:
Tue Aug 21 13:25:37 2018 +0000
Revision:
8:9e6ead2ee8d7
Child:
9:6c803986dbde
COG+ADXL355

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vtoffoli 8:9e6ead2ee8d7 1 #include <stdint.h>
vtoffoli 8:9e6ead2ee8d7 2 #include <stdio.h>
vtoffoli 8:9e6ead2ee8d7 3 #include <math.h>
vtoffoli 8:9e6ead2ee8d7 4
vtoffoli 8:9e6ead2ee8d7 5 #include "mbed.h"
vtoffoli 8:9e6ead2ee8d7 6 #include "CALIBRATION.h"
vtoffoli 8:9e6ead2ee8d7 7
vtoffoli 8:9e6ead2ee8d7 8
vtoffoli 8:9e6ead2ee8d7 9 #define PI 3.14159265
vtoffoli 8:9e6ead2ee8d7 10
vtoffoli 8:9e6ead2ee8d7 11 CALIBRATION::calib_data_t CALIBRATION::convert_2p(float angle[11][2], float meas[11][2]){
vtoffoli 8:9e6ead2ee8d7 12 calib_data_t res;
vtoffoli 8:9e6ead2ee8d7 13 for(int i=0; i<3; i++){
vtoffoli 8:9e6ead2ee8d7 14 res.S[i][i]= (angle[i][1]-angle[i][0])/(meas[i][1]-meas[i][0]);
vtoffoli 8:9e6ead2ee8d7 15 res.B[i][0]= (angle[i][0]*meas[i][1]-angle[i][1]*meas[i][0])/(meas[i][1]-meas[i][0]);
vtoffoli 8:9e6ead2ee8d7 16 }
vtoffoli 8:9e6ead2ee8d7 17 return res;
vtoffoli 8:9e6ead2ee8d7 18 }
vtoffoli 8:9e6ead2ee8d7 19 CALIBRATION::calib_data_t CALIBRATION::convert_6p(float angle[11][2], float meas[11][2]){
vtoffoli 8:9e6ead2ee8d7 20 calib_data_t res;
vtoffoli 8:9e6ead2ee8d7 21 for(int i=0; i<3; i++){
vtoffoli 8:9e6ead2ee8d7 22 res.S[i][i]= (angle[i][1]-angle[i][0])/(meas[i][1]-meas[i][0]);
vtoffoli 8:9e6ead2ee8d7 23 res.B[i][0]= (angle[i][0]*meas[i][1]-angle[i][1]*meas[i][0])/(meas[i][1]-meas[i][0]);
vtoffoli 8:9e6ead2ee8d7 24 }
vtoffoli 8:9e6ead2ee8d7 25 return res;
vtoffoli 8:9e6ead2ee8d7 26 }
vtoffoli 8:9e6ead2ee8d7 27 //CALIBRATION::calib_data_t CALIBRATION::convert_12p(float angle[11][2], float meas[11][2], int size){}
vtoffoli 8:9e6ead2ee8d7 28
vtoffoli 8:9e6ead2ee8d7 29
vtoffoli 8:9e6ead2ee8d7 30 /*For calculating Determinant of the Matrix */
vtoffoli 8:9e6ead2ee8d7 31
vtoffoli 8:9e6ead2ee8d7 32 void CALIBRATION::matrix_reset(){
vtoffoli 8:9e6ead2ee8d7 33 memset(g_matrix, 0, sizeof(g_matrix[0][0]) * 2 * 11);
vtoffoli 8:9e6ead2ee8d7 34 memset(coeff_matrix.x_matrix, 0, sizeof(coeff_matrix.x_matrix[0][0]) * 11 * 3);
vtoffoli 8:9e6ead2ee8d7 35 memset(w_matrix, 0, sizeof(w_matrix[0][0]) * 3 * 2);
vtoffoli 8:9e6ead2ee8d7 36 }
vtoffoli 8:9e6ead2ee8d7 37
vtoffoli 8:9e6ead2ee8d7 38
vtoffoli 8:9e6ead2ee8d7 39 void CALIBRATION::matrix_g(float angle[4][12]){
vtoffoli 8:9e6ead2ee8d7 40 // angle expressed in degree: roll, pitch, yaw
vtoffoli 8:9e6ead2ee8d7 41 double roll, pitch;
vtoffoli 8:9e6ead2ee8d7 42 int i;
vtoffoli 8:9e6ead2ee8d7 43 for ( i = 0; i < 12; ++i) {
vtoffoli 8:9e6ead2ee8d7 44 roll = angle[0][i]*PI/180;
vtoffoli 8:9e6ead2ee8d7 45 pitch = angle[1][i]*PI/180;
vtoffoli 8:9e6ead2ee8d7 46 g_matrix[0][i] = -sin(pitch);
vtoffoli 8:9e6ead2ee8d7 47 g_matrix[1][i] = cos(pitch)*sin(roll);
vtoffoli 8:9e6ead2ee8d7 48 g_matrix[2][i] = cos(pitch)*cos(roll);
vtoffoli 8:9e6ead2ee8d7 49 }
vtoffoli 8:9e6ead2ee8d7 50 }
vtoffoli 8:9e6ead2ee8d7 51
vtoffoli 8:9e6ead2ee8d7 52 void CALIBRATION::matrix_x(float meas[3][12]){
vtoffoli 8:9e6ead2ee8d7 53 int i,j,k;
vtoffoli 8:9e6ead2ee8d7 54 // define xt and x
vtoffoli 8:9e6ead2ee8d7 55 for( i = 0; i < 12; i++){
vtoffoli 8:9e6ead2ee8d7 56 coeff_matrix.x_matrix[i][0] = meas[0][i];
vtoffoli 8:9e6ead2ee8d7 57 coeff_matrix.x_matrix[i][1] = meas[1][i];
vtoffoli 8:9e6ead2ee8d7 58 coeff_matrix.x_matrix[i][2] = meas[2][i];
vtoffoli 8:9e6ead2ee8d7 59 coeff_matrix.x_matrix[i][3] = 1;
vtoffoli 8:9e6ead2ee8d7 60 coeff_matrix.x_transpose[0][i] = meas[0][i];
vtoffoli 8:9e6ead2ee8d7 61 coeff_matrix.x_transpose[1][i] = meas[1][i];
vtoffoli 8:9e6ead2ee8d7 62 coeff_matrix.x_transpose[2][i] = meas[2][i];
vtoffoli 8:9e6ead2ee8d7 63 coeff_matrix.x_transpose[3][i] = 1;
vtoffoli 8:9e6ead2ee8d7 64 }
vtoffoli 8:9e6ead2ee8d7 65 // product xt * x
vtoffoli 8:9e6ead2ee8d7 66 double sum = 0;
vtoffoli 8:9e6ead2ee8d7 67 for ( i=0; i< 12; i++){
vtoffoli 8:9e6ead2ee8d7 68 for( j=0; j<3; j++){
vtoffoli 8:9e6ead2ee8d7 69 sum = sum + coeff_matrix.x_transpose[i][j]+coeff_matrix.x_matrix[i][j];
vtoffoli 8:9e6ead2ee8d7 70 }
vtoffoli 8:9e6ead2ee8d7 71 coeff_matrix.xtx_product[i][j] = sum;
vtoffoli 8:9e6ead2ee8d7 72 sum = 0;
vtoffoli 8:9e6ead2ee8d7 73 }
vtoffoli 8:9e6ead2ee8d7 74 // determinte of xt*t
vtoffoli 8:9e6ead2ee8d7 75 float det;
vtoffoli 8:9e6ead2ee8d7 76 det = matrix_determinant(coeff_matrix.xtx_product,12);
vtoffoli 8:9e6ead2ee8d7 77 // cofactor of ct*x
vtoffoli 8:9e6ead2ee8d7 78 float b[12][12],fac[12][12];
vtoffoli 8:9e6ead2ee8d7 79 int p,q;
vtoffoli 8:9e6ead2ee8d7 80 int m,n;
vtoffoli 8:9e6ead2ee8d7 81 for (q=0;q<12;q++){
vtoffoli 8:9e6ead2ee8d7 82 for (p=0;p<12;p++){
vtoffoli 8:9e6ead2ee8d7 83 m=0; n=0;
vtoffoli 8:9e6ead2ee8d7 84 for (i=0;i<12;i++){
vtoffoli 8:9e6ead2ee8d7 85 for (j=0;j<12;j++){
vtoffoli 8:9e6ead2ee8d7 86 if (i != q && j != p){
vtoffoli 8:9e6ead2ee8d7 87 b[m][n]=coeff_matrix.xtx_product[i][j];
vtoffoli 8:9e6ead2ee8d7 88 if (n<(12-2))
vtoffoli 8:9e6ead2ee8d7 89 n++;
vtoffoli 8:9e6ead2ee8d7 90 else{
vtoffoli 8:9e6ead2ee8d7 91 n=0; m++;}
vtoffoli 8:9e6ead2ee8d7 92 }
vtoffoli 8:9e6ead2ee8d7 93 }
vtoffoli 8:9e6ead2ee8d7 94 }
vtoffoli 8:9e6ead2ee8d7 95 //fac[q][p] = pow(-1,p+q)* matrix_determinant(b,12-1);
vtoffoli 8:9e6ead2ee8d7 96 }
vtoffoli 8:9e6ead2ee8d7 97 }
vtoffoli 8:9e6ead2ee8d7 98 }
vtoffoli 8:9e6ead2ee8d7 99
vtoffoli 8:9e6ead2ee8d7 100 float CALIBRATION::matrix_determinant(float a[12][12], float k){
vtoffoli 8:9e6ead2ee8d7 101 float s = 1, det = 0, b[12][12];
vtoffoli 8:9e6ead2ee8d7 102 int i, j, m, n, c;
vtoffoli 8:9e6ead2ee8d7 103 if (k == 1) { return (a[0][0]);}
vtoffoli 8:9e6ead2ee8d7 104 else{
vtoffoli 8:9e6ead2ee8d7 105 det = 0;
vtoffoli 8:9e6ead2ee8d7 106 for (c = 0; c < k; c++){
vtoffoli 8:9e6ead2ee8d7 107 m = 0; n = 0;
vtoffoli 8:9e6ead2ee8d7 108 for (i = 0;i < k; i++){
vtoffoli 8:9e6ead2ee8d7 109 for (j = 0 ;j < k; j++){
vtoffoli 8:9e6ead2ee8d7 110 b[i][j] = 0;
vtoffoli 8:9e6ead2ee8d7 111 if (i != 0 && j != c){
vtoffoli 8:9e6ead2ee8d7 112 b[m][n] = a[i][j];
vtoffoli 8:9e6ead2ee8d7 113 if (n < (k - 2))
vtoffoli 8:9e6ead2ee8d7 114 n++;
vtoffoli 8:9e6ead2ee8d7 115 else
vtoffoli 8:9e6ead2ee8d7 116 {n = 0; m++;}
vtoffoli 8:9e6ead2ee8d7 117 }
vtoffoli 8:9e6ead2ee8d7 118 }
vtoffoli 8:9e6ead2ee8d7 119 }
vtoffoli 8:9e6ead2ee8d7 120 det = det + s * (a[0][c] * matrix_determinant(b, k - 1));
vtoffoli 8:9e6ead2ee8d7 121 s = -1 * s;
vtoffoli 8:9e6ead2ee8d7 122 }
vtoffoli 8:9e6ead2ee8d7 123 }
vtoffoli 8:9e6ead2ee8d7 124 return (det);
vtoffoli 8:9e6ead2ee8d7 125 }
vtoffoli 8:9e6ead2ee8d7 126
vtoffoli 8:9e6ead2ee8d7 127 //https://www.sanfoundry.com/c-program-find-inverse-matrix/
vtoffoli 8:9e6ead2ee8d7 128
vtoffoli 8:9e6ead2ee8d7 129 //http://www.ccodechamp.com/c-program-to-find-inverse-of-matrix/