Revision 0:d36bfb8300a2, committed 2017-01-09

Comitter:

Gaku0606

Date:

Mon Jan 09 06:28:04 2017 +0000

Child:

1:6a4c2f84180b

Commit message:

mpu9250 library with I2C

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mpu9250_i2c.cpp	Mon Jan 09 06:28:04 2017 +0000
@@ -0,0 +1,185 @@
+#include "mbed.h"
+#include "mpu9250_i2c.h"
+
+char mpu9250::_addr = SLAVE_ADDR_HIGH;
+double mpu9250::acc_coef = ACC_LSB;
+double mpu9250::gyro_coef = GYRO_LSB;
+double mpu9250::mag_coef = MAG_LSB;
+double mpu9250::acc_offset[3] = {0,0,0};
+double mpu9250::gyro_offset[3] = {0,0,0};
+double mpu9250::mag_offset[3] = {0,0,0};
+
+mpu9250::mpu9250(I2C &_i2c, AD0 celect){
+    
+    _nine = &_i2c;
+    if(celect == AD0_HIGH) _addr = SLAVE_ADDR_HIGH;   
+    else _addr = SLAVE_ADDR_LOW;   
+    
+    _nine->frequency(400000);//400kHz
+    init();
+}
+void mpu9250::frequency(int Hz){
+    _nine->frequency(Hz);   
+}
+bool mpu9250::senserTest(){
+    if(readReg(_addr, WHO_AM_I) == 0x71){
+        return true;   
+    }   
+    else return false;
+}
+
+bool mpu9250::mag_senserTest(){
+     if(readReg(MAG_ADDR, WIA) == 0x48){
+        return true;   
+    }   
+    else return false;
+}
+
+void mpu9250::setOffset(double gx, double gy, double gz, double ax, double ay, double az, double mx, double my, double mz){
+    gyro_offset[0] = gx;
+    gyro_offset[1] = gy;
+    gyro_offset[2] = gz;
+    acc_offset[0] = ax;
+    acc_offset[1] = ay;
+    acc_offset[2] = az;
+    mag_offset[0] = mx;
+    mag_offset[1] = my;
+    mag_offset[2] = mz;
+}
+
+void mpu9250::setGyro(GYRO_RANGE g_range){
+    char fchoice = readReg(_addr, GYRO_CONFIG) & 0x03;
+    char send;
+    if(g_range == _250DPS){
+        send = 0x00 | fchoice;
+        gyro_coef = GYRO_LSB;
+    } 
+    else if(g_range == _500DPS){
+        send = 0x08 | fchoice;
+        gyro_coef = GYRO_LSB * 2.0; 
+    }
+    else if(g_range == _1000DPS){
+        send = 0x10 | fchoice;   
+        gyro_coef = GYRO_LSB * 4.0;
+    }
+    else if(g_range == _2000DPS){
+        send = 0x18 | fchoice;
+        gyro_coef = GYRO_LSB * 8.0;   
+    }
+    writeReg(_addr, GYRO_CONFIG, send);
+}
+void mpu9250::setAcc(ACC_RANGE a_range){
+    char send;
+    if(a_range == _2G){
+        send = 0x00;
+        acc_coef = ACC_LSB;
+    }
+    else if(a_range == _4G){
+        send = 0x08;
+        acc_coef = ACC_LSB * 2.0;   
+    }
+    else if(a_range == _8G){
+        send = 0x10;
+        acc_coef = ACC_LSB * 4.0;   
+    }
+    else if(a_range == _16G){
+        send = 0x18;
+        acc_coef = ACC_LSB * 8.0;   
+    }
+    writeReg(_addr, ACCEL_CONFIG, send);
+}
+
+void mpu9250::init(){
+    
+    acc_coef = ACC_LSB;
+    gyro_coef = GYRO_LSB;
+    mag_coef = MAG_LSB;
+    
+    writeReg(MAG_ADDR, 0x6B, 0x00);
+    wait_us(70);
+    writeReg(_addr, 0x37, 0x02);
+    wait_us(70);
+    writeReg(MAG_ADDR, CNTL1, 0x16); 
+    wait_us(70);
+    setAcc(_4G);
+    wait_us(70);
+    setGyro(_500DPS);      
+}
+
+void mpu9250::getAcc(double *acc){
+    char data[6];  
+    short sign;
+    readReg(_addr, ACCEL_XOUT_H, data, 6);
+    sign = ((short)data[0] << 8) | (short)data[1];
+    acc[0] = (double)sign * acc_coef - acc_offset[0];
+    sign = ((short)data[2] << 8) | (short)data[3];
+    acc[1] = (double)sign * acc_coef - acc_offset[1];
+    sign = ((short)data[4] << 8) | (short)data[5];
+    acc[2] = (double)sign * acc_coef - acc_offset[2];
+}
+void mpu9250::getAcc(double *ax, double *ay, double *az){
+    double acc[3];
+    getAcc(acc);
+    *ax = acc[0];
+    *ay = acc[1];
+    *az = acc[2];   
+}
+void mpu9250::getGyro(double *gyro){
+    char data[6];  
+    short sign;
+    readReg(_addr, GYRO_XOUT_H, data, 6);
+    sign = ((short)data[0] << 8) | (short)data[1];
+    gyro[0] = (double)sign * gyro_coef - gyro_offset[0];
+    sign = ((short)data[2] << 8) | (short)data[3];
+    gyro[1] = (double)sign * gyro_coef - gyro_offset[1];
+    sign = ((short)data[4] << 8) | (short)data[5];
+    gyro[2] = (double)sign * gyro_coef - gyro_offset[2];
+}
+void mpu9250::getGyro(double *gx, double *gy, double *gz){
+    double gyro[3];
+    getGyro(gyro);
+    *gx = gyro[0];
+    *gy = gyro[1];
+    *gz = gyro[2];   
+}
+void mpu9250::getMag(double *mag){
+    char data[8];   
+    short sign;
+    readReg(MAG_ADDR, ST1, data, 8);
+    sign = ((short)data[2] << 8) | (short)data[1];
+    mag[1] = (double)sign * mag_coef - mag_offset[1];
+    sign = ((short)data[4] << 8) | (short)data[3];
+    mag[0] = (double)sign * mag_coef - mag_offset[0];
+    sign = ((short)data[6] << 8) | (short)data[5];
+    mag[2] = (double)sign * -mag_coef - mag_offset[2];
+}
+void mpu9250::getMag(double *mx, double *my, double *mz){
+    double mag[3];
+    getMag(mag);
+    *mx = mag[0];
+    *my = mag[1];
+    *mz = mag[2];   
+}
+void mpu9250::getGyroAcc(double *imu){
+    char data[14];  
+    short sign;
+    readReg(_addr, ACCEL_XOUT_H, data, 14);
+    sign = ((short)data[0] << 8) | (short)data[1];
+    imu[3] = (double)sign * acc_coef - acc_offset[0];
+    sign = ((short)data[2] << 8) | (short)data[3];
+    imu[4] = (double)sign * acc_coef - acc_offset[1];
+    sign = ((short)data[4] << 8) | (short)data[5];
+    imu[5] = (double)sign * acc_coef - acc_offset[2]; 
+    
+    sign = ((short)data[8] << 8) | (short)data[9];
+    imu[0] = (double)sign * gyro_coef - gyro_offset[0];
+    sign = ((short)data[10] << 8) | (short)data[11];
+    imu[1] = (double)sign * gyro_coef - gyro_offset[1];
+    sign = ((short)data[12] << 8) | (short)data[13];
+    imu[2] = (double)sign * gyro_coef - gyro_offset[2];   
+}
+
+void mpu9250::setAccLPF(A_BAND_WIDTH band){
+    writeReg(_addr, ACCEL_CONFIG2, band);
+    wait_us(70);  
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mpu9250_i2c.h	Mon Jan 09 06:28:04 2017 +0000
@@ -0,0 +1,158 @@
+#ifndef _MPU9250_I2C_H_
+#define _MPU9250_I2C_H_
+
+#define SLAVE_ADDR_LOW (0b1101000 << 1)//AD0 == LOW
+#define SLAVE_ADDR_HIGH (0b1101001 << 1)//AD0 == HIGH
+#define MAG_ADDR (0b0001100 << 1)
+
+#define WRITE_FLAG 0b00000000
+#define READ_FLAG  0b00000001
+#define CONFIG 0x1A
+#define GYRO_CONFIG 0x1B
+#define ACCEL_CONFIG 0x1C
+#define ACCEL_CONFIG2 0x1D
+#define LP_ACCEL_ODR 0x1E
+#define INT_PIN_CFG 0x37
+#define ACCEL_XOUT_H 0x3B
+#define ACCEL_XOUT_L 0x3C
+#define ACCEL_YOUT_H 0x3D
+#define ACCEL_YOUT_L 0x3E
+#define ACCLE_ZOUT_H 0x3F
+#define ACCEL_ZOUT_L 0x40
+#define TEMP_OUT_H 0x41
+#define TEMP_OUT_L 0x42
+#define GYRO_XOUT_H 0x43
+#define GYRO_XOUT_L 0x44
+#define GYRO_YOUT_H 0x45
+#define GYRO_YOUT_L 0x46
+#define GYRO_ZOUT_H 0x47
+#define GYRO_ZOUT_L 0x48
+#define WHO_AM_I 0x75 //0x71ならおｋ
+#define XG_OFFSET_H 0x13
+#define XG_OFFSET_L 0x14
+#define YG_OFFSET_H 0x15
+#define YG_OFFSET_L 0x16
+#define ZG_OFFSET_H 0x17
+#define ZG_OFFSET_L 0x18
+#define XA_OFFSET_H 0x77
+#define XA_OFFSET_L 0x78
+#define YA_OFFSET_H 0x79
+#define YA_OFFERT_L 0x80
+#define ZA_OFFSET_H 0x81
+#define ZA_OFFSET_L 0x82
+
+#define WIA 0x00 //device ID
+#define INFO 0x01
+#define ST1 0x02
+#define HXL 0x03//Low -> Highの順に注意
+#define HXH 0x04
+#define HYL 0x05
+#define HYH 0x06
+#define HZL 0x07
+#define HZH 0x08
+#define ST2 0x09
+#define CNTL1 0x0A
+#define CNTL2 0x0B
+
+#define ACC_LSB (0.0000610350)//[G / LSB]
+#define GYRO_LSB (0.007630) //[(degree / s) / LSB]
+#define MAG_LSB (0.150) //[uT / LSB]
+
+typedef enum AD0{
+    AD0_HIGH = 1,
+    AD0_LOW  = 0  
+}ad0;
+
+typedef enum ACC_RANGE{
+    _2G = 1,
+    _4G = 2,
+    _8G = 4,
+    _16G = 8
+}acc_range;
+
+typedef enum GYRO_RANGE{
+    _250DPS = 1,
+    _500DPS = 2,
+    _1000DPS = 4,
+    _2000DPS = 8
+}gyro_range;
+
+typedef enum MAG_RATE{
+    _8HZ = 0,
+    _100HZ = 1
+}mag_rate;
+
+typedef enum A_BAND_WIDTH{
+    NO_USE = 0b00000000,
+    _460HZ = 0b00001000,
+    _184HZ = 0b00001001,
+    _92HZ  = 0b00001010,
+    _41HZ  = 0b00001011,
+    _20HZ  = 0b00001100,
+    _10HZ  = 0b00001101,
+    _5HZ   = 0b00001110,
+}a_band_width;
+
+class mpu9250{
+
+public:
+    mpu9250(I2C &_i2c, AD0 celect);
+    I2C *_nine;
+public:
+    void writeReg(char addr, char data);
+    void writeReg(char addr, char reg, char data);
+    char readReg(char addr, char reg);
+    void readReg(char addr, char start_reg, char* buff, char num);
+    bool senserTest();
+    bool mag_senserTest();
+    void setAcc(ACC_RANGE a_range);
+    void setGyro(GYRO_RANGE g_range);
+    void setMag(MAG_RATE rate);
+    void init();
+    void frequency(int Hz);
+    void setAccLPF(A_BAND_WIDTH band);
+    void setOffset(double ax, double ay, double az,
+                   double gx, double gy, double gz,
+                   double mx, double my, double mz);
+    
+    void getAcc(double *ax, double *ay, double *az);
+    void getAcc(double *acc);
+    
+    void getGyro(double *gx, double *gy, double *gz);
+    void getGyro(double *gyro);
+    
+    void getMag(double *mx, double *my, double *mz);
+    void getMag(double *mag);  
+
+    void getGyroAcc(double *imu);//gx,gy,gz,ax,ay,az
+
+    static char _addr;
+    static double acc_coef;//coefficient
+    static double gyro_coef;
+    static double mag_coef;
+    static double acc_offset[3];
+    static double gyro_offset[3];
+    static double mag_offset[3];
+};
+
+
+
+inline void mpu9250::writeReg(char addr, char data){
+    _nine->write( addr | WRITE_FLAG, &data, 1, false);
+}
+inline void mpu9250::writeReg(char addr, char reg, char data){
+    char temp[2] = { reg, data};
+    _nine->write(addr | WRITE_FLAG, temp, 2, false);
+}
+inline char mpu9250::readReg(char addr, char reg){
+    char buff[1];
+    writeReg(addr, reg);
+    _nine->read(addr | READ_FLAG, buff, 1, true);
+    return buff[0];   
+}
+inline void mpu9250::readReg(char addr, char start_reg, char* buff, char num){
+    writeReg(addr, start_reg);
+    _nine->read(addr | READ_FLAG, buff, num, true);   
+}
+
+#endif
\ No newline at end of file