Diff: Hepta9axis.cpp

Revision:

2:306058b9d04e

Parent:

1:5b35162b3b6a

--- a/Hepta9axis.cpp	Sat Aug 05 13:35:52 2017 +0000
+++ b/Hepta9axis.cpp	Wed Aug 23 06:18:26 2017 +0000
@@ -1,256 +1,352 @@
 #include"Hepta9axis.h"
 #include"mbed.h"
 
-HeptaMPU9250::HeptaMPU9250(PinName sda, PinName scl, int aaddr, int agaddr) : MPU9250(sda,scl),addr_accel_gyro(aaddr),addr_compus(agaddr)
+Hepta9axis::Hepta9axis(PinName sda, PinName scl, int aaddr, int agaddr) : _9axis(sda,scl),addr_accel_gyro(aaddr),addr_compus(agaddr)
 {
-    MPU9250.frequency(100000);
+    _9axis.frequency(100000);
     cmd[0]=0x6B;
     cmd[1]=0x00;
-    MPU9250.write(addr_accel_gyro,cmd,2);
+    _9axis.write(addr_accel_gyro,cmd,2);
     cmd[0] = 0x37;
     cmd[1] = 0x02;
-    MPU9250.write(addr_accel_gyro,cmd,2);
-    MPU9250.stop();
+    _9axis.write(addr_accel_gyro,cmd,2);
+    _9axis.stop();
 }
 
-void HeptaMPU9250::setup()
+void Hepta9axis::setup()
 {
 
-    MPU9250.frequency(100000);
+    _9axis.frequency(100000);
     cmd[0]=0x6B;
     cmd[1]=0x00;
-    MPU9250.write(addr_accel_gyro,cmd,2);
+    _9axis.write(addr_accel_gyro,cmd,2);
     cmd[0] = 0x37;
     cmd[1] = 0x02;
-    MPU9250.write(addr_accel_gyro,cmd,2);
-    MPU9250.stop();
+    _9axis.write(addr_accel_gyro,cmd,2);
+    _9axis.stop();
 }
 
-void HeptaMPU9250::sen_acc(float *ax,float *ay,float *az)
+void Hepta9axis::sen_acc(float *ax,float *ay,float *az)
 {
 //x軸加速度
-    MPU9250.start();
-    MPU9250.write(addr_accel_gyro);
-    MPU9250.write(0x3B);
-    MPU9250.start();
-    MPU9250.write(addr_accel_gyro|0x01);
-    xh = MPU9250.read(0);
-    MPU9250.stop();
+    _9axis.start();
+    _9axis.write(addr_accel_gyro);
+    _9axis.write(0x3B);
+    _9axis.start();
+    _9axis.write(addr_accel_gyro|0x01);
+    xh = _9axis.read(0);
+    _9axis.stop();
 
-    MPU9250.start();
-    MPU9250.write(addr_accel_gyro);
-    MPU9250.write(0x3C);
-    MPU9250.start();
-    MPU9250.write(addr_accel_gyro|0x01);
-    xl = MPU9250.read(0);
-    MPU9250.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;
+    *ax = (acc_ax)*2/32764*9.81;
 
 
 //Y軸加速度
-    MPU9250.start();
-    MPU9250.write(addr_accel_gyro);
-    MPU9250.write(0x3D);
-    MPU9250.start();
-    MPU9250.write(addr_accel_gyro|0x01);
-    yh = MPU9250.read(0);
-   
-    MPU9250.stop();
-    MPU9250.start();
-    MPU9250.write(addr_accel_gyro);
-    MPU9250.write(0x3E);
-    MPU9250.start();
-    MPU9250.write(addr_accel_gyro|0x01);
-    yl = MPU9250.read(0);
-    
-    MPU9250.stop();
+    _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軸加速度
-    MPU9250.start();
-    MPU9250.write(addr_accel_gyro);
-    MPU9250.write(0x3F);
-    MPU9250.start();
-    MPU9250.write(addr_accel_gyro|0x01);
-    zh = MPU9250.read(0);
-    MPU9250.stop();
-    
-    MPU9250.start();
-    MPU9250.write(addr_accel_gyro);
-    MPU9250.write(0x40);
-    MPU9250.start();
-    MPU9250.write(addr_accel_gyro|0x01);
-    zl = MPU9250.read(0);
-    MPU9250.stop();
-    
+    _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 HeptaMPU9250::sen_gyro(float *gx,float *gy,float *gz)
+void Hepta9axis::sen_gyro(float *gx,float *gy,float *gz)
 {
 //x軸GYRO
-    MPU9250.start();
-    MPU9250.write(addr_accel_gyro);
-    MPU9250.write(0x43);
-    MPU9250.start();
-    MPU9250.write(addr_accel_gyro|0x01);
-    gxh = MPU9250.read(0);
-    MPU9250.stop();
-    
-    MPU9250.start();
-    MPU9250.write(addr_accel_gyro);
-    MPU9250.write(0x44);
-    MPU9250.start();
-    MPU9250.write(addr_accel_gyro|0x01);
-    gxl = MPU9250.read(0);
-    MPU9250.stop();
-    
+    _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
-    MPU9250.start();
-    MPU9250.write(addr_accel_gyro);
-    MPU9250.write(0x45);
-    MPU9250.start();
-    MPU9250.write(addr_accel_gyro|0x01);
-    gyh = MPU9250.read(0);
-    MPU9250.stop();
-    
-    MPU9250.start();
-    MPU9250.write(addr_accel_gyro);
-    MPU9250.write(0x46);
-    MPU9250.start();
-    MPU9250.write(addr_accel_gyro|0x01);
-    gyl = MPU9250.read(0);
-    MPU9250.stop();
-    
+    _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
-    MPU9250.start();
-    MPU9250.write(addr_accel_gyro);
-    MPU9250.write(0x47);
-    MPU9250.start();
-    MPU9250.write(addr_accel_gyro|0x01);
-    gzh = MPU9250.read(0);
-    MPU9250.stop();
-    
-    MPU9250.start();
-    MPU9250.write(addr_accel_gyro);
-    MPU9250.write(0x48);
-    MPU9250.start();
-    MPU9250.write(addr_accel_gyro|0x01);
-    gzl = MPU9250.read(0);
-    MPU9250.stop();
-    
+    _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 HeptaMPU9250::sen_mag(float *mx,float *my,float *mz)
+void Hepta9axis::sen_mag(float *mx,float *my,float *mz)
 {
 
-    MPU9250.start();
-    MPU9250.write(addr_compus);
-    MPU9250.write(0x03);
-    MPU9250.start();
-    MPU9250.write(addr_compus|0x01);
-    mxl = MPU9250.read(0);
-    MPU9250.stop();
-    MPU9250.start();
-    MPU9250.write(addr_compus);
-    MPU9250.write(0x04);
-    MPU9250.start();
-    MPU9250.write(addr_compus|0x01);
-    mxh = MPU9250.read(0);
-    MPU9250.stop();
-    cmd[0] = 0x0a;
-    cmd[1] = 0x12;
-    MPU9250.write( addr_compus,cmd,2);
-    cmd[0]=0x09;
-    MPU9250.write( addr_compus,cmd,1);
+    _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);
 
-    MPU9250.start();
+    _9axis.start();
 
-    MPU9250.write(addr_compus);
-    MPU9250.write(0x05);
-    MPU9250.start();
-    MPU9250.write(addr_compus|0x01);
-    myl = MPU9250.read(0);
-    MPU9250.stop();
-    MPU9250.start();
-    MPU9250.write(addr_compus);
-    MPU9250.write(0x06);
-    MPU9250.start();
-    MPU9250.write(addr_compus|0x01);
-    myh = MPU9250.read(0);
-    MPU9250.stop();
-    cmd[0] = 0x0a;
-    cmd[1] = 0x12;
-    MPU9250.write( addr_compus,cmd,2);
-    cmd[0]=0x09;
-    MPU9250.write( addr_compus,cmd,1);
+    _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);
 
-    MPU9250.start();
+    _9axis.start();
 
-    MPU9250.write(addr_compus);
-    MPU9250.write(0x07);
-    MPU9250.start();
-    MPU9250.write(addr_compus|0x01);
-    mzl = MPU9250.read(0);
-    MPU9250.stop();
-    MPU9250.start();
-    MPU9250.write(addr_compus);
-    MPU9250.write(0x08);
-    MPU9250.start();
-    MPU9250.write(addr_compus|0x01);
-    mzh = MPU9250.read(0);
-    MPU9250.stop();
-    cmd[0] = 0x0a;
-    cmd[1] = 0x12;
-    MPU9250.write( addr_compus,cmd,2);
-    cmd[0]=0x09;
-    MPU9250.write( addr_compus,cmd,1);
+    _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 HeptaMPU9250::sen_gyro_u16(char* gx_u16,char* gy_u16,char* gz_u16, int *dsize)
+void Hepta9axis::sen_gyro_u16(char* gx_u16,char* gy_u16,char* gz_u16, int *dsize)
 {
     //x軸GYRO
-    MPU9250.start();
-    MPU9250.write(addr_accel_gyro);
-    MPU9250.write(0x43);
-    MPU9250.start();
-    MPU9250.write(addr_accel_gyro|0x01);
-    gxh = MPU9250.read(0);
-    MPU9250.stop();
-    
-    MPU9250.start();
-    MPU9250.write(addr_accel_gyro);
-    MPU9250.write(0x44);
-    MPU9250.start();
-    MPU9250.write(addr_accel_gyro|0x01);
-    gxl = MPU9250.read(0);
-    MPU9250.stop();
-    
+    _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];
@@ -259,45 +355,45 @@
     gx_u16[3]=g2[1];
 
 //y軸GYRO
-    MPU9250.start();
-    MPU9250.write(addr_accel_gyro);
-    MPU9250.write(0x45);
-    MPU9250.start();
-    MPU9250.write(addr_accel_gyro|0x01);
-    gyh = MPU9250.read(0);
-    MPU9250.stop();
-    
-    MPU9250.start();
-    MPU9250.write(addr_accel_gyro);
-    MPU9250.write(0x46);
-    MPU9250.start();
-    MPU9250.write(addr_accel_gyro|0x01);
-    gyl = MPU9250.read(0);
-    MPU9250.stop();
+    _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
-    MPU9250.start();
-    MPU9250.write(addr_accel_gyro);
-    MPU9250.write(0x47);
-    MPU9250.start();
-    MPU9250.write(addr_accel_gyro|0x01);
-    gzh = MPU9250.read(0);
-    MPU9250.stop();
-    
-    MPU9250.start();
-    MPU9250.write(addr_accel_gyro);
-    MPU9250.write(0x48);
-    MPU9250.start();
-    MPU9250.write(addr_accel_gyro|0x01);
-    gzl = MPU9250.read(0);
-    MPU9250.stop();
-    
+    _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];
@@ -305,27 +401,27 @@
     gz_u16[2]=g2[0];
     gz_u16[3]=g2[1];
     *dsize = 4;
-    }
-    
-void HeptaMPU9250::sen_acc_u16(char* ax_u16,char* ay_u16,char* az_u16, int *dsize)
+}
+
+void Hepta9axis::sen_acc_u16(char* ax_u16,char* ay_u16,char* az_u16, int *dsize)
 {
-   //x軸加速度
-    MPU9250.start();
-    MPU9250.write(addr_accel_gyro);
-    MPU9250.write(0x3B);
-    MPU9250.start();
-    MPU9250.write(addr_accel_gyro|0x01);
-    xh = MPU9250.read(0);
-    MPU9250.stop();
+    //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();
 
-    MPU9250.start();
-    MPU9250.write(addr_accel_gyro);
-    MPU9250.write(0x3C);
-    MPU9250.start();
-    MPU9250.write(addr_accel_gyro|0x01);
-    xl = MPU9250.read(0);
-    MPU9250.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];
@@ -334,22 +430,22 @@
     ax_u16[3]=a2[1];
 
 //Y軸加速度
-    MPU9250.start();
-    MPU9250.write(addr_accel_gyro);
-    MPU9250.write(0x3D);
-    MPU9250.start();
-    MPU9250.write(addr_accel_gyro|0x01);
-    yh = MPU9250.read(0);
-   
-    MPU9250.stop();
-    MPU9250.start();
-    MPU9250.write(addr_accel_gyro);
-    MPU9250.write(0x3E);
-    MPU9250.start();
-    MPU9250.write(addr_accel_gyro|0x01);
-    yl = MPU9250.read(0);    
-    MPU9250.stop();
-    
+    _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];
@@ -357,22 +453,22 @@
     ay_u16[2]=a2[0];
     ay_u16[3]=a2[1];
 //z軸加速度
-    MPU9250.start();
-    MPU9250.write(addr_accel_gyro);
-    MPU9250.write(0x3F);
-    MPU9250.start();
-    MPU9250.write(addr_accel_gyro|0x01);
-    zh = MPU9250.read(0);
-    MPU9250.stop();
-    
-    MPU9250.start();
-    MPU9250.write(addr_accel_gyro);
-    MPU9250.write(0x40);
-    MPU9250.start();
-    MPU9250.write(addr_accel_gyro|0x01);
-    zl = MPU9250.read(0);
-    MPU9250.stop();
-    
+    _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];
@@ -380,61 +476,56 @@
     az_u16[2]=a2[0];
     az_u16[3]=a2[1];
     *dsize = 4;
-    }
-    
-void HeptaMPU9250::sen_mag_u16(char* mx_u16,char* my_u16,char* mz_u16, int *dsize)
+}
+
+void Hepta9axis::sen_mag_u16(char* mx_u16,char* my_u16,char* mz_u16, int *dsize)
 {
-    
-    MPU9250.start();
-    MPU9250.write(addr_compus);
-    MPU9250.write(0x03);
-    MPU9250.start();
-    MPU9250.write(addr_compus|0x01);
-    mxl = MPU9250.read(0);
-    MPU9250.stop();
-    MPU9250.start();
-    MPU9250.write(addr_compus);
-    MPU9250.write(0x04);
-    MPU9250.start();
-    MPU9250.write(addr_compus|0x01);
-    mxh = MPU9250.read(0);
-    MPU9250.stop();
-    cmd[0] = 0x0a;
-    cmd[1] = 0x12;
-    MPU9250.write( addr_compus,cmd,2);
-    cmd[0]=0x09;
-    MPU9250.write( addr_compus,cmd,1);
-    
+    _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);
 
-    MPU9250.start();
+    _9axis.start();
 
-    MPU9250.write(addr_compus);
-    MPU9250.write(0x05);
-    MPU9250.start();
-    MPU9250.write(addr_compus|0x01);
-    myl = MPU9250.read(0);
-    MPU9250.stop();
-    MPU9250.start();
-    MPU9250.write(addr_compus);
-    MPU9250.write(0x06);
-    MPU9250.start();
-    MPU9250.write(addr_compus|0x01);
-    myh = MPU9250.read(0);
-    MPU9250.stop();
-    cmd[0] = 0x0a;
-    cmd[1] = 0x12;
-    MPU9250.write( addr_compus,cmd,2);
-    cmd[0]=0x09;
-    MPU9250.write( addr_compus,cmd,1);
-    
+    _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];
@@ -443,27 +534,29 @@
     my_u16[3]=m2[1];
     // pc.printf("MAGY = %f\t",my);
 
-    MPU9250.start();
+    _9axis.start();
 
-    MPU9250.write(addr_compus);
-    MPU9250.write(0x07);
-    MPU9250.start();
-    MPU9250.write(addr_compus|0x01);
-    mzl = MPU9250.read(0);
-    MPU9250.stop();
-    MPU9250.start();
-    MPU9250.write(addr_compus);
-    MPU9250.write(0x08);
-    MPU9250.start();
-    MPU9250.write(addr_compus|0x01);
-    mzh = MPU9250.read(0);
-    MPU9250.stop();
-    cmd[0] = 0x0a;
-    cmd[1] = 0x12;
-    MPU9250.write( addr_compus,cmd,2);
-    cmd[0]=0x09;
-    MPU9250.write( addr_compus,cmd,1);
-   
+    _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];
@@ -471,4 +564,4 @@
     mz_u16[2]=m2[0];
     mz_u16[3]=m2[1];
     *dsize = 4;
-    }
\ No newline at end of file
+}
\ No newline at end of file