only gyro and angle accel
Dependencies: mbed MPU9250_test
MPU9250.cpp
- Committer:
- yukiTakezawa
- Date:
- 2019-02-16
- Revision:
- 3:aa8f84e4244f
File content as of revision 3:aa8f84e4244f:
#include "MPU9250.h" MPU9250::MPU9250(PinName sda,PinName scl) : i2c(sda,scl){ wakeUp(); }; int MPU9250::wakeUp(){ int errorFlag = write2Bytes(MPU9250_ADDRESS,PWR_MGMT_1,0x00); int errorFlag2 = write2Bytes(MPU9250_ADDRESS,0x37,0x02); if(errorFlag ==0 && errorFlag2==0){ return 0; } return -1; } int MPU9250::getAccelAndGyroRawData(float* accelX,float* accelY,float* accelZ,float* gyroX,float* gyroY,float* gyroZ){ int16_t ax,ay,az,gx,gy,gz; int errorFlag = getAccelAndGyroBitData(&ax,&ay,&az,&gx,&gy,&gz); /*実際の値に変換*/ *accelX = (float)ax * ACCEL_RANGE / 32768.0; *accelY = (float)ay * ACCEL_RANGE / 32768.0; *accelZ = (float)az * ACCEL_RANGE / 32768.0; /*あとで*/ *gyroX = gx; *gyroY = gy; *gyroZ = gz; return errorFlag; } int MPU9250::getAccelAndGyroBitData(int16_t* ax,int16_t* ay,int16_t* az,int16_t* gx,int16_t* gy,int16_t* gz){ const char cmd[1] = {0x3b}; char data[14]; int errorFlag = i2c.write(MPU9250_ADDRESS,cmd,1); int errorFlag2 = i2c.read(MPU9250_ADDRESS,data,14); if(errorFlag == 0 && errorFlag2 == 0){ *ax = ((int)data[0] << 8) | (int)data[1]; *ay = ((int)data[2] << 8) | (int)data[3]; *az = ((int)data[4] << 8) | (int)data[5]; *gx = ((int)data[8] << 8) | (int)data[9]; *gy = ((int)data[10] << 8) | (int)data[11]; *gz = ((int)data[12] << 8) | (int)data[13]; return 0; } return -1; } int MPU9250::whoAmI(char* address){ char data = WHO_AM_I; i2c.write(MPU9250_ADDRESS,&data,1); wait(0.07); int errorFlag = i2c.read(MPU9250_ADDRESS,&data,1);; *address = data; return errorFlag; } int MPU9250::write2Bytes(char address,char reg,char data){ char tmp[2] = {reg,data}; return i2c.write(address,tmp,2); }