Hepta9axisSensor Library
Dependents: Lab9-01_All_transmit Lab9-03_Thermal_chamber 3daf572bcae1 Team ... more
Fork of Hepta9axis by
Diff: Hepta9axis.cpp
- Revision:
- 2:306058b9d04e
- Parent:
- 1:5b35162b3b6a
- Child:
- 3:d5eed0bb962e
--- 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