Diff: MPU9250.cpp

Revision:

3:aa8f84e4244f

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU9250.cpp	Sat Feb 16 09:46:10 2019 +0000
@@ -0,0 +1,61 @@
+#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);
+}
\ No newline at end of file