Diff: MPU9250.cpp

Revision:

1:e16407b5e24f

Parent:

0:b502ea2d6ebb

Child:

2:a36510ff4272

--- a/MPU9250.cpp	Wed Jan 20 14:50:12 2016 +0000
+++ b/MPU9250.cpp	Fri Dec 20 11:49:05 2019 +0000
@@ -1,7 +1,7 @@
 #include "MPU9250.h"
 
 
-MPU9250::MPU9250(PinName sda, PinName scl, PinName tx, PinName rx, int address) : i2c(sda, scl), pc(tx,rx)
+MPU9250::MPU9250(I2C * _i2c, Serial * _pc, int address)
 {
     if(address == 0)
         MPU9250_ADDRESS = MPU9250_ADDRESS_68;
@@ -10,8 +10,10 @@
         printf("Wrong Address\n");
         while(1);
     }
-
-    i2c.frequency(400000);
+    i2c=_i2c;
+    pc=_pc;
+    pc->printf("MPU hello\n");
+    i2c->frequency(400000);
 
     for(int i=0; i<=3; i++) {
         magCalibration[i] = 0;
@@ -25,51 +27,53 @@
 void MPU9250::Start()
 {
     whoami = readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);  // Read WHO_AM_I register for MPU-9250
-    pc.printf("I AM 0x%x\n\r", whoami);
-    pc.printf("I SHOULD BE 0x71\n\r");
+    pc->printf("I AM 0x%x\n\r", whoami);
+    pc->printf("I SHOULD BE 0x71\n\r");
 
     if (whoami == 0x71) { // WHO_AM_I should always be 0x68
-        pc.printf("MPU9250 WHO_AM_I is 0x%x\n\r", whoami);
-        pc.printf("MPU9250 is online...\n\r");
-        wait(1);
+        pc->printf("MPU9250 WHO_AM_I is 0x%x\n\r", whoami);
+        pc->printf("MPU9250 is online...\n\r");
+        wait(0.1);
 
         resetMPU9250(); // Reset registers to default in preparation for device calibration
         MPU9250SelfTest(); // Start by performing self test and reporting values
-        /*pc.printf("x-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[0]);
-        pc.printf("y-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[1]);
-        pc.printf("z-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[2]);
-        pc.printf("x-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[3]);
-        pc.printf("y-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[4]);
-        pc.printf("z-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[5]);*/
-        calibrateMPU9250(); // Calibrate gyro and accelerometers, load biases in bias registers
-        /*pc.printf("x gyro bias = %f\n\r", gyroBias[0]);
-        pc.printf("y gyro bias = %f\n\r", gyroBias[1]);
-        pc.printf("z gyro bias = %f\n\r", gyroBias[2]);
-        pc.printf("x accel bias = %f\n\r", accelBias[0]);
-        pc.printf("y accel bias = %f\n\r", accelBias[1]);
-        pc.printf("z accel bias = %f\n\r", accelBias[2]);*/
-        wait(2);
+        /*pc->printf("x-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[0]);
+        pc->printf("y-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[1]);
+        pc->printf("z-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[2]);
+        pc->printf("x-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[3]);
+        pc->printf("y-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[4]);
+        pc->printf("z-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[5]);*/
+        
+        //calibrateMPU9250(); // Calibrate gyro and accelerometers, load biases in bias registers
+        
+        /*pc->printf("x gyro bias = %f\n\r", gyroBias[0]);
+        pc->printf("y gyro bias = %f\n\r", gyroBias[1]);
+        pc->printf("z gyro bias = %f\n\r", gyroBias[2]);
+        pc->printf("x accel bias = %f\n\r", accelBias[0]);
+        pc->printf("y accel bias = %f\n\r", accelBias[1]);
+        pc->printf("z accel bias = %f\n\r", accelBias[2]);*/
+        wait(0.2);
         initMPU9250();
-        pc.printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
+        pc->printf("MPU9250 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
         initAK8963();
-        pc.printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
+        pc->printf("AK8963 initialized for active data mode....\n\r"); // Initialize device for active mode read of magnetometer
 
         whoami = readByte(AK8963_ADDRESS, WHO_AM_I_AK8963);  // Read WHO_AM_I register for MPU-9250
-        pc.printf("I AM 0x%x\n\r", whoami);
-        pc.printf("I SHOULD BE 0x48\n\r");
+        pc->printf("I AM 0x%x\n\r", whoami);
+        pc->printf("I SHOULD BE 0x48\n\r");
         if(whoami != 0x48) {
             while(1);
         }
-        /*pc.printf("Accelerometer full-scale range = %f  g\n\r", 2.0f*(float)(1<<Ascale));
-        pc.printf("Gyroscope full-scale range = %f  deg/s\n\r", 250.0f*(float)(1<<Gscale));
-        if(Mscale == 0) pc.printf("Magnetometer resolution = 14  bits\n\r");
-        if(Mscale == 1) pc.printf("Magnetometer resolution = 16  bits\n\r");
-        if(Mmode == 2) pc.printf("Magnetometer ODR = 8 Hz\n\r");
-        if(Mmode == 6) pc.printf("Magnetometer ODR = 100 Hz\n\r");*/
-        wait(1);
+        /*pc->printf("Accelerometer full-scale range = %f  g\n\r", 2.0f*(float)(1<<Ascale));
+        pc->printf("Gyroscope full-scale range = %f  deg/s\n\r", 250.0f*(float)(1<<Gscale));
+        if(Mscale == 0) pc->printf("Magnetometer resolution = 14  bits\n\r");
+        if(Mscale == 1) pc->printf("Magnetometer resolution = 16  bits\n\r");
+        if(Mmode == 2) pc->printf("Magnetometer ODR = 8 Hz\n\r");
+        if(Mmode == 6) pc->printf("Magnetometer ODR = 100 Hz\n\r");*/
+        wait(0.1);
     } else {
-        pc.printf("Could not connect to MPU9250: \n\r");
-        pc.printf("%#x \n",  whoami);
+        pc->printf("Could not connect to MPU9250: \n\r");
+        pc->printf("%#x \n",  whoami);
 
         while(1) ; // Loop forever if communication doesn't happen
     }
@@ -78,9 +82,9 @@
     getAres(); // Get accelerometer sensitivity
     getGres(); // Get gyro sensitivity
     getMres(); // Get magnetometer sensitivity
-    /*pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes);
-    pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes);
-    pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes);*/
+    /*pc->printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes);
+    pc->printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes);
+    pc->printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes);*/
 
     MagCal();
 }
@@ -117,7 +121,7 @@
     char data_write[2];
     data_write[0] = subAddress;
     data_write[1] = data;
-    i2c.write(address, data_write, 2, 0);
+    i2c->write(address, data_write, 2, 0);
 }
 
 char MPU9250::readByte(uint8_t address, uint8_t subAddress)
@@ -125,8 +129,8 @@
     char data[1]; // `data` will store the register data
     char data_write[1];
     data_write[0] = subAddress;
-    i2c.write(address, data_write, 1, 1); // no stop
-    i2c.read(address, data, 1, 0);
+    i2c->write(address, data_write, 1, 1); // no stop
+    i2c->read(address, data, 1, 0);
     return data[0];
 }
 
@@ -135,8 +139,8 @@
     char data[14];
     char data_write[1];
     data_write[0] = subAddress;
-    i2c.write(address, data_write, 1, 1); // no stop
-    i2c.read(address, data, count, 0);
+    i2c->write(address, data_write, 1, 1); // no stop
+    i2c->read(address, data, count, 0);
     for(int ii = 0; ii < count; ii++) {
         dest[ii] = data[ii];
     }
@@ -216,7 +220,7 @@
 
 void MPU9250::getAres()
 {
-    Ascale = AFS_2G;     // AFS_2G, AFS_4G, AFS_8G, AFS_16G
+    Ascale = AFS_16G;     // AFS_2G, AFS_4G, AFS_8G, AFS_16G
 }
 
 void MPU9250::MagCal()