Nanda Fathurrahman / Mbed OS COG4050_adxl355_adxl357

Fixed algorithm to read 3 bytes of accelerometer data registers

Fork of COG4050_adxl355_adxl357 by valeria toffoli

Calibration/CALIBRATION.cpp@10:e054891b3598, 2018-09-10 (annotated)

Committer:
nfathurr
Date:
Mon Sep 10 10:01:49 2018 +0000
Revision:
10:e054891b3598
Parent:
9:6c803986dbde 
Fixed algorithm to read 3 bytes of accelerometer data registers

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 9:6c803986dbde 11 CALIBRATION::calib_data_t CALIBRATION::convert_2p_all(float angle[1], float meas[1][2]){
vtoffoli 9:6c803986dbde 12 // angle = | angle1 angle2| in RAD - 1/sqrt(3)*g
vtoffoli 9:6c803986dbde 13 // meas = | ax1 ay1 az1 |
vtoffoli 9:6c803986dbde 14 // | ax2 ay2 az2 |
vtoffoli 8:9e6ead2ee8d7 15 calib_data_t res;
vtoffoli 9:6c803986dbde 16 angle[1]=sin(angle[1]);
vtoffoli 9:6c803986dbde 17 angle[0]=sin(angle[0]);
vtoffoli 8:9e6ead2ee8d7 18 for(int i=0; i<3; i++){
vtoffoli 9:6c803986dbde 19 res.S[i]= (angle[1]-angle[0])/(meas[i][1]-meas[i][0]);
vtoffoli 9:6c803986dbde 20 res.B[i]= (angle[0]*meas[i][1]-angle[1]*meas[i][0])/(meas[i][1]-meas[i][0]);
vtoffoli 8:9e6ead2ee8d7 21 }
vtoffoli 8:9e6ead2ee8d7 22 return res;
vtoffoli 8:9e6ead2ee8d7 23 }
vtoffoli 9:6c803986dbde 24
vtoffoli 9:6c803986dbde 25 void CALIBRATION::convert_4p(float meas[3], int axis){
vtoffoli 9:6c803986dbde 26 // https://www.eetimes.com/document.asp?doc_id=1274067&page_number=3
vtoffoli 9:6c803986dbde 27 // for each axis
vtoffoli 9:6c803986dbde 28 // angle = | anglex1-0° anglex2-90° angley3-180° angley4-270°|
vtoffoli 9:6c803986dbde 29 // meas = | x1 x2 x3 x4 |
vtoffoli 9:6c803986dbde 30 float m, b;
vtoffoli 9:6c803986dbde 31 m = (meas[1]-meas[3])/2;
vtoffoli 9:6c803986dbde 32 b = (meas[0]-meas[3])/2;
vtoffoli 9:6c803986dbde 33 adxl355_sensitivity.S[axis] = adxl355_sensitivity.S[axis]/m;
vtoffoli 9:6c803986dbde 34 adxl355_sensitivity.B[axis] = adxl355_sensitivity.B[axis]-b;
vtoffoli 9:6c803986dbde 35 }
vtoffoli 9:6c803986dbde 36
vtoffoli 9:6c803986dbde 37 void CALIBRATION::convert_2p(float meas[1], int axis){
vtoffoli 9:6c803986dbde 38 // angle = | anglex1-0° anglex2-180°| = | anglex1@1g anglex2@-1g| in DEGREE
vtoffoli 9:6c803986dbde 39 // meas = | x1 x2|
vtoffoli 9:6c803986dbde 40 float m, b;
vtoffoli 9:6c803986dbde 41 m = 2/(meas[0]-meas[1]);
vtoffoli 9:6c803986dbde 42 b = -(meas[0]+meas[1])/(meas[0]-meas[1]);
vtoffoli 9:6c803986dbde 43 adxl355_sensitivity.S[axis] = adxl355_sensitivity.S[axis]/m;
vtoffoli 9:6c803986dbde 44 adxl355_sensitivity.B[axis] = adxl355_sensitivity.B[axis]-b;
vtoffoli 8:9e6ead2ee8d7 45 }
vtoffoli 9:6c803986dbde 46
vtoffoli 8:9e6ead2ee8d7 47 //CALIBRATION::calib_data_t CALIBRATION::convert_12p(float angle[11][2], float meas[11][2], int size){}
vtoffoli 8:9e6ead2ee8d7 48
vtoffoli 8:9e6ead2ee8d7 49
vtoffoli 8:9e6ead2ee8d7 50 /*For calculating Determinant of the Matrix */
vtoffoli 8:9e6ead2ee8d7 51
vtoffoli 8:9e6ead2ee8d7 52 void CALIBRATION::matrix_reset(){
vtoffoli 9:6c803986dbde 53 // default values
vtoffoli 9:6c803986dbde 54 adxl355_sensitivity.S[0] = 3.9e-6;
vtoffoli 9:6c803986dbde 55 adxl355_sensitivity.S[1] = 3.9e-6;
vtoffoli 9:6c803986dbde 56 adxl355_sensitivity.S[2] = 3.9e-6;
vtoffoli 9:6c803986dbde 57 adxl355_sensitivity.B[0] = 0;
vtoffoli 9:6c803986dbde 58 adxl355_sensitivity.B[1] = 0;
vtoffoli 9:6c803986dbde 59 adxl355_sensitivity.B[2] = 0;
vtoffoli 9:6c803986dbde 60 adxl355_sensitivity.St[0] = 0.02e-6;
vtoffoli 9:6c803986dbde 61 adxl355_sensitivity.St[1] = 0.02e-6;
vtoffoli 9:6c803986dbde 62 adxl355_sensitivity.St[2] = 0.02e-6;
vtoffoli 8:9e6ead2ee8d7 63 }
vtoffoli 8:9e6ead2ee8d7 64
vtoffoli 8:9e6ead2ee8d7 65
vtoffoli 8:9e6ead2ee8d7 66
vtoffoli 8:9e6ead2ee8d7 67 //https://www.sanfoundry.com/c-program-find-inverse-matrix/
vtoffoli 8:9e6ead2ee8d7 68
vtoffoli 8:9e6ead2ee8d7 69 //http://www.ccodechamp.com/c-program-to-find-inverse-of-matrix/