Diff: chair_MPU9250.cpp

Revision:

1:ff1d286b3cf4

Parent:

0:fd1a49d15f7f

Child:

2:66660dcf55fb

--- a/chair_MPU9250.cpp	Fri Jul 20 17:54:29 2018 +0000
+++ b/chair_MPU9250.cpp	Sun Jul 22 06:14:48 2018 +0000
@@ -1,21 +1,29 @@
 #include "chair_MPU9250.h"
+float total_yaw = 0;
 
-Timer t;
-
-chair_MPU9250::chair_MPU9250(Serial* out)
+chair_MPU9250::chair_MPU9250(Serial* out, Timer* time)
 {
     imu = new MPU9250(SDA, SCL);
     usb = out;
+    t = time;
+    start = false;
 }
 
-chair_MPU9250::chair_MPU9250(PinName sda_pin, PinName scl_pin, Serial* out)
+chair_MPU9250::chair_MPU9250(PinName sda_pin, PinName scl_pin, Serial* out, Timer* time)
 {
     imu = new MPU9250(sda_pin, scl_pin);
     usb = out;
+    t = time;
+    start = false;
 }
 
+void chair_MPU9250::setStart(){
+    start = true;
+    }
+    
 void chair_MPU9250::setup()
 {
+    t->start();
     // Read the WHO_AM_I register, this is a good test of communication
     uint8_t whoami = imu->readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);  // Read WHO_AM_I register for MPU-9250
     usb->printf("I AM 0x%x\n\r", whoami);
@@ -24,13 +32,13 @@
     if (whoami == 0x71) { // WHO_AM_I should always be 0x68
         usb->printf("MPU9250 is online...\n\r");
 
-        wait(1);
+        wait(.1);
 
         imu->resetMPU9250(); // Reset registers to default in preparation for device calibration
         imu->calibrateMPU9250(imu->gyroBias, imu->accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
         imu->initMPU9250();
         imu->initAK8963(imu->magCalibration);
-        wait(2);
+        wait(.1);
     } else {
         usb->printf("Could not connect to MPU9250: \n\r");
         usb->printf("%#x \n",  whoami);
@@ -47,17 +55,17 @@
     usb->printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/imu->mRes);
     imu->magbias[0] = +470.;  // User environmental x-axis correction in milliGauss, should be automatically calculated
     imu->magbias[1] = +120.;  // User environmental x-axis correction in milliGauss
-    imu->magbias[2] = +125.;  // User environmental x-axis correction in milliGauss
-    t.start();        
+    imu->magbias[2] = +125.;  // User environmental x-axis correction in milliGauss  
+    usb->printf("setup complete\n");    
  
 }
 
 double chair_MPU9250::yaw()
-{
-
-// If intPin goes high, all data registers have new data
+{   
+    // If intPin goes high, all data registers have new data
     if(imu->readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) {  // On interrupt, check if data ready interrupt
 
+        
         imu->readAccelData(imu->accelCount);  // Read the x/y/z adc values
         // Now we'll calculate the accleration value into actual g's
         imu->ax = (float)imu->accelCount[0]*imu->aRes - imu->accelBias[0];  // get actual g value, this depends on scale being set
@@ -77,16 +85,27 @@
         imu->my = (float)imu->magCount[1]*imu->mRes*imu->magCalibration[1] - imu->magbias[1];
         imu->mz = (float)imu->magCount[2]*imu->mRes*imu->magCalibration[2] - imu->magbias[2];
     }
-
-    float yaw = 0;
+    
+    float readTime;
+    if(!start) {
+        readTime = 0; 
+        chair_MPU9250::setStart();
+        }
+    else {
+        readTime = t->read();
+        }
+        
+    
     if(imu->gz>.3 || imu->gz < -.3) {
-        yaw = (yaw - t.read()*imu->gz);
-        t.reset();
-        if(yaw > 360)
-            yaw -= 360;
-        if(yaw < 0)
-            yaw += 360;
+        total_yaw = (total_yaw - readTime*imu->gz);
+        t->reset();
+        if(total_yaw > 360){
+            total_yaw -= 360;
+            }
+        
+        if(total_yaw < 0){
+            total_yaw += 360;
+            }
     }
-
-    return (double)yaw;
+    return total_yaw;
 }