Masahiko Yamazaki
For Hepta-Sat Lite
Hepta9axis.cpp
- Committer:
- hepta2ume
- Date:
- 2017-07-19
- Revision:
- 0:5aaec0996753
- Child:
- 1:5b35162b3b6a
File content as of revision 0:5aaec0996753:
#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;
}