File content as of revision 3:502b83f7761c:

#include "mbed.h"
#include "LSM9DS0_SH.h"
#define pi  3.141592f
#define d2r 0.01745329f
#define Rms 5000
#define dt  0.005f
#define constrain(amt,low,high) ((amt)<(low)?(low):((amt)>(high)?(high):(amt)))


//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓GPIO registor↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
//~~~structure~~~//
DigitalOut  led(D13);           //detection
DigitalOut  TT_ext(D12);

//~~~IMU_SPI~~~//
DigitalOut  SPI_CSG(D7,1);      //low for GYRO enable
DigitalOut  SPI_CSXM(D6,1);     //low for ACC/MAG enable
SPI spi(D4, D5, D3);            //MOSI MISO SCLK

//~~~Serial~~~//
Serial      pc(D1, D0);         //Serial reg(TX RX)
//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of GPIO registor↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//



//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓Varible registor↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
//~~~globle~~~//
Ticker  TT;                         //call a timer
int     count = 0;                  //one second counter for extrenal led blink

//~~~IMU_SPI~~~//
short low_byte = 0x00;              //buffer
short high_byte = 0x00;
short Buff = 0x00;
float Wx = 0.0;
float Wy = 0.0;
float Wz = 0.0;
float Ax = 0.0;
float Ay = 0.0;
float Az = 0.0;
float gDIR[1][3] = {                //g vector's direction , required unitconstrain
    {0,0,-1},                       //X Y Z
};
float gUnity = 0;
//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of Varible registor↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//



//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓Function registor↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
void init_TIMER();          //set TT_main() rate
void TT_main();             //timebase function rated by TT
void init_IO();             //initialize IO state
void init_IMU();            //initialize IMU
void read_IMU();            //read IMU data give raw data
void state_update();        //estimation of new attitude
//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of Function registor↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//



//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓main funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
int main()
{
    init_IO();                      //initialized value
    init_IMU();                     //initialize IMU
    init_TIMER();                   //start TT_main
    pc.baud(115200);                //set baud rate

    while(1) {                      //main() loop
        if(count >= 200) {          //check if main working
            count=0;
            led = !led;
        }
    }

}
//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of main funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//



//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓Timebase funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
void init_TIMER()                   //set TT_main{} rate
{
    TT.attach_us(&TT_main, Rms);
}
void TT_main()                      //interrupt function by TT
{
    TT_ext = !TT_ext;               //indicate TT_main() function working
    count = count+1;                //one second counter

    read_IMU();                     //read IMU data give raw data
    state_update();                 //estimation of new attitude

    pc.printf("%.2f\t%.2f\t%.2f\n", gDIR[0][0], gDIR[0][1], gDIR[0][2]);
//    pc.printf("%.2f\t%.2f\n", Sele, Srol);
//    pc.printf("%.2f\n", Sx);
}
//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of Timebase funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//



//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓init_IO funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
void init_IO(void)                  //initialize
{
    TT_ext = 0;
    led = 1;
}
//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of init_IO funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//



//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓init_IMU funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
void init_IMU(void)                 //initialize
{
    //gloable config
    SPI_CSXM = 1;                   //high as init for disable SPI
    SPI_CSG = 1;
    spi.format(8, 0);               //byte width, spi mode
    spi.frequency(4000000);         //8MHz

    //for GYRO config
    SPI_CSG = 0;                    //start spi talking
    spi.write(CTRL_REG1_G);
    spi.write(0x9F);                //data rate 380 Hz/ cut off 25 Hz
    SPI_CSG = 1;                    //end spi talking

    SPI_CSG = 0;                    //start spi talking
    spi.write(CTRL_REG4_G);
    spi.write(0x10);                //Scle 500dps
    SPI_CSG = 1;                    //end spi talking

    //for ACC config
    SPI_CSXM = 0;                   //start spi talking
    spi.write(CTRL_REG1_XM);
    spi.write(0x87);                //data rate 400 Hz/ Enable
    SPI_CSXM = 1;                   //end spi talking

    SPI_CSXM = 0;                   //start spi talking
    spi.write(CTRL_REG2_XM);
    spi.write(0xC8);                //cut off 50 Hz/ Scale +-4g
    SPI_CSXM = 1;                   //end spi talking
}
//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of init_IMU funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//



//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓read_IMU funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
void read_IMU(void)                 //read IMU data give raw data
{
    //Wx
    SPI_CSG = 0;                    //start spi talking Wx
    spi.write(0xE8);                //read B11101000 read/multi/address
    low_byte = spi.write(0);
    high_byte = spi.write(0);
    Buff = high_byte << 8 |low_byte;
    SPI_CSG = 1;                    //end spi talking
    Wx = Buff * 2.663e-4; //1.526e-2 * d2r =
    //Wy
    SPI_CSG = 0;                    //start spi talking Wx
    spi.write(0xEA);                //read B11101010 read/multi/address
    low_byte = spi.write(0);
    high_byte = spi.write(0);
    Buff = high_byte << 8 |low_byte;
    SPI_CSG = 1;                    //end spi talking
    Wy = Buff * 2.663e-4; //1.526e-2 * d2r =
    //Wz
    SPI_CSG = 0;                    //start spi talking Wx
    spi.write(0xEC);                //read B11101100 read/multi/address
    low_byte = spi.write(0);
    high_byte = spi.write(0);
    Buff = high_byte << 8 |low_byte;
    SPI_CSG = 1;                    //end spi talking
    Wz = Buff * 2.663e-4; //1.526e-2 * d2r =
    //Ax
    SPI_CSXM = 0;                   //start spi talking Ax
    spi.write(0xE8);                //read B11101000 read/multi/address
    low_byte = spi.write(0);
    high_byte = spi.write(0);
    Buff = high_byte << 8 |low_byte;
    SPI_CSXM = 1;                   //end spi talking
    Ax = Buff * 1.22e-4;
    //Ay
    SPI_CSXM = 0;                   //start spi talking Ax
    spi.write(0xEA);                //read B11101010 read/multi/address
    low_byte = spi.write(0);
    high_byte = spi.write(0);
    Buff = high_byte << 8 |low_byte;
    SPI_CSXM = 1;                   //end spi talking
    Ay = Buff * 1.22e-4;
    //Az
    SPI_CSXM = 0;                   //start spi talking Ax
    spi.write(0xEC);                //read B11101100 read/multi/address
    low_byte = spi.write(0);
    high_byte = spi.write(0);
    Buff = high_byte << 8 |low_byte;
    SPI_CSXM = 1;                   //end spi talking
    Az = Buff * 1.22e-4;
}
//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of read_IMU funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//



//↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓state_update funtion↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓↓//
void state_update(void)         //estimation of new attitude
{
    //pridict
    gDIR[0][0] = gDIR[0][0] - (Wy*gDIR[0][2] - Wz*gDIR[0][1])*dt;
    gDIR[0][1] = gDIR[0][1] - (Wz*gDIR[0][0] - Wx*gDIR[0][2])*dt;
    gDIR[0][2] = gDIR[0][2] - (Wx*gDIR[0][1] - Wy*gDIR[0][0])*dt;
    
    //update
    
    
    //nutralizing
    gUnity = gDIR[0][0]*gDIR[0][0] + gDIR[0][1]*gDIR[0][1] + gDIR[0][2]*gDIR[0][2];
    for(int i=0; i<3; i++) {
        gDIR[0][i] =  gDIR[0][i] / gUnity;
    }
}
//↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑end of state_update funtion↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑↑//