Diff: mpu9250_i2c.cpp

Revision:

15:d8b263a8138e

Parent:

1:6a4c2f84180b

diff -r cf5787acc101 -r d8b263a8138e mpu9250_i2c.cpp
--- a/mpu9250_i2c.cpp	Sat Jan 28 21:24:27 2017 +0000
+++ b/mpu9250_i2c.cpp	Wed Dec 06 13:07:09 2017 +0000
@@ -1,19 +1,20 @@
 #include "mbed.h"
 #include "mpu9250_i2c.h"
 
-char mpu9250::_addr = SLAVE_ADDR_HIGH;
+/*
+char mpu9250::_addr = MPU9250_SLAVE_ADDR_HIGH;
 double mpu9250::acc_coef = ACC_LSB;
 double mpu9250::gyro_coef = GYRO_LSB;
-double mpu9250::mag_coef = MAG_LSB;
+double mpu9250::mag_coef = MPU9250_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};
+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;   
+    if(celect == AD0_HIGH) _addr = MPU9250_SLAVE_ADDR_HIGH;   
+    else _addr = MPU9250_SLAVE_ADDR_LOW;   
     
     _nine->frequency(400000);//400kHz
     init();
@@ -21,21 +22,21 @@
 void mpu9250::frequency(int Hz){
     _nine->frequency(Hz);   
 }
-bool mpu9250::senserTest(){
+bool mpu9250::sensorTest(){
     if(readReg(_addr, WHO_AM_I_MPU9250) == 0x71){
         return true;   
     }   
     else return false;
 }
 
-bool mpu9250::mag_senserTest(){
-     if(readReg(MAG_ADDR, WIA) == 0x48){
+bool mpu9250::mag_sensorTest(){
+     if(readReg(MPU9250_MAG_ADDR, MPU9250_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){
+void mpu9250::setOffset(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz){
     gyro_offset[0] = gx;
     gyro_offset[1] = gy;
     gyro_offset[2] = gz;
@@ -93,13 +94,13 @@
     
     acc_coef = ACC_LSB;
     gyro_coef = GYRO_LSB;
-    mag_coef = MAG_LSB;
+    mag_coef = MPU9250_MAG_LSB;
     
-    writeReg(MAG_ADDR, 0x6B, 0x00);
+    writeReg(MPU9250_MAG_ADDR, 0x6B, 0x00);
     wait_us(70);
     writeReg(_addr, 0x37, 0x02);
     wait_us(70);
-    writeReg(MAG_ADDR, CNTL1, 0x16); 
+    writeReg(MPU9250_MAG_ADDR, MPU9250_CNTL1, 0x16); 
     wait_us(70);
     setAcc(_4G);
     wait_us(70);
@@ -111,73 +112,138 @@
     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];
+    acc[0] = (float)sign * acc_coef - acc_offset[0];
     sign = ((short)data[2] << 8) | (short)data[3];
-    acc[1] = (double)sign * acc_coef - acc_offset[1];
+    acc[1] = (float)sign * acc_coef - acc_offset[1];
     sign = ((short)data[4] << 8) | (short)data[5];
-    acc[2] = (double)sign * acc_coef - acc_offset[2];
+    acc[2] = (float)sign * acc_coef - acc_offset[2];
 }
+
 template<typename T>void mpu9250::getAcc(T *ax, T *ay, T *az){
-    double acc[3];
+    float acc[3];
     getAcc(acc);
     *ax = acc[0];
     *ay = acc[1];
     *az = acc[2];   
 }
+
 template<typename T>void mpu9250::getGyro(T *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];
+    gyro[0] = (float)sign * gyro_coef - gyro_offset[0];
     sign = ((short)data[2] << 8) | (short)data[3];
-    gyro[1] = (double)sign * gyro_coef - gyro_offset[1];
+    gyro[1] = (float)sign * gyro_coef - gyro_offset[1];
     sign = ((short)data[4] << 8) | (short)data[5];
-    gyro[2] = (double)sign * gyro_coef - gyro_offset[2];
+    gyro[2] = (float)sign * gyro_coef - gyro_offset[2];
 }
+
 template<typename T>void mpu9250::getGyro(T *gx, T *gy, T *gz){
-    double gyro[3];
+    float gyro[3];
     getGyro(gyro);
     *gx = gyro[0];
     *gy = gyro[1];
     *gz = gyro[2];   
 }
+
 template<typename T>void mpu9250::getMag(T *mag){
     char data[8];   
     short sign;
-    readReg(MAG_ADDR, ST1, data, 8);
+    readReg(MPU9250_MAG_ADDR, MPU9250_ST1, data, 8);
     sign = ((short)data[2] << 8) | (short)data[1];
-    mag[1] = (double)sign * mag_coef - mag_offset[1];
+    mag[1] = (float)sign * mag_coef - mag_offset[1];
     sign = ((short)data[4] << 8) | (short)data[3];
-    mag[0] = (double)sign * mag_coef - mag_offset[0];
+    mag[0] = (float)sign * mag_coef - mag_offset[0];
     sign = ((short)data[6] << 8) | (short)data[5];
-    mag[2] = (double)sign * -mag_coef - mag_offset[2];
+    mag[2] = (float)sign * -mag_coef - mag_offset[2];
 }
+
 template<typename T>void mpu9250::getMag(T *mx, T *my, T *mz){
-    double mag[3];
+    float mag[3];
     getMag(mag);
     *mx = mag[0];
     *my = mag[1];
     *mz = mag[2];   
 }
+
 template<typename T>void mpu9250::getGyroAcc(T *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];
+    imu[3] = (float)sign * acc_coef - acc_offset[0];
     sign = ((short)data[2] << 8) | (short)data[3];
-    imu[4] = (double)sign * acc_coef - acc_offset[1];
+    imu[4] = (float)sign * acc_coef - acc_offset[1];
     sign = ((short)data[4] << 8) | (short)data[5];
-    imu[5] = (double)sign * acc_coef - acc_offset[2]; 
+    imu[5] = (float)sign * acc_coef - acc_offset[2]; 
+    
+    sign = ((short)data[8] << 8) | (short)data[9];
+    imu[0] = (float)sign * gyro_coef - gyro_offset[0];
+    sign = ((short)data[10] << 8) | (short)data[11];
+    imu[1] = (float)sign * gyro_coef - gyro_offset[1];
+    sign = ((short)data[12] << 8) | (short)data[13];
+    imu[2] = (float)sign * gyro_coef - gyro_offset[2];   
+}
+
+template<typename T>void mpu9250::getAll(T *imu, T *mag){
+    
+    char data[14];  
+    short sign;
+    readReg(_addr, ACCEL_XOUT_H, data, 14);
+    sign = ((short)data[0] << 8) | (short)data[1];
+    imu[3] = (float)sign * acc_coef - acc_offset[0];
+    sign = ((short)data[2] << 8) | (short)data[3];
+    imu[4] = (float)sign * acc_coef - acc_offset[1];
+    sign = ((short)data[4] << 8) | (short)data[5];
+    imu[5] = (float)sign * acc_coef - acc_offset[2]; 
     
     sign = ((short)data[8] << 8) | (short)data[9];
-    imu[0] = (double)sign * gyro_coef - gyro_offset[0];
+    imu[0] = (float)sign * gyro_coef - gyro_offset[0];
     sign = ((short)data[10] << 8) | (short)data[11];
-    imu[1] = (double)sign * gyro_coef - gyro_offset[1];
+    imu[1] = (float)sign * gyro_coef - gyro_offset[1];
     sign = ((short)data[12] << 8) | (short)data[13];
-    imu[2] = (double)sign * gyro_coef - gyro_offset[2];   
+    imu[2] = (float)sign * gyro_coef - gyro_offset[2];   
+    
+    readReg(MPU9250_MAG_ADDR, MPU9250_ST1, data, 8);
+    sign = ((short)data[2] << 8) | (short)data[1];
+    mag[1] = (float)sign * mag_coef - mag_offset[1];
+    sign = ((short)data[4] << 8) | (short)data[3];
+    mag[0] = (float)sign * mag_coef - mag_offset[0];
+    sign = ((short)data[6] << 8) | (short)data[5];
+    mag[2] = (float)sign * -mag_coef - mag_offset[2];
+    
 }
+     
+template<typename T>void mpu9250::getAll(T *all){
+    
+    char data[14];  
+    short sign;
+    readReg(_addr, ACCEL_XOUT_H, data, 14);
+    sign = ((short)data[0] << 8) | (short)data[1];
+    all[3] = (float)sign * acc_coef - acc_offset[0];
+    sign = ((short)data[2] << 8) | (short)data[3];
+    all[4] = (float)sign * acc_coef - acc_offset[1];
+    sign = ((short)data[4] << 8) | (short)data[5];
+    all[5] = (float)sign * acc_coef - acc_offset[2]; 
+    
+    sign = ((short)data[8] << 8) | (short)data[9];
+    all[0] = (float)sign * gyro_coef - gyro_offset[0];
+    sign = ((short)data[10] << 8) | (short)data[11];
+    all[1] = (float)sign * gyro_coef - gyro_offset[1];
+    sign = ((short)data[12] << 8) | (short)data[13];
+    all[2] = (float)sign * gyro_coef - gyro_offset[2];   
+    
+    readReg(MPU9250_MAG_ADDR, MPU9250_ST1, data, 8);
+    sign = ((short)data[2] << 8) | (short)data[1];
+    all[7] = (float)sign * mag_coef - mag_offset[1];
+    sign = ((short)data[4] << 8) | (short)data[3];
+    all[6] = (float)sign * mag_coef - mag_offset[0];
+    sign = ((short)data[6] << 8) | (short)data[5];
+    all[8] = (float)sign * -mag_coef - mag_offset[2];   
+    return;
+}
+
 
 void mpu9250::setAccLPF(A_BAND_WIDTH band){
     writeReg(_addr, ACCEL_CONFIG2, band);
@@ -201,4 +267,9 @@
 
 template void mpu9250::getGyroAcc<double>(double *imu);//gx,gy,gz,ax,ay,az
 template void mpu9250::getGyroAcc<float>(float *imu);//gx,gy,gz,ax,ay,az
-  
\ No newline at end of file
+
+template void mpu9250::getAll<double>(double *all);
+template void mpu9250::getAll<float>(float *all);
+
+template void mpu9250::getAll<float>(float *imu, float *mag);
+template void mpu9250::getAll<double>(double *imu, double *mag);
\ No newline at end of file