IMU - | Mbed

Users » higedura » Code » IMU

main.cpp@11:b32b3d6be8d2, 2012-03-15 (annotated)

Committer:: higedura
Date:: Thu Mar 15 10:41:22 2012 +0000
Revision:: 11:b32b3d6be8d2
Parent:: 10:3cfc137e23e8

Who changed what in which revision?

User	Revision	Line number	New contents of line
higedura	2:78c3d0598819	1	// IMU for Sparkfun 9DOF Sensor Stick
higedura	2:78c3d0598819	2
higedura	6:2f8a7136e326	3	#include "mbed.h"
higedura	2:78c3d0598819	4	#include "ADXL345_I2C.h"
higedura	2:78c3d0598819	5	#include "ITG3200.h"
higedura	2:78c3d0598819	6	#include "HMC5883L.h"
higedura	2:78c3d0598819	7
higedura	2:78c3d0598819	8	DigitalOut led1(LED1);
higedura	2:78c3d0598819	9	DigitalOut led2(LED2);
higedura	2:78c3d0598819	10	DigitalOut led3(LED3);
higedura	2:78c3d0598819	11	DigitalOut led4(LED4);
higedura	2:78c3d0598819	12	ADXL345_I2C accelerometer(p9, p10);
higedura	2:78c3d0598819	13	ITG3200 gyro(p9, p10);
higedura	2:78c3d0598819	14	HMC5883L compass(p9, p10);
higedura	2:78c3d0598819	15	Serial pc(USBTX, USBRX);
higedura	2:78c3d0598819	16
higedura	2:78c3d0598819	17	#define pi 3.14159265
higedura	2:78c3d0598819	18
higedura	2:78c3d0598819	19	#define N 3
higedura	2:78c3d0598819	20	#define N_LWMA 10
higedura	2:78c3d0598819	21
higedura	6:2f8a7136e326	22	int preparing_acc();
higedura	6:2f8a7136e326	23	double* calib();
higedura	2:78c3d0598819	24	double* RK4( double, double[N], double[N] );
higedura	2:78c3d0598819	25	double* func( double[N], double[N] );
higedura	2:78c3d0598819	26	double* LWMA( double[N] );
higedura	2:78c3d0598819	27	double* EKF_predict( double[N], double[N] );
higedura	2:78c3d0598819	28	double* EKF_correct( double[N], double[N], double[N] );
higedura	2:78c3d0598819	29
higedura	2:78c3d0598819	30	// 0 1 2
higedura	2:78c3d0598819	31	// [ accXn-1 accXn-2 ... ] 0
higedura	2:78c3d0598819	32	// zBuf = [ accYn-1 accYn-2 ... ] 1
higedura	2:78c3d0598819	33	// [ accZn-1 accZn-2 ... ] 2
higedura	2:78c3d0598819	34	double z_buf[N][N_LWMA] = {0}; // For LWMA
higedura	2:78c3d0598819	35
higedura	2:78c3d0598819	36	double P[N][N] = {{1,0,0},{0,1,0},{0,0,1}}; // For EKF
higedura	2:78c3d0598819	37
higedura	2:78c3d0598819	38	int main(){
higedura	2:78c3d0598819	39
higedura	2:78c3d0598819	40	pc.baud(921600);
higedura	2:78c3d0598819	41
higedura	2:78c3d0598819	42	int correct_loop = 0;
higedura	2:78c3d0598819	43
higedura	4:41289b278f12	44	double dt_wait = 0.00514;
higedura	4:41289b278f12	45	double dt = 0.01;
higedura	2:78c3d0598819	46	double t = 0;
higedura	2:78c3d0598819	47
higedura	2:78c3d0598819	48	int bit_acc[N] = {0}; // Buffer of the accelerometer
higedura	2:78c3d0598819	49	int get_mag[N] = {0}; // Buffer of the compass
higedura	2:78c3d0598819	50
higedura	3:9f3e1a43f68f	51	// Calibration routine
higedura	2:78c3d0598819	52	double calib_acc[N] = {0};
higedura	2:78c3d0598819	53	double calib_gyro[N] = {0};
higedura	2:78c3d0598819	54	double compass_basis_rad = 0;
higedura	2:78c3d0598819	55
higedura	3:9f3e1a43f68f	56	// Getting data
higedura	2:78c3d0598819	57	double acc[N] = {0};
higedura	2:78c3d0598819	58	double gyro_deg[N] = {0};
higedura	2:78c3d0598819	59	double gyro_rad[N] = {0};
higedura	2:78c3d0598819	60	int mag[N] = {0};
higedura	2:78c3d0598819	61
higedura	3:9f3e1a43f68f	62	// Measurement algorithm
higedura	2:78c3d0598819	63	double ang_acc[2] = {0};
higedura	2:78c3d0598819	64	double ang_deg[N] = {0};
higedura	2:78c3d0598819	65	double ang_rad[N] = {0};
higedura	2:78c3d0598819	66	double zLWMA[N] = {0};
higedura	2:78c3d0598819	67	double compass_rad = 0;
higedura	2:78c3d0598819	68	double compass_deg = 0;
higedura	2:78c3d0598819	69
higedura	8:56fb656dcef6	70	for( int i=0;i<N_LWMA;i++ ){ z_buf[2][i] = 1; }
higedura	2:78c3d0598819	71
higedura	6:2f8a7136e326	72	double* p_calib;
higedura	6:2f8a7136e326	73	double* p_RK4;
higedura	6:2f8a7136e326	74	double* p_EKF;
higedura	6:2f8a7136e326	75	double* p_zLWMA;
higedura	2:78c3d0598819	76
higedura	6:2f8a7136e326	77	// * Setting up sensors *
higedura	6:2f8a7136e326	78	int preparing_acc();
higedura	2:78c3d0598819	79	gyro.setLpBandwidth(LPFBW_42HZ);
higedura	2:78c3d0598819	80	compass.setDefault();
higedura	6:2f8a7136e326	81	wait(0.1); // Wait some time for all sensors (Need at least 5ms)
higedura	2:78c3d0598819	82
higedura	2:78c3d0598819	83	// * Calibration routine *
higedura	6:2f8a7136e326	84	p_calib = calib();
higedura	6:2f8a7136e326	85	for( int i=0;i<3;i++ ){ calib_acc[i] = *p_calib; p_calib = p_calib+1; }
higedura	6:2f8a7136e326	86	for( int i=3;i<6;i++ ){ calib_gyro[i-3] = *p_calib; p_calib = p_calib+1; }
higedura	6:2f8a7136e326	87	compass_basis_rad = *p_calib;
higedura	6:2f8a7136e326	88
higedura	10:3cfc137e23e8	89	pc.printf("Starting IMU unit\n\r");
higedura	2:78c3d0598819	90	pc.printf(" Time AccX AccY AccZ GyroX GyroY GyroZ MagX MagY MagZ\n\r");
higedura	2:78c3d0598819	91	while(1){
higedura	2:78c3d0598819	92
higedura	2:78c3d0598819	93	// Updating accelerometer and compass
higedura	2:78c3d0598819	94	accelerometer.getOutput(bit_acc);
higedura	2:78c3d0598819	95	compass.readData(get_mag);
higedura	2:78c3d0598819	96
higedura	2:78c3d0598819	97	// Transfering units (Acc[g], Gyro[deg/s], Compass[Ga])
higedura	9:69ec742957ea	98	acc[0] = ((int16_t)bit_acc[0]-calib_acc[0])*0.004;
higedura	2:78c3d0598819	99	acc[1] = ((int16_t)bit_acc[1]-calib_acc[1])*0.004;
higedura	2:78c3d0598819	100	acc[2] = ((int16_t)bit_acc[2]-calib_acc[2])*0.004+1;
higedura	2:78c3d0598819	101
higedura	6:2f8a7136e326	102	gyro_deg[0] = (gyro.getGyroX()-calib_gyro[0])/14.375;
higedura	9:69ec742957ea	103	gyro_deg[1] = -(gyro.getGyroY()-calib_gyro[1])/14.375; // Modify the differencial of the sensor axis
higedura	9:69ec742957ea	104	gyro_deg[2] = -(gyro.getGyroZ()-calib_gyro[2])/14.375;
higedura	6:2f8a7136e326	105
higedura	6:2f8a7136e326	106	for( int i=0;i<N;i++ ){ mag[i] = (int16_t)get_mag[i]; }
higedura	2:78c3d0598819	107
higedura	2:78c3d0598819	108	// Low pass filter for acc
higedura	2:78c3d0598819	109	//for( int i=0;i<N;i++ ){ if( -0.05<acc[i] && acc[i]<0.05 ){ acc[i] = 0; } }
higedura	3:9f3e1a43f68f	110
higedura	2:78c3d0598819	111	for( int i=0;i<N;i++ ){ if( acc[i]<-2 ){ acc[i] = -2; } }
higedura	2:78c3d0598819	112	for( int i=0;i<N;i++ ){ if( acc[i]>2 ){ acc[i] = 2; } }
higedura	2:78c3d0598819	113
higedura	2:78c3d0598819	114	// Low pass filter for gyro
higedura	6:2f8a7136e326	115	//for( int i=0;i<N;i++ ){ if( -0.5<gyro_deg[i] && gyro_deg[i]<0.5 ){ gyro_deg[i] = 0; } }
higedura	2:78c3d0598819	116
higedura	2:78c3d0598819	117	for( int i=0;i<N;i++ ){ gyro_rad[i] = gyro_deg[i]*pi/180; }
higedura	2:78c3d0598819	118
higedura	2:78c3d0598819	119	// Compass yaw
higedura	4:41289b278f12	120	/*compass_rad = (double)mag[1]/mag[0];
higedura	2:78c3d0598819	121	compass_rad = atan(compass_rad);
higedura	2:78c3d0598819	122	//compass_rad = compass_rad-compass_basis_rad;
higedura	2:78c3d0598819	123	compass_deg = compass_rad*180/pi;
higedura	4:41289b278f12	124	*/
higedura	2:78c3d0598819	125	// LWMA (Observation)
higedura	6:2f8a7136e326	126	p_zLWMA = LWMA(acc);
higedura	6:2f8a7136e326	127	for( int i=0;i<N;i++ ){ zLWMA[i] = *p_zLWMA; p_zLWMA = p_zLWMA+1; }
higedura	2:78c3d0598819	128	// LWMA angle
higedura	6:2f8a7136e326	129	ang_acc[0] = asin(zLWMA[1])*180/pi;
higedura	4:41289b278f12	130	ang_acc[1] = asin(zLWMA[0])*180/pi;
higedura	2:78c3d0598819	131
higedura	2:78c3d0598819	132	// RK4 (Prediction)
higedura	6:2f8a7136e326	133	p_RK4 = RK4(dt,ang_rad,gyro_rad);
higedura	6:2f8a7136e326	134	for( int i=0;i<N;i++ ){ ang_rad[i] = *p_RK4; p_RK4 = p_RK4+1; }
higedura	2:78c3d0598819	135
higedura	2:78c3d0598819	136	// EKF (Correction)
higedura	2:78c3d0598819	137	EKF_predict(ang_rad,gyro_rad);
higedura	2:78c3d0598819	138	correct_loop++;
higedura	4:41289b278f12	139	if (correct_loop>=10){
higedura	6:2f8a7136e326	140	p_EKF = EKF_correct(ang_rad,gyro_rad,zLWMA);
higedura	6:2f8a7136e326	141	for ( int i=0;i<N;i++ ){ ang_rad[i] = *p_EKF; p_EKF = p_EKF+1; }
higedura	2:78c3d0598819	142	correct_loop = 0;
higedura	2:78c3d0598819	143	}
higedura	2:78c3d0598819	144	for( int i=0;i<N;i++ ){ ang_deg[i] = ang_rad[i]*180/pi; }
higedura	2:78c3d0598819	145
higedura	2:78c3d0598819	146	//pc.printf("%7.2f, %7.3f, %7.3f, %7.3f, %7.1f, %7.1f, %7.1f, %5d, %5d, %5d\n\r", t, acc[0], acc[1], acc[2], gyro_deg[0], gyro_deg[1], gyro_deg[2], mag[0], mag[1], mag[2]);
higedura	2:78c3d0598819	147	//pc.printf("%7.2f, %7.3f, %7.3f, %7.3f, %7.3f, %7.3f\n\r", t, zLWMA[0], zLWMA[1], zLWMA[2], ang_acc[0], ang_acc[1]);
higedura	6:2f8a7136e326	148	//pc.printf("%7.2f, %7.3f, %7.3f, %7.1f, %7.1f, %7.1f\n\r", t, ang_acc[0], ang_acc[1], ang_deg[0], ang_deg[1], ang_deg[2]);
higedura	6:2f8a7136e326	149	pc.printf("%7.2f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f\n\r", t, gyro_deg[0], gyro_deg[1], gyro_deg[2], ang_deg[0], ang_deg[1], ang_deg[2]);
higedura	6:2f8a7136e326	150
higedura	4:41289b278f12	151	wait(dt_wait);
higedura	2:78c3d0598819	152	t += dt;
higedura	2:78c3d0598819	153
higedura	2:78c3d0598819	154	}
higedura	2:78c3d0598819	155	}
higedura	2:78c3d0598819	156
higedura	2:78c3d0598819	157
higedura	2:78c3d0598819	158	double* EKF_predict( double y[N], double x[N] ){
higedura	2:78c3d0598819	159	// x = F * x;
higedura	2:78c3d0598819	160	// P = F * P * F' + G * Q * G';
higedura	2:78c3d0598819	161
higedura	6:2f8a7136e326	162	//double Q[N][N] = { {0.1, 0, 0}, {0, 0.1, 0}, {0, 0, 0.1} };
higedura	6:2f8a7136e326	163	double Q[N][N] = { {0.00263, 0, 0}, {0, 0.00373, 0}, {0, 0, 0.00509} };
higedura	2:78c3d0598819	164
higedura	2:78c3d0598819	165	double Fjac[N][N] = {{x[1]cos(y[0])tan(y[1])-x[2]sin(y[0])tan(y[1]), x[1]sin(y[0])/(cos(y[1])cos(y[1]))+x[2]cos(y[0])/(cos(y[1])cos(y[1])), 0}, {-x[1]sin(y[0])-x[2]cos(y[0]), 0, 0}, {x[1]cos(y[0])/cos(y[1])-x[2]sin(y[0])/cos(y[1]), x[1]sin(y[0])sin(y[1])/(cos(y[1])cos(y[1]))+x[2]cos(y[0])sin(y[1])/(cos(y[1])cos(y[1])), 0}};
higedura	2:78c3d0598819	166	double Fjac_t[N][N] = {{x[1]cos(y[0])tan(y[1])-x[2]sin(y[0])tan(y[1]), -x[1]sin(y[0])-x[2]cos(y[0]), x[1]cos(y[0])/cos(y[1])-x[2]sin(y[0])/cos(y[1])}, {x[1]sin(y[0])/(cos(y[1])cos(y[1]))+x[2]cos(y[0])/(cos(y[1])cos(y[1])), 0, x[1]sin(y[0])sin(y[1])/(cos(y[1])cos(y[1]))+x[2]cos(y[0])sin(y[1])/(cos(y[1])cos(y[1]))}, {0, 0, 0}};
higedura	2:78c3d0598819	167	double Gjac[N][N] = {{1, sin(y[0])tan(y[1]), cos(y[0])tan(y[1])}, {0, cos(y[0]), -sin(y[0])}, {0, sin(y[0])/cos(y[1]), cos(y[0])/cos(y[1])}};
higedura	2:78c3d0598819	168	double Gjac_t[N][N] = {{1, 0, 0}, {sin(y[0])tan(y[1]), cos(y[0]), sin(y[0])/cos(y[1])}, {cos(y[0])tan(y[1]), -sin(y[0]), cos(y[0])/cos(y[1])}};
higedura	2:78c3d0598819	169
higedura	2:78c3d0598819	170	double FPF[N][N] = {0};
higedura	2:78c3d0598819	171	double GQG[N][N] = {0};
higedura	2:78c3d0598819	172
higedura	2:78c3d0598819	173	double FP[N][N] = {0};
higedura	2:78c3d0598819	174	double GQ[N][N] = {0};
higedura	2:78c3d0598819	175
higedura	2:78c3d0598819	176	for( int i=0;i<N;i++ ){
higedura	2:78c3d0598819	177	for( int j=0;j<N;j++ ){
higedura	2:78c3d0598819	178	for( int k=0;k<N;k++ ){
higedura	2:78c3d0598819	179	FP[i][j] += Fjac[i][k]*P[k][j];
higedura	2:78c3d0598819	180	GQ[i][j] += Gjac[i][k]*Q[k][j];
higedura	2:78c3d0598819	181
higedura	2:78c3d0598819	182	}
higedura	2:78c3d0598819	183	}
higedura	2:78c3d0598819	184	}
higedura	2:78c3d0598819	185
higedura	2:78c3d0598819	186	for( int i=0;i<N;i++ ){
higedura	2:78c3d0598819	187	for( int j=0;j<N;j++ ){
higedura	2:78c3d0598819	188	for( int k=0;k<N;k++ ){
higedura	2:78c3d0598819	189	FPF[i][j] += FP[i][k]*Fjac_t[k][j];
higedura	2:78c3d0598819	190	GQG[i][j] += GQ[i][k]*Gjac_t[k][j];
higedura	2:78c3d0598819	191	}
higedura	2:78c3d0598819	192	}
higedura	2:78c3d0598819	193	}
higedura	2:78c3d0598819	194	for( int i=0;i<N;i++ ){
higedura	2:78c3d0598819	195	for( int j=0;j<N;j++ ){
higedura	2:78c3d0598819	196	P[i][j] = FPF[i][j]+GQG[i][j];
higedura	2:78c3d0598819	197	}
higedura	2:78c3d0598819	198	}
higedura	2:78c3d0598819	199
higedura	2:78c3d0598819	200	return 0;
higedura	2:78c3d0598819	201
higedura	2:78c3d0598819	202	}
higedura	2:78c3d0598819	203
higedura	2:78c3d0598819	204	double* EKF_correct( double y[N], double x[N], double z[N] ){
higedura	2:78c3d0598819	205	// K = P * H' / ( H * P * H' + R )
higedura	2:78c3d0598819	206	// x = x + K * ( yobs(t) - H * x )
higedura	2:78c3d0598819	207	// P = P - K * H * P
higedura	2:78c3d0598819	208
higedura	6:2f8a7136e326	209	//double R[N][N] = { {0.15, 0, 0}, {0, 0.15, 0}, {0, 0, 0.15} };
higedura	6:2f8a7136e326	210	double R[N][N] = { {0.00015, 0, 0}, {0, 0.00016, 0}, {0, 0, 0.00032} };
higedura	2:78c3d0598819	211
higedura	2:78c3d0598819	212	double Hjac[N][N] = {{0, cos(y[1]), 0}, {cos(y[0]), 0, 0}, {-sin(y[0])cos(y[1]), -cos(y[0])sin(y[1]), 0}};
higedura	2:78c3d0598819	213	double Hjac_t[N][N] = {{0, cos(y[0]), -sin(y[0])cos(y[1])}, {cos(y[1]), 0, -cos(y[0])sin(y[1])}, {0, 0, 0}};
higedura	2:78c3d0598819	214	double K[N][N] = {0}; // Kalman gain
higedura	2:78c3d0598819	215	double K_deno[N][N] = {0}; // Denominator of the kalman gain
higedura	2:78c3d0598819	216	double det_K_deno_inv = 0;
higedura	2:78c3d0598819	217	double K_deno_inv[N][N] = {0};
higedura	2:78c3d0598819	218	double HPH[N][N] = {0};
higedura	2:78c3d0598819	219	double HP[N][N] = {0};
higedura	2:78c3d0598819	220	double PH[N][N] = {0};
higedura	2:78c3d0598819	221	double KHP[N][N] = {0};
higedura	2:78c3d0598819	222
higedura	2:78c3d0598819	223	double Hx[N] = {0};
higedura	2:78c3d0598819	224	double z_Hx[N] = {0};
higedura	2:78c3d0598819	225	double Kz_Hx[N] = {0};
higedura	2:78c3d0598819	226
higedura	2:78c3d0598819	227	double* py = y;
higedura	2:78c3d0598819	228
higedura	2:78c3d0598819	229	// Kalman gain
higedura	2:78c3d0598819	230	for( int i=0;i<N;i++ ){
higedura	2:78c3d0598819	231	for( int j=0;j<N;j++ ){
higedura	2:78c3d0598819	232	for( int k=0;k<N;k++ ){
higedura	2:78c3d0598819	233	HP[i][j] += Hjac[i][k]*P[k][j];
higedura	2:78c3d0598819	234	PH[i][j] += P[i][k]*Hjac_t[k][j];
higedura	2:78c3d0598819	235	}
higedura	2:78c3d0598819	236	}
higedura	2:78c3d0598819	237	}
higedura	2:78c3d0598819	238	for( int i=0;i<N;i++ ){
higedura	2:78c3d0598819	239	for( int j=0;j<N;j++ ){
higedura	2:78c3d0598819	240	for( int k=0;k<N;k++ ){
higedura	2:78c3d0598819	241	HPH[i][j] += HP[i][k]*Hjac_t[k][j];
higedura	2:78c3d0598819	242	}
higedura	2:78c3d0598819	243	}
higedura	2:78c3d0598819	244	}
higedura	2:78c3d0598819	245	for( int i=0;i<N;i++ ){
higedura	2:78c3d0598819	246	for( int j=0;j<N;j++ ){
higedura	2:78c3d0598819	247	K_deno[i][j] = HPH[i][j]+R[i][j];
higedura	2:78c3d0598819	248	}
higedura	2:78c3d0598819	249	}
higedura	3:9f3e1a43f68f	250	// Inverce matrix
higedura	2:78c3d0598819	251	det_K_deno_inv = K_deno[0][0]K_deno[1][1]K_deno[2][2]+K_deno[1][0]K_deno[2][1]K_deno[0][2]+K_deno[2][0]K_deno[0][1]K_deno[1][2]-K_deno[0][0]K_deno[2][1]K_deno[1][2]-K_deno[2][0]K_deno[1][1]K_deno[0][2]-K_deno[1][0]K_deno[0][1]K_deno[2][2];
higedura	2:78c3d0598819	252	K_deno_inv[0][0] = (K_deno[1][1]K_deno[2][2]-K_deno[1][2]K_deno[2][1])/det_K_deno_inv;
higedura	2:78c3d0598819	253	K_deno_inv[0][1] = (K_deno[0][2]K_deno[2][1]-K_deno[0][1]K_deno[2][2])/det_K_deno_inv;
higedura	2:78c3d0598819	254	K_deno_inv[0][2] = (K_deno[0][1]K_deno[1][2]-K_deno[0][2]K_deno[1][1])/det_K_deno_inv;
higedura	2:78c3d0598819	255	K_deno_inv[1][0] = (K_deno[1][2]K_deno[2][0]-K_deno[1][0]K_deno[2][2])/det_K_deno_inv;
higedura	2:78c3d0598819	256	K_deno_inv[1][1] = (K_deno[0][0]K_deno[2][2]-K_deno[0][2]K_deno[2][0])/det_K_deno_inv;
higedura	2:78c3d0598819	257	K_deno_inv[1][2] = (K_deno[0][2]K_deno[1][0]-K_deno[0][0]K_deno[1][2])/det_K_deno_inv;
higedura	2:78c3d0598819	258	K_deno_inv[2][0] = (K_deno[1][0]K_deno[2][1]-K_deno[1][1]K_deno[2][0])/det_K_deno_inv;
higedura	2:78c3d0598819	259	K_deno_inv[2][1] = (K_deno[0][1]K_deno[2][0]-K_deno[0][0]K_deno[2][1])/det_K_deno_inv;
higedura	2:78c3d0598819	260	K_deno_inv[2][2] = (K_deno[0][0]K_deno[1][1]-K_deno[0][1]K_deno[1][0])/det_K_deno_inv;
higedura	2:78c3d0598819	261
higedura	2:78c3d0598819	262	for( int i=0;i<N;i++ ){
higedura	2:78c3d0598819	263	for( int j=0;j<N;j++ ){
higedura	2:78c3d0598819	264	for( int k=0;k<N;k++ ){
higedura	2:78c3d0598819	265	K[i][j] += PH[i][k]*K_deno_inv[k][j];
higedura	2:78c3d0598819	266	}
higedura	2:78c3d0598819	267	}
higedura	2:78c3d0598819	268	}
higedura	2:78c3d0598819	269
higedura	2:78c3d0598819	270	// Filtering
higedura	2:78c3d0598819	271	for( int i=0;i<N;i++ ){
higedura	2:78c3d0598819	272	for( int j=0;j<N;j++ ){
higedura	2:78c3d0598819	273	Hx[i] += Hjac[i][j]*y[j];
higedura	2:78c3d0598819	274	}
higedura	2:78c3d0598819	275	}
higedura	2:78c3d0598819	276	for( int i=0;i<N;i++ ){
higedura	2:78c3d0598819	277	z_Hx[i] = z[i]-Hx[i];
higedura	2:78c3d0598819	278	}
higedura	2:78c3d0598819	279	for( int i=0;i<N;i++ ){
higedura	2:78c3d0598819	280	for( int j=0;j<N;j++ ){
higedura	2:78c3d0598819	281	Kz_Hx[i] += K[i][j]*z_Hx[j];
higedura	2:78c3d0598819	282	}
higedura	2:78c3d0598819	283	}
higedura	2:78c3d0598819	284	for( int i=0;i<N;i++ ){
higedura	2:78c3d0598819	285	y[i] = y[i]+Kz_Hx[i];
higedura	2:78c3d0598819	286	}
higedura	2:78c3d0598819	287
higedura	2:78c3d0598819	288	// Covarience
higedura	2:78c3d0598819	289	for( int i=0;i<N;i++ ){
higedura	2:78c3d0598819	290	for( int j=0;j<N;j++ ){
higedura	2:78c3d0598819	291	for( int k=0;k<N;k++ ){
higedura	2:78c3d0598819	292	KHP[i][j] += K[i][k]*HP[k][j];
higedura	2:78c3d0598819	293	}
higedura	2:78c3d0598819	294	}
higedura	2:78c3d0598819	295	}
higedura	2:78c3d0598819	296	for( int i=0;i<N;i++ ){
higedura	2:78c3d0598819	297	for( int j=0;j<N;j++ ){
higedura	2:78c3d0598819	298	P[i][j] = P[i][j]-KHP[i][j];
higedura	2:78c3d0598819	299	}
higedura	2:78c3d0598819	300	}
higedura	2:78c3d0598819	301
higedura	2:78c3d0598819	302	return py;
higedura	2:78c3d0598819	303
higedura	2:78c3d0598819	304	}
higedura	2:78c3d0598819	305
higedura	2:78c3d0598819	306	double* LWMA( double z[N] ){
higedura	2:78c3d0598819	307
higedura	2:78c3d0598819	308	double zLWMA[N] = {0};
higedura	2:78c3d0598819	309	double zLWMA_num[N] = {0};
higedura	2:78c3d0598819	310	double zLWMA_den = 0;
higedura	2:78c3d0598819	311
higedura	6:2f8a7136e326	312	double* p_zLWMA = zLWMA;
higedura	2:78c3d0598819	313
higedura	2:78c3d0598819	314	for( int i=1;i<N_LWMA;i++ ){
higedura	2:78c3d0598819	315	for( int j=0;j<N;j++ ){
higedura	2:78c3d0598819	316	z_buf[j][N_LWMA-i] = z_buf[j][N_LWMA-i-1];
higedura	2:78c3d0598819	317	}
higedura	2:78c3d0598819	318	}
higedura	2:78c3d0598819	319	for( int i=0;i<N;i++ ){
higedura	2:78c3d0598819	320	z_buf[i][0] = z[i];
higedura	2:78c3d0598819	321	}
higedura	2:78c3d0598819	322
higedura	2:78c3d0598819	323	for( int i=0;i<N_LWMA;i++ ){
higedura	2:78c3d0598819	324	for( int j=0;j<N;j++ ){
higedura	2:78c3d0598819	325	zLWMA_num[j] += (N_LWMA-i)*z_buf[j][i];
higedura	2:78c3d0598819	326	}
higedura	2:78c3d0598819	327	zLWMA_den += N_LWMA-i;
higedura	2:78c3d0598819	328	}
higedura	2:78c3d0598819	329	for( int i=0;i<N;i++ ){
higedura	2:78c3d0598819	330	zLWMA[i] = zLWMA_num[i]/zLWMA_den;
higedura	2:78c3d0598819	331	}
higedura	2:78c3d0598819	332
higedura	6:2f8a7136e326	333	return p_zLWMA;
higedura	2:78c3d0598819	334
higedura	2:78c3d0598819	335	}
higedura	2:78c3d0598819	336
higedura	2:78c3d0598819	337	double* RK4( double dt, double y[N], double x[N] ){
higedura	2:78c3d0598819	338
higedura	2:78c3d0598819	339	double yBuf[N] = {0};
higedura	2:78c3d0598819	340	double k[N][4] = {0};
higedura	2:78c3d0598819	341
higedura	2:78c3d0598819	342	double* p_y = y;
higedura	2:78c3d0598819	343
higedura	2:78c3d0598819	344	double* pk1;
higedura	2:78c3d0598819	345	double* pk2;
higedura	2:78c3d0598819	346	double* pk3;
higedura	2:78c3d0598819	347	double* pk4;
higedura	2:78c3d0598819	348
higedura	2:78c3d0598819	349	for( int i=0;i<N;i++){ yBuf[i] = y[i]; }
higedura	2:78c3d0598819	350	pk1 = func (yBuf,x);
higedura	2:78c3d0598819	351	for( int i=0;i<N;i++ ){ k[i][0] = *pk1; pk1 = pk1+1; }
higedura	2:78c3d0598819	352
higedura	2:78c3d0598819	353	for( int i=0;i<N;i++){ yBuf[i] = y[i]+0.5dtk[i][1]; }
higedura	2:78c3d0598819	354	pk2 = func (yBuf,x);
higedura	2:78c3d0598819	355	for( int i=0;i<N;i++ ){ k[i][1] = *pk2; pk2 = pk2+1; }
higedura	2:78c3d0598819	356
higedura	2:78c3d0598819	357	for( int i=0;i<N;i++){ yBuf[i] = y[i]+0.5dtk[i][2]; }
higedura	2:78c3d0598819	358	pk3 = func (yBuf,x);
higedura	2:78c3d0598819	359	for( int i=0;i<N;i++ ){ k[i][2] = *pk3; pk3 = pk3+1; }
higedura	2:78c3d0598819	360
higedura	2:78c3d0598819	361	for( int i=0;i<N;i++){ yBuf[i] = y[i]+dt*k[i][3]; }
higedura	2:78c3d0598819	362	pk4 = func (yBuf,x);
higedura	2:78c3d0598819	363	for( int i=0;i<N;i++ ){ k[i][3] = *pk4; pk4 = pk4+1; }
higedura	2:78c3d0598819	364
higedura	2:78c3d0598819	365	for( int i=0;i<N;i++){ y[i] = y[i]+dt(k[i][0]+2.0k[i][1]+2.0*k[i][2]+k[i][3])/6.0; }
higedura	2:78c3d0598819	366
higedura	2:78c3d0598819	367	return p_y;
higedura	2:78c3d0598819	368
higedura	2:78c3d0598819	369	}
higedura	2:78c3d0598819	370
higedura	2:78c3d0598819	371	double* func( double y[N], double x[N] ){
higedura	2:78c3d0598819	372
higedura	2:78c3d0598819	373	double f[N] = {0};
higedura	2:78c3d0598819	374	double* p_f = f;
higedura	2:78c3d0598819	375
higedura	2:78c3d0598819	376	f[0] = x[0]+x[1]sin(y[0])tan(y[1])+x[2]cos(y[0])tan(y[1]);
higedura	2:78c3d0598819	377	f[1] = x[1]cos(y[0])-x[2]sin(y[0]);
higedura	2:78c3d0598819	378	f[2] = x[1]sin(y[0])/cos(y[1])+x[2]cos(y[0])/cos(y[1]);
higedura	2:78c3d0598819	379
higedura	2:78c3d0598819	380	return p_f;
higedura	2:78c3d0598819	381
higedura	2:78c3d0598819	382	}
higedura	6:2f8a7136e326	383
higedura	6:2f8a7136e326	384	double* calib(){
higedura	6:2f8a7136e326	385
higedura	6:2f8a7136e326	386	int calib_loop = 1000;
higedura	6:2f8a7136e326	387
higedura	6:2f8a7136e326	388	int bit_acc[N] = {0}; // Buffer of the accelerometer
higedura	6:2f8a7136e326	389	int get_mag[N] = {0}; // Buffer of the compass
higedura	6:2f8a7136e326	390
higedura	6:2f8a7136e326	391	double calib_acc[N] = {0};
higedura	6:2f8a7136e326	392	double calib_gyro_buf[N] = {0};
higedura	6:2f8a7136e326	393	double calib_gyro[N] = {0};
higedura	6:2f8a7136e326	394	double compass_basis_buf[N] = {0};
higedura	6:2f8a7136e326	395	double compass_basis_rad = 0;
higedura	6:2f8a7136e326	396	double calib_result[7] = {0};
higedura	6:2f8a7136e326	397
higedura	6:2f8a7136e326	398	double* p_calib_result = calib_result;
higedura	6:2f8a7136e326	399
higedura	10:3cfc137e23e8	400	pc.printf("\n\r\n\rDon't touch... Calibrating now!!\n\r");
higedura	6:2f8a7136e326	401	led1 = 1;
higedura	6:2f8a7136e326	402
higedura	6:2f8a7136e326	403	for( int i=0;i<calib_loop;i++ ){
higedura	6:2f8a7136e326	404
higedura	6:2f8a7136e326	405	accelerometer.getOutput(bit_acc);
higedura	6:2f8a7136e326	406	compass.readData(get_mag);
higedura	6:2f8a7136e326	407
higedura	6:2f8a7136e326	408	calib_gyro_buf[0] = gyro.getGyroX();
higedura	6:2f8a7136e326	409	calib_gyro_buf[1] = gyro.getGyroY();
higedura	6:2f8a7136e326	410	calib_gyro_buf[2] = gyro.getGyroZ();
higedura	6:2f8a7136e326	411
higedura	6:2f8a7136e326	412	for( int j=0;j<N;j++ ){
higedura	6:2f8a7136e326	413	calib_acc[j] += (int16_t)bit_acc[j];
higedura	6:2f8a7136e326	414	calib_gyro[j] += calib_gyro_buf[j];
higedura	6:2f8a7136e326	415	compass_basis_buf[j] += (int16_t)get_mag[j];
higedura	6:2f8a7136e326	416	}
higedura	6:2f8a7136e326	417
higedura	6:2f8a7136e326	418	if( i>calib_loop*3/4 ){
higedura	6:2f8a7136e326	419	led4 = 1;
higedura	6:2f8a7136e326	420	}else if( i>calib_loop/2 ){
higedura	6:2f8a7136e326	421	led3 = 1;
higedura	6:2f8a7136e326	422	}else if( i>calib_loop/4 ){
higedura	6:2f8a7136e326	423	led2 = 1;
higedura	6:2f8a7136e326	424	}
higedura	6:2f8a7136e326	425
higedura	6:2f8a7136e326	426	}
higedura	6:2f8a7136e326	427
higedura	6:2f8a7136e326	428	for( int i=0;i<N;i++ ){
higedura	6:2f8a7136e326	429	calib_acc[i] = calib_acc[i]/calib_loop;
higedura	6:2f8a7136e326	430	calib_gyro[i] = calib_gyro[i]/calib_loop;
higedura	6:2f8a7136e326	431	compass_basis_buf[i] = compass_basis_buf[i]/calib_loop;
higedura	6:2f8a7136e326	432	}
higedura	6:2f8a7136e326	433
higedura	6:2f8a7136e326	434	compass_basis_rad = compass_basis_buf[1]/compass_basis_buf[0];
higedura	6:2f8a7136e326	435	compass_basis_rad = atan(compass_basis_rad);
higedura	6:2f8a7136e326	436	led1 = 0; led2 = 0; led3 = 0; led4 = 0;
higedura	6:2f8a7136e326	437
higedura	8:56fb656dcef6	438	pc.printf(" accX accY accZ gyroX gyroY gyroZ yaw_basis\n\r");
higedura	8:56fb656dcef6	439	pc.printf("%6.1f, %6.1f, %6.1f, %6.1f, %6.1f, %6.1f, %6.1f\n\r",calib_acc[0],calib_acc[1],calib_acc[2],calib_gyro[0],calib_gyro[1],calib_gyro[2],compass_basis_rad*180/pi);
higedura	6:2f8a7136e326	440
higedura	6:2f8a7136e326	441	for( int i=0;i<3;i++ ){ calib_result[i] = calib_acc[i]; }
higedura	6:2f8a7136e326	442	for( int i=3;i<6;i++ ){ calib_result[i] = calib_gyro[i-3]; }
higedura	6:2f8a7136e326	443	calib_result[6] = compass_basis_rad;
higedura	6:2f8a7136e326	444
higedura	6:2f8a7136e326	445	for( int i=0;i<3;i++ ){
higedura	6:2f8a7136e326	446	wait(0.5);
higedura	6:2f8a7136e326	447	led1 = 1; led2 = 1; led3 = 1; led4 = 1;
higedura	6:2f8a7136e326	448	wait(0.5);
higedura	6:2f8a7136e326	449	led1 = 0; led2 = 0; led3 = 0; led4 = 0;
higedura	6:2f8a7136e326	450	}
higedura	6:2f8a7136e326	451
higedura	6:2f8a7136e326	452	return p_calib_result;
higedura	6:2f8a7136e326	453
higedura	6:2f8a7136e326	454	}
higedura	6:2f8a7136e326	455
higedura	6:2f8a7136e326	456	int preparing_acc(){
higedura	6:2f8a7136e326	457
higedura	6:2f8a7136e326	458	// These are here to test whether any of the initialization fails. It will print the failure.
higedura	6:2f8a7136e326	459	if (accelerometer.setPowerControl(0x00)){
higedura	6:2f8a7136e326	460	pc.printf("didn't intitialize power control\n\r");
higedura	6:2f8a7136e326	461	return 0;
higedura	6:2f8a7136e326	462	}
higedura	6:2f8a7136e326	463	// Full resolution, +/-16g, 4mg/LSB.
higedura	6:2f8a7136e326	464	wait(.001);
higedura	6:2f8a7136e326	465
higedura	6:2f8a7136e326	466	if(accelerometer.setDataFormatControl(0x0B)){
higedura	6:2f8a7136e326	467	pc.printf("didn't set data format\n\r");
higedura	6:2f8a7136e326	468	return 0;
higedura	6:2f8a7136e326	469	}
higedura	6:2f8a7136e326	470	wait(.001);
higedura	6:2f8a7136e326	471
higedura	6:2f8a7136e326	472	// 3.2kHz data rate.
higedura	6:2f8a7136e326	473	if(accelerometer.setDataRate(ADXL345_3200HZ)){
higedura	6:2f8a7136e326	474	pc.printf("didn't set data rate\n\r");
higedura	6:2f8a7136e326	475	return 0;
higedura	6:2f8a7136e326	476	}
higedura	6:2f8a7136e326	477	wait(.001);
higedura	6:2f8a7136e326	478
higedura	6:2f8a7136e326	479	if(accelerometer.setPowerControl(MeasurementMode)) {
higedura	6:2f8a7136e326	480	pc.printf("didn't set the power control to measurement\n\r");
higedura	6:2f8a7136e326	481	return 0;
higedura	6:2f8a7136e326	482	}
higedura	11:b32b3d6be8d2	483	wait(.001);
higedura	11:b32b3d6be8d2	484
higedura	6:2f8a7136e326	485	return 0;
higedura	6:2f8a7136e326	486
higedura	6:2f8a7136e326	487	}

Repository toolbox

Export to desktop IDE

Build repository

Repository details

Type:	Program
Mbed OS support:	Mbed 2 deprecated
Created:	30 Jan 2012
Imports:	245
Forks:	0
Commits:	12
Dependents:	0
Dependencies:	1
Followers:	4

main.cpp@11:b32b3d6be8d2, 2012-03-15 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning