File content as of revision 0:d173af3ca217:

#ifndef MYMPU9250_H
#define MYMPU9250_H
 
#include "mpu9250Define.h"
 
class MyMPU9250
{
public:
    MyMPU9250()
        : pc_(USBTX, USBRX), i2c_(p28, p27) {
        accelLSB2MG_ = 10.0;
        gyroLSB2DegPerSec_ = 0.0;
    };
 
    bool initialize() {
        i2c_.frequency(400000);
        i2c_.start();
 
        wait(0.5);
 
        char cmd[2] = { 0x00 };
        cmd[0] = PWR_MGMT_1; // 0x6B
        cmd[1] = (PWR_MGMT_1_CLKSEL_0); // 0x00
        this->write(cmd, BIT_2);
 
        wait(0.5);
 
        char i_am[1] = { 0x00 };
        if(! this->read(WHO_AM_I, i_am, BIT_1)) {
            pc_.printf("Fail to read! I am %x\n", i_am[0]);
            return false;
        } else if(!(i_am[0] == 0x71)) {
            pc_.printf("Fail to identify! I am %x\n", i_am[0]);
            return false;
        }
        return true;
    };
 
    void setup(AccelSensitivity accel_mode=AFS_SEL_2,
               GyroSensitivity gyro_mode=FS_SEL_250) {
        char cmd[2] = { 0x00 };
 
        cmd[0] = ACCEL_CONFIG;
        cmd[1] = ACCEL_CONFIG_AFS_SEL_2;//(0x18);
        this->write(cmd, BIT_2);
        accelLSB2MG_ = getAccelLSB2MG(accel_mode);
        pc_.printf("accelLSB2MG: %f, %x", accelLSB2MG_, cmd[1]);
 
        cmd[0] = GYRO_CONFIG;
        cmd[1] = getGyroSensitivity(gyro_mode);//(0x18);
        this->write(cmd, BIT_2);
        gyroLSB2DegPerSec_ = getGyroLSB2DegPerSec(gyro_mode);
 
        cmd[0] = INT_PIN_CFG; // 0x37
        cmd[1] = (INT_PIN_DFG_I2C_BYPASS_EN); // 0x02
        this->write(cmd, BIT_2);
    };
 
    bool updateAccelAllAxes() {
        return this->read(ACCEL_XOUT_H, accel_all_, BIT_6);
    };
 
    int accelXRaw() {
        return (int16_t)(accel_all_[0]*0x100 +  accel_all_[1]) ;
    };
 
    double accelX() {
        return accelLSB2MG_ * accelXRaw() ;
    };
 
    int accelYRaw() {
        return (int16_t)(accel_all_[2]*0x100 +  accel_all_[3]) ;
    };
 
    double accelY() {
        return accelLSB2MG_ * accelYRaw() ;
    };
 
    int accelZRaw() {
        return (int16_t)(accel_all_[4]*0x100 +  accel_all_[5]) ;
    };
 
    double accelZ() {
        return accelLSB2MG_ * accelZRaw() ;
    };
 
 
    bool updateGyroAllAxes() {
        return this->read(GYRO_XOUT_H, gyro_all_, BIT_6);
    };
 
    int gyroXRaw() {
        return (int16_t)(gyro_all_[0]*0x100 +  gyro_all_[1]) ;
    };
 
    double gyroX() {
        return accelLSB2MG_ * gyroXRaw() ;
    };
 
    int gyroYRaw() {
        return (int16_t)(gyro_all_[2]*0x100 +  gyro_all_[3]) ;
    };
 
    double gyroY() {
        return accelLSB2MG_ * gyroYRaw() ;
    };
 
    int gyroZRaw() {
        return (int16_t)(gyro_all_[4]*0x100 +  gyro_all_[5]) ;
    };
 
    double gyroZ() {
        return accelLSB2MG_ * gyroZRaw() ;
    };
 
 
private:
    Serial pc_;
    I2C i2c_;
    float accelLSB2MG_;
    float gyroLSB2DegPerSec_;
    char accel_all_[6];
    char gyro_all_[6];
 
    bool write(char* _cmd, int _length) {
        int w_status = i2c_.write(MPU9250_ADDRESS, _cmd, _length, false);
        if(w_status == 0) {
            return true;
        } else {
            pc_.printf("write register: %x \n", _cmd[0]);
            pc_.printf("  write: %d \n", w_status);
            return false;
        }
    }
 
    bool read(char _register, char* _data, int _length) {
        char cmd[1] = { _register };
        int w_status = i2c_.write(MPU9250_ADDRESS, cmd, BIT_1, false);
        int r_status = i2c_.read(MPU9250_ADDRESS, _data, _length, false);
        if(w_status + r_status == 0) {
            return true;
        } else {
            pc_.printf("write register: %x \n", cmd[0]);
            pc_.printf("read register: %x \n", _register);
            pc_.printf("  write: %d | read: %d \n", w_status, r_status);
            return false;
        }
    };
};
 
#endif  /* MYMPU9250_H */