File content as of revision 0:b61f317f452e:

// IMU for Sparkfun 9DOF Sensor Stick

#include "ADXL345_I2C.h"
#include "ITG3200.h"
#include "HMC5883L.h"

ADXL345_I2C accelerometer(p9, p10);
ITG3200 gyro(p9, p10);
HMC5883L compass(p9, p10);
Serial pc(USBTX, USBRX);

#define N		3
#define pi		3.14159265

double* RK4( double, double[N], double[N] );
double* func( double[N], double[N] );

int main(){ 
    
    float dt = 0.01;
    float t  = 0;    

    pc.baud(921600);

    int    bit_acc[N]   =   {0};    // Buffer of the accelerometer
    int    get_mag[N]   =   {0};    // Buffer of the compass

    double acc  [N]     =   {0};
    double gyro_deg [N] =   {0};
    double gyro_rad [N] =   {0};
    int    mag  [N]     =   {0};

	double ang_deg[N]	=	{0};
	double ang_rad[N]	=	{0};
	
	double* p_ang;
    
    // *** Setting up accelerometer ***
    // These are here to test whether any of the initialization fails. It will print the failure.
    if (accelerometer.setPowerControl(0x00)){
        pc.printf("didn't intitialize power control\n\r"); 
        return 0;
    }
    // Full resolution, +/-16g, 4mg/LSB.
    wait(.001);
     
    if(accelerometer.setDataFormatControl(0x0B)){
        pc.printf("didn't set data format\n\r");
        return 0;  }
    wait(.001);
     
    // 3.2kHz data rate.
    if(accelerometer.setDataRate(ADXL345_3200HZ)){
        pc.printf("didn't set data rate\n\r");
        return 0;
    }
    wait(.001);
      
    if(accelerometer.setPowerControl(MeasurementMode)) {
        pc.printf("didn't set the power control to measurement\n\r"); 
        return 0;
    } 
    // ***  Setting up accelerometer ***
    
    // ***  Setting up gyro ***
    gyro.setLpBandwidth(LPFBW_42HZ);

    // ***  Setting up compass ***
    compass.setDefault();
    
    // Wait some time for all sensors (Need at least 5ms)
    wait(0.1);
    
    pc.printf("\n\rStarting ADXL345, ITG3200 and HMC5883L test...\n\r");
    pc.printf("   Time     AccX     AccY     AccZ    GyroX    GyroY    GyroZ   MagX   MagY   MagZ\n\r");
                
    while(1){
        // Updating accelerometer and compass
        accelerometer.getOutput(bit_acc);
        compass.readData(get_mag);
        
        // Transfering units (Acc[g], Gyro[deg/s], Compass[Ga])
        // Calibration YAL 9DOF acc : X+6, Y-12, Z+44
        // Calibration green acc : X+1, Y-18, Z+45
        acc[0]      =  ((int16_t)bit_acc[0]+1)*0.004;
        acc[1]      =  ((int16_t)bit_acc[1]-18)*0.004;
        acc[2]      =  ((int16_t)bit_acc[2]+45)*0.004;
        // Calibration YAL 9DOF gyro : X+28, Y-45, Z+34 
        // Calibration green gyro : X+135, Y-12, Z-53 
        gyro_deg[0] =  (gyro.getGyroX()+135)/14.375;
        gyro_deg[1] =  (gyro.getGyroY()-12)/14.375;
        gyro_deg[2] =  (gyro.getGyroZ()-48)/14.375;
        // Calibration YAL 9DOF Compass:X, Y, Z
        mag[0]      =  (int16_t)get_mag[0];
        mag[1]      =  (int16_t)get_mag[1];
        mag[2]      =  (int16_t)get_mag[2];
        
        // Low pass filter for acc
        for ( int i=0;i<N;i++ ){    if( -0.05<acc[i] && acc[i]<0.05 ){    acc[i]=0;  }  }
        
        // Low pass filter for gyro
        for ( int i=0;i<N;i++ ){    if( -0.5<gyro_deg[i] && gyro_deg[i]<0.5 ){    gyro_deg[i]=0;  } }
        
        for ( int i=0;i<N;i++ ){	gyro_rad[i] = gyro_deg[i]*pi/180;	}
        
   		// RK4 (Prediction)
		p_ang = RK4(dt,ang_rad,gyro_rad);
		for ( int i=0;i<N;i++ ){	ang_rad[i] = *p_ang;		p_ang = p_ang+1;	}
        
        for ( int i=0;i<N;i++ ){	ang_deg[i] = ang_rad[i]*180/pi;	}
        //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]);
        //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]);
        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]);
   		
   		wait(dt);
        t += dt;
        
    }
}


double* RK4( double dt, double y[N], double x[N] ){
	
	double yBuf[N]	=	{0};
	double k[N][4]	=	{0};

	double* p_y	=	y;

	double* pk1;
	double* pk2;
	double* pk3;
	double* pk4;

		for ( int i=0;i<N;i++){	yBuf[i]	= y[i];	}
		pk1	=	func (yBuf,x);
		for ( int i=0;i<N;i++ ){	k[i][0] = *pk1;		pk1	= pk1+1;	}

		for ( int i=0;i<N;i++){	yBuf[i]	= y[i]+0.5*dt*k[i][1];	}
		pk2	=	func (yBuf,x);
		for ( int i=0;i<N;i++ ){	k[i][1]	= *pk2;		pk2	= pk2+1;	}

		for ( int i=0;i<N;i++){	yBuf[i]	= y[i]+0.5*dt*k[i][2];	}
		pk3	=	func (yBuf,x);
		for ( int i=0;i<N;i++ ){	k[i][2]	= *pk3;		pk3	= pk3+1;	}

		for ( int i=0;i<N;i++){	yBuf[i]	= y[i]+dt*k[i][3];	}
		pk4	=	func (yBuf,x);
		for ( int i=0;i<N;i++ ){	k[i][3]	= *pk4;		pk4 = pk4+1;	}

		for ( int i=0;i<N;i++){	y[i] = y[i]+dt*(k[i][0]+2.0*k[i][1]+2.0*k[i][2]+k[i][3])/6.0;	}

	return p_y;

}


double* func( double y[N], double x[N] ){

	double f[N]	=	{0};
	double* p_f	=	f;
	
	f[0] = x[0]+x[1]*sin(y[0])*tan(y[1])+x[2]*cos(y[0])*tan(y[1]);
	f[1] = x[1]*cos(y[0])-x[2]*sin(y[0]);
	f[2] = x[1]*sin(y[0])/cos(y[1])+x[2]*cos(y[0])/cos(y[1]);
        
	return p_f;

}