Revision 12:2fb5ac400fbd, committed 2017-12-19

Comitter:

Muglug

Date:

Tue Dec 19 10:14:59 2017 +0000

Parent:

11:084e8ba240c1

Commit message:

Updated to Integer only.;

Changed in this revision

--- a/MPU9250.cpp	Tue Jul 01 13:59:45 2014 +0000
+++ b/MPU9250.cpp	Tue Dec 19 10:14:59 2017 +0000
@@ -14,7 +14,6 @@
     spi.write(WriteAddr);
     temp_val=spi.write(WriteData);
     deselect();
-    wait_us(50);
     return temp_val;
 }
 unsigned int  mpu9250_spi::ReadReg( uint8_t WriteAddr, uint8_t WriteData )
@@ -30,7 +29,6 @@
     for(i=0; i<Bytes; i++)
         ReadBuf[i] = spi.write(0x00);
     deselect();
-    wait_us(50);
 }
 
 /*-----------------------------------------------------------------------------------------------
@@ -42,14 +40,15 @@
 BITS_DLPF_CFG_98HZ
 BITS_DLPF_CFG_42HZ
 BITS_DLPF_CFG_20HZ
-BITS_DLPF_CFG_10HZ 
-BITS_DLPF_CFG_5HZ 
+BITS_DLPF_CFG_10HZ
+BITS_DLPF_CFG_5HZ
 BITS_DLPF_CFG_2100HZ_NOLPF
 returns 1 if an error occurred
 -----------------------------------------------------------------------------------------------*/
 #define MPU_InitRegNum 17
 
-bool mpu9250_spi::init(int sample_rate_div,int low_pass_filter){
+bool mpu9250_spi::init(int sample_rate_div,int low_pass_filter)
+{
     uint8_t i = 0;
     uint8_t MPU_Init_Data[MPU_InitRegNum][2] = {
         {0x80, MPUREG_PWR_MGMT_1},     // Reset Device
@@ -64,7 +63,7 @@
         //{0x20, MPUREG_USER_CTRL},      // Enable AUX
         {0x20, MPUREG_USER_CTRL},       // I2C Master mode
         {0x0D, MPUREG_I2C_MST_CTRL}, //  I2C configuration multi-master  IIC 400KHz
-        
+
         {AK8963_I2C_ADDR, MPUREG_I2C_SLV0_ADDR},  //Set the I2C slave addres of AK8963 and set for write.
         //{0x09, MPUREG_I2C_SLV4_CTRL},
         //{0x81, MPUREG_I2C_MST_DELAY_CTRL}, //Enable I2C delay
@@ -76,10 +75,10 @@
         {AK8963_CNTL1, MPUREG_I2C_SLV0_REG}, //I2C slave 0 register address from where to begin data transfer
         {0x12, MPUREG_I2C_SLV0_DO}, // Register value to continuous measurement in 16bit
         {0x81, MPUREG_I2C_SLV0_CTRL}  //Enable I2C and set 1 byte
-        
+
     };
     spi.format(8,0);
-    spi.frequency(1000000);
+    spi.frequency(25000000);
 
     for(i=0; i<MPU_InitRegNum; i++) {
         WriteReg(MPU_Init_Data[i][1], MPU_Init_Data[i][0]);
@@ -88,7 +87,7 @@
 
     set_acc_scale(2);
     set_gyro_scale(250);
-    
+
     //AK8963_calib_Magnetometer();  //Can't load this function here , strange problem?
     return 0;
 }
@@ -102,39 +101,40 @@
 BITS_FS_16G
 returns the range set (2,4,8 or 16)
 -----------------------------------------------------------------------------------------------*/
-unsigned int mpu9250_spi::set_acc_scale(int scale){
+unsigned int mpu9250_spi::set_acc_scale(int scale)
+{
     unsigned int temp_scale;
     WriteReg(MPUREG_ACCEL_CONFIG, scale);
-    
-    switch (scale){
+
+    switch (scale) {
         case BITS_FS_2G:
             acc_divider=16384;
-        break;
+            break;
         case BITS_FS_4G:
             acc_divider=8192;
-        break;
+            break;
         case BITS_FS_8G:
             acc_divider=4096;
-        break;
+            break;
         case BITS_FS_16G:
             acc_divider=2048;
-        break;   
+            break;
     }
     temp_scale=WriteReg(MPUREG_ACCEL_CONFIG|READ_FLAG, 0x00);
-    
-    switch (temp_scale){
+
+    switch (temp_scale) {
         case BITS_FS_2G:
             temp_scale=2;
-        break;
+            break;
         case BITS_FS_4G:
             temp_scale=4;
-        break;
+            break;
         case BITS_FS_8G:
             temp_scale=8;
-        break;
+            break;
         case BITS_FS_16G:
             temp_scale=16;
-        break;   
+            break;
     }
     return temp_scale;
 }
@@ -150,37 +150,38 @@
 BITS_FS_2000DPS
 returns the range set (250,500,1000 or 2000)
 -----------------------------------------------------------------------------------------------*/
-unsigned int mpu9250_spi::set_gyro_scale(int scale){
+unsigned int mpu9250_spi::set_gyro_scale(int scale)
+{
     unsigned int temp_scale;
     WriteReg(MPUREG_GYRO_CONFIG, scale);
-    switch (scale){
+    switch (scale) {
         case BITS_FS_250DPS:
             gyro_divider=131;
-        break;
+            break;
         case BITS_FS_500DPS:
             gyro_divider=65.5;
-        break;
+            break;
         case BITS_FS_1000DPS:
             gyro_divider=32.8;
-        break;
+            break;
         case BITS_FS_2000DPS:
             gyro_divider=16.4;
-        break;   
+            break;
     }
     temp_scale=WriteReg(MPUREG_GYRO_CONFIG|READ_FLAG, 0x00);
-    switch (temp_scale){
+    switch (temp_scale) {
         case BITS_FS_250DPS:
             temp_scale=250;
-        break;
+            break;
         case BITS_FS_500DPS:
             temp_scale=500;
-        break;
+            break;
         case BITS_FS_1000DPS:
             temp_scale=1000;
-        break;
+            break;
         case BITS_FS_2000DPS:
             temp_scale=2000;
-        break;   
+            break;
     }
     return temp_scale;
 }
@@ -192,7 +193,8 @@
 mpu9250 which should be 104 when in SPI mode.
 returns the I2C address (104)
 -----------------------------------------------------------------------------------------------*/
-unsigned int mpu9250_spi::whoami(){
+unsigned int mpu9250_spi::whoami()
+{
     unsigned int response;
     response=WriteReg(MPUREG_WHOAMI|READ_FLAG, 0x00);
     return response;
@@ -209,16 +211,11 @@
 void mpu9250_spi::read_acc()
 {
     uint8_t response[6];
-    int16_t bit_data;
-    float data;
-    int i;
     ReadRegs(MPUREG_ACCEL_XOUT_H,response,6);
-    for(i=0; i<3; i++) {
-        bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
-        data=(float)bit_data;
-        accelerometer_data[i]=data/acc_divider;
-    }
-    
+
+    accelerometer_data[0] = ((int16_t)response[0] << 8) | response[1]; // Turn the MSB and LSB into a signed 16-bit value
+    accelerometer_data[1] = ((int16_t)response[2] << 8) | response[3];
+    accelerometer_data[2] = ((int16_t)response[4] << 8) | response[5];
 }
 
 /*-----------------------------------------------------------------------------------------------
@@ -231,30 +228,26 @@
 void mpu9250_spi::read_rot()
 {
     uint8_t response[6];
-    int16_t bit_data;
-    float data;
-    int i;
     ReadRegs(MPUREG_GYRO_XOUT_H,response,6);
-    for(i=0; i<3; i++) {
-        bit_data=((int16_t)response[i*2]<<8)|response[i*2+1];
-        data=(float)bit_data;
-        gyroscope_data[i]=data/gyro_divider;
-    }
 
+    gyroscope_data[0] = ((int16_t)response[0] << 8) | response[1]; // Turn the MSB and LSB into a signed 16-bit value
+    gyroscope_data[1] = ((int16_t)response[2] << 8) | response[3];
+    gyroscope_data[2] = ((int16_t)response[4] << 8) | response[5];
 }
 
 /*-----------------------------------------------------------------------------------------------
                                 READ TEMPERATURE
-usage: call this function to read temperature data. 
+usage: call this function to read temperature data.
 returns the value in °C
 -----------------------------------------------------------------------------------------------*/
-void mpu9250_spi::read_temp(){
+void mpu9250_spi::read_temp()
+{
     uint8_t response[2];
     int16_t bit_data;
     float data;
     ReadRegs(MPUREG_TEMP_OUT_H,response,2);
 
-    bit_data=((int16_t)response[0]<<8)|response[1];
+    bit_data = ((int16_t)response[0] << 8) | response[1];
     data=(float)bit_data;
     Temperature=(data/340)+36.53;
     deselect();
@@ -285,7 +278,8 @@
 
     set_acc_scale(temp_scale);
 }
-uint8_t mpu9250_spi::AK8963_whoami(){
+uint8_t mpu9250_spi::AK8963_whoami()
+{
     uint8_t response;
     WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read.
     WriteReg(MPUREG_I2C_SLV0_REG, AK8963_WIA); //I2C slave 0 register address from where to begin data transfer
@@ -293,13 +287,14 @@
 
     //WriteReg(MPUREG_I2C_SLV0_CTRL, 0x81);    //Enable I2C and set bytes
     wait(0.001);
-    response=WriteReg(MPUREG_EXT_SENS_DATA_00|READ_FLAG, 0x00);    //Read I2C 
+    response=WriteReg(MPUREG_EXT_SENS_DATA_00|READ_FLAG, 0x00);    //Read I2C
     //ReadRegs(MPUREG_EXT_SENS_DATA_00,response,1);
-    //response=WriteReg(MPUREG_I2C_SLV0_DO, 0x00);    //Read I2C 
+    //response=WriteReg(MPUREG_I2C_SLV0_DO, 0x00);    //Read I2C
 
     return response;
 }
-void mpu9250_spi::AK8963_calib_Magnetometer(){
+void mpu9250_spi::AK8963_calib_Magnetometer()
+{
     uint8_t response[3];
     float data;
     int i;
@@ -310,16 +305,17 @@
 
     //WriteReg(MPUREG_I2C_SLV0_CTRL, 0x81);    //Enable I2C and set bytes
     wait(0.001);
-    //response[0]=WriteReg(MPUREG_EXT_SENS_DATA_01|READ_FLAG, 0x00);    //Read I2C 
+    //response[0]=WriteReg(MPUREG_EXT_SENS_DATA_01|READ_FLAG, 0x00);    //Read I2C
     ReadRegs(MPUREG_EXT_SENS_DATA_00,response,3);
-    
-    //response=WriteReg(MPUREG_I2C_SLV0_DO, 0x00);    //Read I2C 
+
+    //response=WriteReg(MPUREG_I2C_SLV0_DO, 0x00);    //Read I2C
     for(i=0; i<3; i++) {
         data=response[i];
         Magnetometer_ASA[i]=((data-128)/256+1)*Magnetometer_Sensitivity_Scale_Factor;
     }
 }
-void mpu9250_spi::AK8963_read_Magnetometer(){
+void mpu9250_spi::AK8963_read_Magnetometer()
+{
     uint8_t response[7];
     int16_t bit_data;
     float data;
@@ -338,7 +334,8 @@
         Magnetometer[i]=data*Magnetometer_ASA[i];
     }
 }
-void mpu9250_spi::read_all(){
+void mpu9250_spi::read_all()
+{
     uint8_t response[21];
     int16_t bit_data;
     float data;
@@ -380,11 +377,13 @@
                                 SPI SELECT AND DESELECT
 usage: enable and disable mpu9250 communication bus
 -----------------------------------------------------------------------------------------------*/
-void mpu9250_spi::select() {
+void mpu9250_spi::select()
+{
     //Set CS low to start transmission (interrupts conversion)
     cs = 0;
 }
-void mpu9250_spi::deselect() {
+void mpu9250_spi::deselect()
+{
     //Set CS high to stop transmission (restarts conversion)
     cs = 1;
 }
\ No newline at end of file

--- a/MPU9250.h	Tue Jul 01 13:59:45 2014 +0000
+++ b/MPU9250.h	Tue Dec 19 10:14:59 2017 +0000
@@ -34,17 +34,15 @@
     void AK8963_read_Magnetometer();
     void read_all();
 
-
-    
-    float acc_divider;
-    float gyro_divider;
+    int16_t acc_divider;
+    int16_t gyro_divider;
     
     int calib_data[3];
     float Magnetometer_ASA[3];
 
-    float accelerometer_data[3];
+    int16_t accelerometer_data[3];
     float Temperature;
-    float gyroscope_data[3];
+    int16_t gyroscope_data[3];
     float Magnetometer[3];
     
   private: