Hepta 2
HEPTA2 9軸センサー
Dependencies: Hepta2_9axis mbed
Diff: HeptaMPU9250.cpp
- Revision:
- 1:6ee85df13ebd
- Parent:
- 0:91fc70cdd15d
--- a/HeptaMPU9250.cpp Wed Jul 19 07:35:07 2017 +0000
+++ /dev/null Thu Jan 01 00:00:00 1970 +0000
@@ -1,474 +0,0 @@
-#include"HeptaMPU9250.h"
-#include"mbed.h"
-
-HeptaMPU9250::HeptaMPU9250(PinName sda, PinName scl, int aaddr, int agaddr) : MPU9250(sda,scl),addr_accel_gyro(aaddr),addr_compus(agaddr)
-{
- MPU9250.frequency(100000);
- cmd[0]=0x6B;
- cmd[1]=0x00;
- MPU9250.write(addr_accel_gyro,cmd,2);
- cmd[0] = 0x37;
- cmd[1] = 0x02;
- MPU9250.write(addr_accel_gyro,cmd,2);
- MPU9250.stop();
-}
-
-void HeptaMPU9250::setup()
-{
-
- MPU9250.frequency(100000);
- cmd[0]=0x6B;
- cmd[1]=0x00;
- MPU9250.write(addr_accel_gyro,cmd,2);
- cmd[0] = 0x37;
- cmd[1] = 0x02;
- MPU9250.write(addr_accel_gyro,cmd,2);
- MPU9250.stop();
-}
-
-void HeptaMPU9250::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();
-
- MPU9250.start();
- MPU9250.write(addr_accel_gyro);
- MPU9250.write(0x3C);
- MPU9250.start();
- MPU9250.write(addr_accel_gyro|0x01);
- xl = MPU9250.read(0);
- MPU9250.stop();
-
- double acc_ax = short((xh<<8) | (xl));
- *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();
- 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();
-
- 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)
-{
-//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();
-
- 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();
-
- 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();
-
- double gyro_az = short((gzh<<8) | (gzl));
- *gz = (gyro_az)*0.00763;
-
-}
-
-void HeptaMPU9250::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);
- double mg_x = short((mxh<<8) | (mxl));
- *mx = ( mg_x)*0.15;
- //pc.printf("MGX = %f\t",mx);
-
- MPU9250.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);
- double mg_y = short((myh<<8) | (myl));
- *my = ( mg_y)*0.15;
- // pc.printf("MAGY = %f\t",my);
-
- MPU9250.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);
- 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)
-{
- //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();
-
- 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
- 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();
- 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();
-
- 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 HeptaMPU9250::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();
-
- MPU9250.start();
- MPU9250.write(addr_accel_gyro);
- MPU9250.write(0x3C);
- MPU9250.start();
- MPU9250.write(addr_accel_gyro|0x01);
- xl = MPU9250.read(0);
- MPU9250.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軸加速度
- 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();
-
- 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軸加速度
- 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();
-
- 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 HeptaMPU9250::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);
-
- 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();
-
- 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);
-
- 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);
-
- MPU9250.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);
-
- 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;
- }
\ No newline at end of file