IMU - | Mbed

Users » higedura » Code » IMU

main.cpp@0:b61f317f452e, 2012-01-30 (annotated)

Committer:: higedura
Date:: Mon Jan 30 08:02:30 2012 +0000
Revision:: 0:b61f317f452e
Child:: 1:aca0c191fb1c

Who changed what in which revision?

User	Revision	Line number	New contents of line
higedura	0:b61f317f452e	1	// IMU for Sparkfun 9DOF Sensor Stick
higedura	0:b61f317f452e	2
higedura	0:b61f317f452e	3	#include "ADXL345_I2C.h"
higedura	0:b61f317f452e	4	#include "ITG3200.h"
higedura	0:b61f317f452e	5	#include "HMC5883L.h"
higedura	0:b61f317f452e	6
higedura	0:b61f317f452e	7	ADXL345_I2C accelerometer(p9, p10);
higedura	0:b61f317f452e	8	ITG3200 gyro(p9, p10);
higedura	0:b61f317f452e	9	HMC5883L compass(p9, p10);
higedura	0:b61f317f452e	10	Serial pc(USBTX, USBRX);
higedura	0:b61f317f452e	11
higedura	0:b61f317f452e	12	#define N 3
higedura	0:b61f317f452e	13	#define pi 3.14159265
higedura	0:b61f317f452e	14
higedura	0:b61f317f452e	15	double* RK4( double, double[N], double[N] );
higedura	0:b61f317f452e	16	double* func( double[N], double[N] );
higedura	0:b61f317f452e	17
higedura	0:b61f317f452e	18	int main(){
higedura	0:b61f317f452e	19
higedura	0:b61f317f452e	20	float dt = 0.01;
higedura	0:b61f317f452e	21	float t = 0;
higedura	0:b61f317f452e	22
higedura	0:b61f317f452e	23	pc.baud(921600);
higedura	0:b61f317f452e	24
higedura	0:b61f317f452e	25	int bit_acc[N] = {0}; // Buffer of the accelerometer
higedura	0:b61f317f452e	26	int get_mag[N] = {0}; // Buffer of the compass
higedura	0:b61f317f452e	27
higedura	0:b61f317f452e	28	double acc [N] = {0};
higedura	0:b61f317f452e	29	double gyro_deg [N] = {0};
higedura	0:b61f317f452e	30	double gyro_rad [N] = {0};
higedura	0:b61f317f452e	31	int mag [N] = {0};
higedura	0:b61f317f452e	32
higedura	0:b61f317f452e	33	double ang_deg[N] = {0};
higedura	0:b61f317f452e	34	double ang_rad[N] = {0};
higedura	0:b61f317f452e	35
higedura	0:b61f317f452e	36	double* p_ang;
higedura	0:b61f317f452e	37
higedura	0:b61f317f452e	38	// * Setting up accelerometer *
higedura	0:b61f317f452e	39	// These are here to test whether any of the initialization fails. It will print the failure.
higedura	0:b61f317f452e	40	if (accelerometer.setPowerControl(0x00)){
higedura	0:b61f317f452e	41	pc.printf("didn't intitialize power control\n\r");
higedura	0:b61f317f452e	42	return 0;
higedura	0:b61f317f452e	43	}
higedura	0:b61f317f452e	44	// Full resolution, +/-16g, 4mg/LSB.
higedura	0:b61f317f452e	45	wait(.001);
higedura	0:b61f317f452e	46
higedura	0:b61f317f452e	47	if(accelerometer.setDataFormatControl(0x0B)){
higedura	0:b61f317f452e	48	pc.printf("didn't set data format\n\r");
higedura	0:b61f317f452e	49	return 0; }
higedura	0:b61f317f452e	50	wait(.001);
higedura	0:b61f317f452e	51
higedura	0:b61f317f452e	52	// 3.2kHz data rate.
higedura	0:b61f317f452e	53	if(accelerometer.setDataRate(ADXL345_3200HZ)){
higedura	0:b61f317f452e	54	pc.printf("didn't set data rate\n\r");
higedura	0:b61f317f452e	55	return 0;
higedura	0:b61f317f452e	56	}
higedura	0:b61f317f452e	57	wait(.001);
higedura	0:b61f317f452e	58
higedura	0:b61f317f452e	59	if(accelerometer.setPowerControl(MeasurementMode)) {
higedura	0:b61f317f452e	60	pc.printf("didn't set the power control to measurement\n\r");
higedura	0:b61f317f452e	61	return 0;
higedura	0:b61f317f452e	62	}
higedura	0:b61f317f452e	63	// * Setting up accelerometer *
higedura	0:b61f317f452e	64
higedura	0:b61f317f452e	65	// * Setting up gyro *
higedura	0:b61f317f452e	66	gyro.setLpBandwidth(LPFBW_42HZ);
higedura	0:b61f317f452e	67
higedura	0:b61f317f452e	68	// * Setting up compass *
higedura	0:b61f317f452e	69	compass.setDefault();
higedura	0:b61f317f452e	70
higedura	0:b61f317f452e	71	// Wait some time for all sensors (Need at least 5ms)
higedura	0:b61f317f452e	72	wait(0.1);
higedura	0:b61f317f452e	73
higedura	0:b61f317f452e	74	pc.printf("\n\rStarting ADXL345, ITG3200 and HMC5883L test...\n\r");
higedura	0:b61f317f452e	75	pc.printf(" Time AccX AccY AccZ GyroX GyroY GyroZ MagX MagY MagZ\n\r");
higedura	0:b61f317f452e	76
higedura	0:b61f317f452e	77	while(1){
higedura	0:b61f317f452e	78	// Updating accelerometer and compass
higedura	0:b61f317f452e	79	accelerometer.getOutput(bit_acc);
higedura	0:b61f317f452e	80	compass.readData(get_mag);
higedura	0:b61f317f452e	81
higedura	0:b61f317f452e	82	// Transfering units (Acc[g], Gyro[deg/s], Compass[Ga])
higedura	0:b61f317f452e	83	// Calibration YAL 9DOF acc : X+6, Y-12, Z+44
higedura	0:b61f317f452e	84	// Calibration green acc : X+1, Y-18, Z+45
higedura	0:b61f317f452e	85	acc[0] = ((int16_t)bit_acc[0]+1)*0.004;
higedura	0:b61f317f452e	86	acc[1] = ((int16_t)bit_acc[1]-18)*0.004;
higedura	0:b61f317f452e	87	acc[2] = ((int16_t)bit_acc[2]+45)*0.004;
higedura	0:b61f317f452e	88	// Calibration YAL 9DOF gyro : X+28, Y-45, Z+34
higedura	0:b61f317f452e	89	// Calibration green gyro : X+135, Y-12, Z-53
higedura	0:b61f317f452e	90	gyro_deg[0] = (gyro.getGyroX()+135)/14.375;
higedura	0:b61f317f452e	91	gyro_deg[1] = (gyro.getGyroY()-12)/14.375;
higedura	0:b61f317f452e	92	gyro_deg[2] = (gyro.getGyroZ()-48)/14.375;
higedura	0:b61f317f452e	93	// Calibration YAL 9DOF Compass:X, Y, Z
higedura	0:b61f317f452e	94	mag[0] = (int16_t)get_mag[0];
higedura	0:b61f317f452e	95	mag[1] = (int16_t)get_mag[1];
higedura	0:b61f317f452e	96	mag[2] = (int16_t)get_mag[2];
higedura	0:b61f317f452e	97
higedura	0:b61f317f452e	98	// Low pass filter for acc
higedura	0:b61f317f452e	99	for ( int i=0;i<N;i++ ){ if( -0.05<acc[i] && acc[i]<0.05 ){ acc[i]=0; } }
higedura	0:b61f317f452e	100
higedura	0:b61f317f452e	101	// Low pass filter for gyro
higedura	0:b61f317f452e	102	for ( int i=0;i<N;i++ ){ if( -0.5<gyro_deg[i] && gyro_deg[i]<0.5 ){ gyro_deg[i]=0; } }
higedura	0:b61f317f452e	103
higedura	0:b61f317f452e	104	for ( int i=0;i<N;i++ ){ gyro_rad[i] = gyro_deg[i]*pi/180; }
higedura	0:b61f317f452e	105
higedura	0:b61f317f452e	106	// RK4 (Prediction)
higedura	0:b61f317f452e	107	p_ang = RK4(dt,ang_rad,gyro_rad);
higedura	0:b61f317f452e	108	for ( int i=0;i<N;i++ ){ ang_rad[i] = *p_ang; p_ang = p_ang+1; }
higedura	0:b61f317f452e	109
higedura	0:b61f317f452e	110	for ( int i=0;i<N;i++ ){ ang_deg[i] = ang_rad[i]*180/pi; }
higedura	0:b61f317f452e	111	//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	0:b61f317f452e	112	//pc.printf("%7.2f, %7.3f, %7.3f, %7.3f, %7.1f, %7.1f, %7.1f\n\r", t, acc[0], acc[1], acc[2], gyro_deg[0], gyro_deg[1], gyro_deg[2]);
higedura	0:b61f317f452e	113	pc.printf("%7.2f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f\n\r", t, acc[0], acc[1], acc[2], gyro_deg[0], gyro_deg[1], gyro_deg[2], ang_deg[0], ang_deg[1], ang_deg[2]);
higedura	0:b61f317f452e	114
higedura	0:b61f317f452e	115	wait(dt);
higedura	0:b61f317f452e	116	t += dt;
higedura	0:b61f317f452e	117
higedura	0:b61f317f452e	118	}
higedura	0:b61f317f452e	119	}
higedura	0:b61f317f452e	120
higedura	0:b61f317f452e	121
higedura	0:b61f317f452e	122	double* RK4( double dt, double y[N], double x[N] ){
higedura	0:b61f317f452e	123
higedura	0:b61f317f452e	124	double yBuf[N] = {0};
higedura	0:b61f317f452e	125	double k[N][4] = {0};
higedura	0:b61f317f452e	126
higedura	0:b61f317f452e	127	double* p_y = y;
higedura	0:b61f317f452e	128
higedura	0:b61f317f452e	129	double* pk1;
higedura	0:b61f317f452e	130	double* pk2;
higedura	0:b61f317f452e	131	double* pk3;
higedura	0:b61f317f452e	132	double* pk4;
higedura	0:b61f317f452e	133
higedura	0:b61f317f452e	134	for ( int i=0;i<N;i++){ yBuf[i] = y[i]; }
higedura	0:b61f317f452e	135	pk1 = func (yBuf,x);
higedura	0:b61f317f452e	136	for ( int i=0;i<N;i++ ){ k[i][0] = *pk1; pk1 = pk1+1; }
higedura	0:b61f317f452e	137
higedura	0:b61f317f452e	138	for ( int i=0;i<N;i++){ yBuf[i] = y[i]+0.5dtk[i][1]; }
higedura	0:b61f317f452e	139	pk2 = func (yBuf,x);
higedura	0:b61f317f452e	140	for ( int i=0;i<N;i++ ){ k[i][1] = *pk2; pk2 = pk2+1; }
higedura	0:b61f317f452e	141
higedura	0:b61f317f452e	142	for ( int i=0;i<N;i++){ yBuf[i] = y[i]+0.5dtk[i][2]; }
higedura	0:b61f317f452e	143	pk3 = func (yBuf,x);
higedura	0:b61f317f452e	144	for ( int i=0;i<N;i++ ){ k[i][2] = *pk3; pk3 = pk3+1; }
higedura	0:b61f317f452e	145
higedura	0:b61f317f452e	146	for ( int i=0;i<N;i++){ yBuf[i] = y[i]+dt*k[i][3]; }
higedura	0:b61f317f452e	147	pk4 = func (yBuf,x);
higedura	0:b61f317f452e	148	for ( int i=0;i<N;i++ ){ k[i][3] = *pk4; pk4 = pk4+1; }
higedura	0:b61f317f452e	149
higedura	0:b61f317f452e	150	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	0:b61f317f452e	151
higedura	0:b61f317f452e	152	return p_y;
higedura	0:b61f317f452e	153
higedura	0:b61f317f452e	154	}
higedura	0:b61f317f452e	155
higedura	0:b61f317f452e	156
higedura	0:b61f317f452e	157	double* func( double y[N], double x[N] ){
higedura	0:b61f317f452e	158
higedura	0:b61f317f452e	159	double f[N] = {0};
higedura	0:b61f317f452e	160	double* p_f = f;
higedura	0:b61f317f452e	161
higedura	0:b61f317f452e	162	f[0] = x[0]+x[1]sin(y[0])tan(y[1])+x[2]cos(y[0])tan(y[1]);
higedura	0:b61f317f452e	163	f[1] = x[1]cos(y[0])-x[2]sin(y[0]);
higedura	0:b61f317f452e	164	f[2] = x[1]sin(y[0])/cos(y[1])+x[2]cos(y[0])/cos(y[1]);
higedura	0:b61f317f452e	165
higedura	0:b61f317f452e	166	return p_f;
higedura	0:b61f317f452e	167
higedura	0:b61f317f452e	168	}

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@0:b61f317f452e, 2012-01-30 (annotated)

Who changed what in which revision?

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning