File content as of revision 5:83a3021f51f8:

#include "mbed.h"
#include "mma7361.h"

/*---------------------------------description-----------------------------------------*/

//    range=0 : range is 1.5G
//    range=1 : range is 6.0G

/*-------------------------------about mat[3][3]---------------------------------------*/

/*
      | mat[0][0] mat[0][1] mat[0][2] |   | A00 A10 A20 |   : middle bits (=0G) of each axes when range is 1.5G
      | mat[1][0] mat[1][1] mat[1][2] | = | A01 A11 A21 |   : middle bits (=0G) of each axes when range is 6.0G
      | mat[2][0] mat[2][1] mat[2][2] |   | G00 G10 G20 |   : bits/G of each axes when range is 1.5G
      | mat[3][0] mat[3][1] mat[3][2] |   | G01 G11 G21 |   : bits/G of each axes when range is 6.0G
      
      You mast change this value in "mma7361.h". You can see this value with function : calibration() & show_mat()
*/

/*---------------------------------description-----------------------------------------*/
/*---------------------------------constracta------------------------------------------*/

mma7361::mma7361(PinName accelx,PinName accely,PinName accelz,PinName set,PinName set_range): dx(accelx),dy(accely),dz(accelz),ST(set),GS(set_range),pcdev(USBTX,USBRX)
{
    mma7361::ST=0;
    mma7361::GS=0;
    mma7361::range=0;
    mma7361::clear_mat();
}

/*---------------------------------constracta------------------------------------------*/
/*-----------------------------------member--------------------------------------------*/

void mma7361::calibration(){
    mma7361::GS=0;
    mma7361::range=0;
    mma7361::ST=1;
    wait_ms(250);
    mma7361::ST=0;
    
    mma7361::pcdev.printf("Prease set devise with z-axis is going up\n\r");
    mma7361::pcdev.printf("Press Enter key\n\r");
    while(getchar()==-35);
    int ax_0_0 = mma7361::read_bit(x_axis);
    int ay_0_0 = mma7361::read_bit(y_axis);
    int gz_0_0 = mma7361::read_bit(z_axis);
    for(int j=0; j<100; j++){
        mma7361::low_pass_filter(&ax_0_0,x_axis);
        mma7361::low_pass_filter(&ay_0_0,y_axis);
        mma7361::low_pass_filter(&gz_0_0,z_axis);
    }
    mma7361::GS=1;
    wait_ms(250);
    int ax_1_0 = mma7361::read_bit(x_axis);
    int ay_1_0 = mma7361::read_bit(y_axis);
    int gz_1_0 = mma7361::read_bit(z_axis);
    for(int j=0; j<100; j++){
        mma7361::low_pass_filter(&ax_1_0,x_axis);
        mma7361::low_pass_filter(&ay_1_0,y_axis);
        mma7361::low_pass_filter(&gz_1_0,z_axis);
    }
    mma7361::GS=0;
    wait_ms(250);
    
    mma7361::pcdev.printf("Prease set devise with x-axis is going up\n\r");
    mma7361::pcdev.printf("Press Enter key\n\r");
    while(getchar()==-35);
    int ay_0_1 = mma7361::read_bit(y_axis);
    int az_0_0 = mma7361::read_bit(z_axis);
    int gx_0_0 = mma7361::read_bit(x_axis);
    for(int j=0; j<100; j++){
        mma7361::low_pass_filter(&ay_0_1,y_axis);
        mma7361::low_pass_filter(&az_0_0,z_axis);
        mma7361::low_pass_filter(&gx_0_0,x_axis);
    }
    mma7361::GS=1;
    wait_ms(250);
    int ay_1_1 = mma7361::read_bit(y_axis);
    int az_1_0 = mma7361::read_bit(z_axis);
    int gx_1_0 = mma7361::read_bit(x_axis);
    for(int j=0; j<100; j++){
        mma7361::low_pass_filter(&ay_1_1,y_axis);
        mma7361::low_pass_filter(&az_1_0,z_axis);
        mma7361::low_pass_filter(&gx_1_0,x_axis);
    }
    mma7361::GS=0;
    wait_ms(250);
    
    mma7361::pcdev.printf("Prease set devise with y-axis is going up\n\r");
    mma7361::pcdev.printf("Press Enter key\n\r");
    while(getchar()==-35);
    int ax_0_1 = mma7361::read_bit(x_axis);
    int az_0_1 = mma7361::read_bit(z_axis);
    int gy_0_0 = mma7361::read_bit(y_axis);
    for(int j=0; j<100; j++){
        mma7361::low_pass_filter(&ax_0_1,x_axis);
        mma7361::low_pass_filter(&az_0_1,z_axis);
        mma7361::low_pass_filter(&gy_0_0,y_axis);
    }
    mma7361::GS=1;
    wait_ms(250);
    int ax_1_1 = mma7361::read_bit(x_axis);
    int az_1_1 = mma7361::read_bit(z_axis);
    int gy_1_0 = mma7361::read_bit(y_axis);
    for(int j=0; j<100; j++){
        mma7361::low_pass_filter(&ax_1_1,x_axis);
        mma7361::low_pass_filter(&az_1_1,z_axis);
        mma7361::low_pass_filter(&gy_1_0,y_axis);
    }
    mma7361::GS=0;
    wait_ms(250);
    
    ax_0_0=(ax_0_0+ax_0_1)/2;
    ay_0_0=(ay_0_0+ay_0_1)/2;
    az_0_0=(az_0_0+az_0_1)/2;
    ax_1_0=(ax_1_0+ax_1_1)/2;
    ay_1_0=(ay_1_0+ay_1_1)/2;
    az_1_0=(az_1_0+az_1_1)/2;
    gx_0_0-=ax_0_0;
    gy_0_0-=ay_0_0;
    gz_0_0-=az_0_0;
    gx_1_0-=ax_0_1;
    gy_1_0-=ay_0_1;
    gz_1_0-=az_0_1;
    
    mma7361::set_each_num(ax_0_0,ay_0_0,az_0_0,ax_1_0,ay_1_0,az_1_0,gx_0_0,gy_0_0,gz_0_0,gx_1_0,gy_1_0,gz_1_0,0);
    wait_ms(100);
}

void mma7361::set_each_num(int ax_0_0,int ay_0_0,int az_0_0,int ax_1_0,int ay_1_0,int az_1_0,int gx_0_0,int gy_0_0,int gz_0_0,int gx_1_0,int gy_1_0,int gz_1_0,int new_range){
    //def or -1 -> don' change value
    mma7361::GS=new_range;
    mma7361::range=new_range;
    int temp[12];
    temp[0]=ax_0_0;
    temp[1]=ay_0_0;
    temp[2]=az_0_0;
    temp[3]=ax_1_0;
    temp[4]=ay_1_0;
    temp[5]=az_1_0;
    temp[6]=gx_0_0;
    temp[7]=gy_0_0;
    temp[8]=gz_0_0;
    temp[9]=gx_1_0;
    temp[10]=gy_1_0;
    temp[11]=gz_1_0;
    for(int i=0; i<4; i++){
        for(int j=0; j<3; j++){
            if(temp[i*3+j]!=def){
                mma7361::mat[i][j]=temp[i*3+j];
            }
        }
    }
}

void mma7361::set_num(int (*new_mat)[3],int new_range){
    mma7361::GS=new_range;
    mma7361::range=new_range;
    for(int i=0; i<4; i++){
        for(int j=0; j<3; j++){
            if(new_mat[i][j]!=def){
                mma7361::mat[i][j]=new_mat[i][j];
            }
        }
    }
}

void mma7361::set_range(int new_range){
    mma7361::GS=new_range;
    mma7361::range=new_range;
}

void mma7361::clear_mat(){
    mma7361::set_each_num(A00,A10,A20,A01,A11,A21,G00,G10,G20,G01,G11,G21,mma7361::range);
}

int mma7361::read_bit(int axis){
    int v;
    switch(axis){
    case 0:
        v=mma7361::dx.read_u16();
        break;
    case 1:
        v=mma7361::dy.read_u16();
        break;
    case 2:
        v=mma7361::dz.read_u16();
        break;
    default:
        v=def;
        break;
    }
    return v;
}

int mma7361::read_bit_0(int axis){
    int v;
    v=mma7361::read_bit(axis);
    v-=mat[mma7361::range][axis];
    return v;
}

float mma7361::read_by_g(int axis){
    float v;
    v=mma7361::read_bit_0(axis);
    v/=mat[mma7361::range+2][axis];
    return v;
}
    
void mma7361::low_pass_filter(int *int_bit,int axis){
    *int_bit=*int_bit*0.9+mma7361::read_bit(axis)*0.1;
}

void mma7361::show_mat(){
    for(int i=0; i<4; i++){
        for(int j=0; j<3; j++){
            mma7361::pcdev.printf("%d ",mat[i][j]);
        }
        mma7361::pcdev.printf("\n\r");
    }
}

float mma7361::bit_to_g(int bit,int axis){
    float v=0;
    v=bit-mma7361::mat[mma7361::range][axis];
    v/=mma7361::mat[mma7361::range+2][axis];
    return v;
}

void mma7361::set_1st_bit(int *variable,int axis,float direction_G){
    *variable=mma7361::mat[mma7361::range][axis]+mma7361::mat[mma7361::range+2][axis]*direction_G;
}


float mma7361::rotate(int *a_value,int a_axis,int *b_value,int b_axis){
    float ga,gb,g;
    ga=mma7361::bit_to_g(*a_value,a_axis);
    gb=mma7361::bit_to_g(*b_value,b_axis);
    if(abs(ga)<THHO){
        ga=0;
    }
    if(abs(gb)<THHO){
        return 0;
    }else{
        g=ga/gb;
        g=atan(g);
        return g;
    }
}

float mma7361::rad_to_deg(float rad){
    float deg;
    deg=rad*180/PI;
    return deg;
}