File content as of revision 2:306058b9d04e:

#include"Hepta9axis.h"
#include"mbed.h"

Hepta9axis::Hepta9axis(PinName sda, PinName scl, int aaddr, int agaddr) : _9axis(sda,scl),addr_accel_gyro(aaddr),addr_compus(agaddr)
{
    _9axis.frequency(100000);
    cmd[0]=0x6B;
    cmd[1]=0x00;
    _9axis.write(addr_accel_gyro,cmd,2);
    cmd[0] = 0x37;
    cmd[1] = 0x02;
    _9axis.write(addr_accel_gyro,cmd,2);
    _9axis.stop();
}

void Hepta9axis::setup()
{

    _9axis.frequency(100000);
    cmd[0]=0x6B;
    cmd[1]=0x00;
    _9axis.write(addr_accel_gyro,cmd,2);
    cmd[0] = 0x37;
    cmd[1] = 0x02;
    _9axis.write(addr_accel_gyro,cmd,2);
    _9axis.stop();
}

void Hepta9axis::sen_acc(float *ax,float *ay,float *az)
{
//x軸加速度
    _9axis.start();
    _9axis.write(addr_accel_gyro);
    _9axis.write(0x3B);
    _9axis.start();
    _9axis.write(addr_accel_gyro|0x01);
    xh = _9axis.read(0);
    _9axis.stop();

    _9axis.start();
    _9axis.write(addr_accel_gyro);
    _9axis.write(0x3C);
    _9axis.start();
    _9axis.write(addr_accel_gyro|0x01);
    xl = _9axis.read(0);
    _9axis.stop();

    double  acc_ax = short((xh<<8) | (xl));
    *ax = (acc_ax)*2/32764*9.81;


//Y軸加速度
    _9axis.start();
    _9axis.write(addr_accel_gyro);
    _9axis.write(0x3D);
    _9axis.start();
    _9axis.write(addr_accel_gyro|0x01);
    yh = _9axis.read(0);

    _9axis.stop();
    _9axis.start();
    _9axis.write(addr_accel_gyro);
    _9axis.write(0x3E);
    _9axis.start();
    _9axis.write(addr_accel_gyro|0x01);
    yl = _9axis.read(0);

    _9axis.stop();
    double  acc_ay = short((yh<<8) | (yl));
    *ay = (acc_ay)*2/32764*9.81;


//z軸加速度
    _9axis.start();
    _9axis.write(addr_accel_gyro);
    _9axis.write(0x3F);
    _9axis.start();
    _9axis.write(addr_accel_gyro|0x01);
    zh = _9axis.read(0);
    _9axis.stop();

    _9axis.start();
    _9axis.write(addr_accel_gyro);
    _9axis.write(0x40);
    _9axis.start();
    _9axis.write(addr_accel_gyro|0x01);
    zl = _9axis.read(0);
    _9axis.stop();

    double  acc_az = short((zh<<8) | (zl));
    *az = (acc_az)*2/32764*9.81-0.1;

}

void Hepta9axis::sen_gyro(float *gx,float *gy,float *gz)
{
//x軸GYRO
    _9axis.start();
    _9axis.write(addr_accel_gyro);
    _9axis.write(0x43);
    _9axis.start();
    _9axis.write(addr_accel_gyro|0x01);
    gxh = _9axis.read(0);
    _9axis.stop();

    _9axis.start();
    _9axis.write(addr_accel_gyro);
    _9axis.write(0x44);
    _9axis.start();
    _9axis.write(addr_accel_gyro|0x01);
    gxl = _9axis.read(0);
    _9axis.stop();

    double  gyro_ax = short((gxh<<8) | (gxl));
    *gx = (gyro_ax)*0.00763-1.6;

//y軸GYRO
    _9axis.start();
    _9axis.write(addr_accel_gyro);
    _9axis.write(0x45);
    _9axis.start();
    _9axis.write(addr_accel_gyro|0x01);
    gyh = _9axis.read(0);
    _9axis.stop();

    _9axis.start();
    _9axis.write(addr_accel_gyro);
    _9axis.write(0x46);
    _9axis.start();
    _9axis.write(addr_accel_gyro|0x01);
    gyl = _9axis.read(0);
    _9axis.stop();

    double  gyro_ay = short((gyh<<8) | (gyl));
    *gy = (gyro_ay)*0.00763;

//z軸GYRO
    _9axis.start();
    _9axis.write(addr_accel_gyro);
    _9axis.write(0x47);
    _9axis.start();
    _9axis.write(addr_accel_gyro|0x01);
    gzh = _9axis.read(0);
    _9axis.stop();

    _9axis.start();
    _9axis.write(addr_accel_gyro);
    _9axis.write(0x48);
    _9axis.start();
    _9axis.write(addr_accel_gyro|0x01);
    gzl = _9axis.read(0);
    _9axis.stop();

    double  gyro_az = short((gzh<<8) | (gzl));
    *gz = (gyro_az)*0.00763;

}

void Hepta9axis::sen_mag(float *mx,float *my,float *mz)
{

    _9axis.start();
    _9axis.write(addr_compus);
    _9axis.write(0x0A);
    _9axis.write(0x12);
    _9axis.stop();

    _9axis.start();
    _9axis.write(addr_compus);
    _9axis.write(0x03);
    _9axis.start();
    _9axis.write(addr_compus|0x01);
    mxl = _9axis.read(0);
    _9axis.stop();
    _9axis.start();
    _9axis.write(addr_compus);
    _9axis.write(0x04);
    _9axis.start();
    _9axis.write(addr_compus|0x01);
    mxh = _9axis.read(0);
    _9axis.stop();
    /*
     _9axis.start();
     _9axis.write(addr_compus);
    _9axis.write(0x09);
    _9axis.write(addr_compus|0x01);
    _9axis.stop();
    */
    double  mg_x = short((mxh<<8) | (mxl));
    *mx = ( mg_x)*0.15;
    //pc.printf("MGX = %f\t",mx);

    _9axis.start();

    _9axis.write(addr_compus);
    _9axis.write(0x05);
    _9axis.start();
    _9axis.write(addr_compus|0x01);
    myl = _9axis.read(0);
    _9axis.stop();
    _9axis.start();
    _9axis.write(addr_compus);
    _9axis.write(0x06);
    _9axis.start();
    _9axis.write(addr_compus|0x01);
    myh = _9axis.read(0);
    _9axis.stop();
    /*
         _9axis.start();
         _9axis.write(addr_compus);
        _9axis.write(0x09);
        _9axis.write(addr_compus|0x01);
        _9axis.stop();
        */
    double  mg_y = short((myh<<8) | (myl));
    *my = ( mg_y)*0.15;
    // pc.printf("MAGY = %f\t",my);

    _9axis.start();

    _9axis.write(addr_compus);
    _9axis.write(0x07);
    _9axis.start();
    _9axis.write(addr_compus|0x01);
    mzl = _9axis.read(0);
    _9axis.stop();
    _9axis.start();
    _9axis.write(addr_compus);
    _9axis.write(0x08);
    _9axis.start();
    _9axis.write(addr_compus|0x01);
    mzh = _9axis.read(0);
    _9axis.stop();

    _9axis.start();
    _9axis.write(addr_compus);
    _9axis.write(0x09);
    _9axis.start();
    _9axis.write(addr_compus|0x01);
    _9axis.stop();
    double  mg_z= short((mzh<<8) | (mzl));
    *mz = ( mg_z)*0.15;
    /*
    _9axis.start();
    _9axis.write(addr_compus);
    _9axis.write(0x0a);
    _9axis.write(0x12);
    _9axis.stop();

    _9axis.start();
    _9axis.write(addr_compus);
    _9axis.write(0x03);
    _9axis.start();
    _9axis.write(addr_compus|0x01);
    mxl = _9axis.read(0);
    _9axis.stop();
    _9axis.start();
    _9axis.write(addr_compus);
    _9axis.write(0x04);
    _9axis.start();
    _9axis.write(addr_compus|0x01);
    mxh = _9axis.read(0);
    _9axis.stop();

    _9axis.start();
    _9axis.write(addr_compus);
    _9axis.write(0x09);
    _9axis.start();
    _9axis.write(addr_compus|0x01);
    _9axis.stop();


    double  mg_x = short((mxh<<8) | (mxl));
    *mx = ( mg_x)*0.15;


    _9axis.start();

    _9axis.write(addr_compus);
    _9axis.write(0x05);
    _9axis.start();
    _9axis.write(addr_compus|0x01);
    myl = _9axis.read(0);
    _9axis.stop();
    _9axis.start();
    _9axis.write(addr_compus);
    _9axis.write(0x06);
    _9axis.start();
    _9axis.write(addr_compus|0x01);
    myh = _9axis.read(0);
    _9axis.stop();

    _9axis.start();
    _9axis.write(addr_compus);
    _9axis.write(0x09);
    _9axis.start();
    _9axis.write(addr_compus|0x01);
    _9axis.stop();
    double  mg_y = short((myh<<8) | (myl));
    *my = ( mg_y)*0.15;

    _9axis.start();

    _9axis.write(addr_compus);
    _9axis.write(0x07);
    _9axis.start();
    _9axis.write(addr_compus|0x01);
    mzl = _9axis.read(0);
    _9axis.stop();
    _9axis.start();
    _9axis.write(addr_compus);
    _9axis.write(0x08);
    _9axis.start();
    _9axis.write(addr_compus|0x01);
    mzh = _9axis.read(0);
    _9axis.stop();

    _9axis.start();
    _9axis.write(addr_compus);
    _9axis.write(0x09);
    _9axis.start();
    _9axis.write(addr_compus|0x01);
    _9axis.stop();

    double  mg_z= short((mzh<<8) | (mzl));
    *mz = ( mg_z)*0.15;*/
    //pc.printf("MAGZ = %f\r\n",mz);
}

//////////////16進数表記/////////////////////////////////////////
void Hepta9axis::sen_gyro_u16(char* gx_u16,char* gy_u16,char* gz_u16, int *dsize)
{
    //x軸GYRO
    _9axis.start();
    _9axis.write(addr_accel_gyro);
    _9axis.write(0x43);
    _9axis.start();
    _9axis.write(addr_accel_gyro|0x01);
    gxh = _9axis.read(0);
    _9axis.stop();

    _9axis.start();
    _9axis.write(addr_accel_gyro);
    _9axis.write(0x44);
    _9axis.start();
    _9axis.write(addr_accel_gyro|0x01);
    gxl = _9axis.read(0);
    _9axis.stop();

    sprintf( g1, "%02X", ((gxh)) & 0xFF);
    sprintf( g2, "%02X", ((gxl)) & 0xFF);
    gx_u16[0]=g1[0];
    gx_u16[1]=g1[1];
    gx_u16[2]=g2[0];
    gx_u16[3]=g2[1];

//y軸GYRO
    _9axis.start();
    _9axis.write(addr_accel_gyro);
    _9axis.write(0x45);
    _9axis.start();
    _9axis.write(addr_accel_gyro|0x01);
    gyh = _9axis.read(0);
    _9axis.stop();

    _9axis.start();
    _9axis.write(addr_accel_gyro);
    _9axis.write(0x46);
    _9axis.start();
    _9axis.write(addr_accel_gyro|0x01);
    gyl = _9axis.read(0);
    _9axis.stop();
    sprintf( g1, "%02X", (gyh) & 0xFF);
    sprintf( g2, "%02X", (gyl) & 0xFF);
    gy_u16[0]=g1[0];
    gy_u16[1]=g1[1];
    gy_u16[2]=g2[0];
    gy_u16[3]=g2[1];

//z軸GYRO
    _9axis.start();
    _9axis.write(addr_accel_gyro);
    _9axis.write(0x47);
    _9axis.start();
    _9axis.write(addr_accel_gyro|0x01);
    gzh = _9axis.read(0);
    _9axis.stop();

    _9axis.start();
    _9axis.write(addr_accel_gyro);
    _9axis.write(0x48);
    _9axis.start();
    _9axis.write(addr_accel_gyro|0x01);
    gzl = _9axis.read(0);
    _9axis.stop();

    sprintf( g1, "%02X", ((gzh)) & 0xFF);
    sprintf( g2, "%02X", ((gzl)) & 0xFF);
    gz_u16[0]=g1[0];
    gz_u16[1]=g1[1];
    gz_u16[2]=g2[0];
    gz_u16[3]=g2[1];
    *dsize = 4;
}

void Hepta9axis::sen_acc_u16(char* ax_u16,char* ay_u16,char* az_u16, int *dsize)
{
    //x軸加速度
    _9axis.start();
    _9axis.write(addr_accel_gyro);
    _9axis.write(0x3B);
    _9axis.start();
    _9axis.write(addr_accel_gyro|0x01);
    xh = _9axis.read(0);
    _9axis.stop();

    _9axis.start();
    _9axis.write(addr_accel_gyro);
    _9axis.write(0x3C);
    _9axis.start();
    _9axis.write(addr_accel_gyro|0x01);
    xl = _9axis.read(0);
    _9axis.stop();

    sprintf( a1, "%02X", ((xh)) & 0xFF);
    sprintf( a2, "%02X", ((xl)) & 0xFF);
    ax_u16[0]=a1[0];
    ax_u16[1]=a1[1];
    ax_u16[2]=a2[0];
    ax_u16[3]=a2[1];

//Y軸加速度
    _9axis.start();
    _9axis.write(addr_accel_gyro);
    _9axis.write(0x3D);
    _9axis.start();
    _9axis.write(addr_accel_gyro|0x01);
    yh = _9axis.read(0);

    _9axis.stop();
    _9axis.start();
    _9axis.write(addr_accel_gyro);
    _9axis.write(0x3E);
    _9axis.start();
    _9axis.write(addr_accel_gyro|0x01);
    yl = _9axis.read(0);
    _9axis.stop();

    sprintf( a1, "%02X", ((yh)) & 0xFF);
    sprintf( a2, "%02X", ((yl)) & 0xFF);
    ay_u16[0]=a1[0];
    ay_u16[1]=a1[1];
    ay_u16[2]=a2[0];
    ay_u16[3]=a2[1];
//z軸加速度
    _9axis.start();
    _9axis.write(addr_accel_gyro);
    _9axis.write(0x3F);
    _9axis.start();
    _9axis.write(addr_accel_gyro|0x01);
    zh = _9axis.read(0);
    _9axis.stop();

    _9axis.start();
    _9axis.write(addr_accel_gyro);
    _9axis.write(0x40);
    _9axis.start();
    _9axis.write(addr_accel_gyro|0x01);
    zl = _9axis.read(0);
    _9axis.stop();

    sprintf( a1, "%02X", ((zh)) & 0xFF);
    sprintf( a2, "%02X", ((zl)) & 0xFF);
    az_u16[0]=a1[0];
    az_u16[1]=a1[1];
    az_u16[2]=a2[0];
    az_u16[3]=a2[1];
    *dsize = 4;
}

void Hepta9axis::sen_mag_u16(char* mx_u16,char* my_u16,char* mz_u16, int *dsize)
{
    _9axis.start();
    _9axis.write(addr_compus);
    _9axis.write(0x0a);
    _9axis.write(0x12);
    _9axis.stop();

    _9axis.start();
    _9axis.write(addr_compus);
    _9axis.write(0x03);
    _9axis.start();
    _9axis.write(addr_compus|0x01);
    mxl = _9axis.read(0);
    _9axis.stop();
    _9axis.start();
    _9axis.write(addr_compus);
    _9axis.write(0x04);
    _9axis.start();
    _9axis.write(addr_compus|0x01);
    mxh = _9axis.read(0);
    _9axis.stop();

    sprintf( m1, "%02X", ((mxh)) & 0xFF);
    sprintf( m2, "%02X", ((mxl)) & 0xFF);
    mx_u16[0]=m1[0];
    mx_u16[1]=m1[1];
    mx_u16[2]=m2[0];
    mx_u16[3]=m2[1];

    //pc.printf("MGX = %f\t",mx);

    _9axis.start();

    _9axis.write(addr_compus);
    _9axis.write(0x05);
    _9axis.start();
    _9axis.write(addr_compus|0x01);
    myl = _9axis.read(0);
    _9axis.stop();
    _9axis.start();
    _9axis.write(addr_compus);
    _9axis.write(0x06);
    _9axis.start();
    _9axis.write(addr_compus|0x01);
    myh = _9axis.read(0);
    _9axis.stop();

    sprintf( m1, "%02X", ((myh)) & 0xFF);
    sprintf( m2, "%02X", ((myl)) & 0xFF);
    my_u16[0]=m1[0];
    my_u16[1]=m1[1];
    my_u16[2]=m2[0];
    my_u16[3]=m2[1];
    // pc.printf("MAGY = %f\t",my);

    _9axis.start();

    _9axis.write(addr_compus);
    _9axis.write(0x07);
    _9axis.start();
    _9axis.write(addr_compus|0x01);
    mzl = _9axis.read(0);
    _9axis.stop();
    _9axis.start();
    _9axis.write(addr_compus);
    _9axis.write(0x08);
    _9axis.start();
    _9axis.write(addr_compus|0x01);
    mzh = _9axis.read(0);
    _9axis.stop();
    
    _9axis.start();
    _9axis.write(addr_compus);
    _9axis.write(0x09);
    _9axis.start();
    _9axis.write(addr_compus|0x01);
    _9axis.stop();

    sprintf( m1, "%02X", ((mzh)) & 0xFF);
    sprintf( m2, "%02X", ((mzl)) & 0xFF);
    mz_u16[0]=m1[0];
    mz_u16[1]=m1[1];
    mz_u16[2]=m2[0];
    mz_u16[3]=m2[1];
    *dsize = 4;
}