File content as of revision 7:ec00db826804:

#include "mbed.h"
#include "ITG3200.h"

#define N    3
#define pi   3.14159265

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

ITG3200 gyro(p9, p10);
Serial pc(USBTX, USBRX);

int main(){ 
    
    // keisan zyou no kizami haba
    float dt        =   0.01;
    // zissai ni wait suru zikan
    float dt_wait   =   0.01;
    // hyouzi you
    float t         =   0;    
    
    pc.baud(921600);
    
    // x = [ p,    q,    r ]
    // y = [ phi,  the,  psi ]
    double x_rad[N] =   {0};
    double x_deg[N] =   {0};
    double y_rad[N] =   {0};
    double y_deg[N] =   {0};
    
    double* pybuf;              // pointer for gyro    
    
    // *** start up ***
    gyro.setLpBandwidth(LPFBW_42HZ);
    wait(0.1);                        // Wait some time for all sensors (Need at least 5ms)
    // *** start up ***
    
    while(1){
        
        x_deg[0] =  (gyro.getGyroX())/14.375+9.4;
        x_deg[1] =  (gyro.getGyroY())/14.375-0.8;
        x_deg[2] =  (gyro.getGyroZ())/14.375-4;
        
        for ( int i=0;i<N;i++ ){    x_rad[i] = x_deg[i]*pi/180; }
        
        // RK4
        pybuf = RK4(dt,y_rad,x_rad);
        for ( int i=0;i<N;i++ ){    y_rad[i] = *pybuf;      pybuf = pybuf+1;    }
        
        for ( int i=0;i<N;i++ ){    y_deg[i] = y_rad[i]*180/pi; }
        
        pc.printf("%7.2f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f, %7.1f\n\r", t, x_deg[0], x_deg[1], x_deg[2], y_deg[0], y_deg[1], y_deg[2]);
        
        t += dt;
        wait(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]);
    
    wait(.000000001);
    
    return p_f;

}