Masahiko Yamazaki
For Hepta-Sat Lite
Diff: Hepta9axis.cpp
- Revision:
- 0:5aaec0996753
- Child:
- 1:5b35162b3b6a
diff -r 000000000000 -r 5aaec0996753 Hepta9axis.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Hepta9axis.cpp Wed Jul 19 08:15:01 2017 +0000
@@ -0,0 +1,474 @@
+#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)
+{
+ 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