Diff: chair_MPU9250.cpp

Revision:

0:fd1a49d15f7f

Child:

1:ff1d286b3cf4

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/chair_MPU9250.cpp	Fri Jul 20 17:54:29 2018 +0000
@@ -0,0 +1,92 @@
+#include "chair_MPU9250.h"
+
+Timer t;
+
+chair_MPU9250::chair_MPU9250(Serial* out)
+{
+    imu = new MPU9250(SDA, SCL);
+    usb = out;
+}
+
+chair_MPU9250::chair_MPU9250(PinName sda_pin, PinName scl_pin, Serial* out)
+{
+    imu = new MPU9250(sda_pin, scl_pin);
+    usb = out;
+}
+
+void chair_MPU9250::setup()
+{
+    // 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);
+    usb->printf("I SHOULD BE 0x71\n\r");
+
+    if (whoami == 0x71) { // WHO_AM_I should always be 0x68
+        usb->printf("MPU9250 is online...\n\r");
+
+        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);
+    } else {
+        usb->printf("Could not connect to MPU9250: \n\r");
+        usb->printf("%#x \n",  whoami);
+
+        while(1) ; // Loop forever if communication doesn't happen
+    }
+    
+        
+    imu->getAres(); // Get accelerometer sensitivity
+    imu->getGres(); // Get gyro sensitivity
+    imu->getMres(); // Get magnetometer sensitivity
+    usb->printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/imu->aRes);
+    usb->printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/imu->gRes);
+    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();        
+ 
+}
+
+double chair_MPU9250::yaw()
+{
+
+// 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
+        imu->ay = (float)imu->accelCount[1]*imu->aRes - imu->accelBias[1];
+        imu->az = (float)imu->accelCount[2]*imu->aRes - imu->accelBias[2];
+
+        imu->readGyroData(imu->gyroCount);  // Read the x/y/z adc values
+        // Calculate the gyro value into actual degrees per second
+        imu->gx = (float)imu->gyroCount[0]*imu->gRes - imu->gyroBias[0];  // get actual gyro value, this depends on scale being set
+        imu->gy = (float)imu->gyroCount[1]*imu->gRes - imu->gyroBias[1];
+        imu->gz = (float)imu->gyroCount[2]*imu->gRes - imu->gyroBias[2];
+
+        imu->readMagData(imu->magCount);  // Read the x/y/z adc values
+        // Calculate the magnetometer values in milliGauss
+        // Include factory calibration per data sheet and user environmental corrections
+        imu->mx = (float)imu->magCount[0]*imu->mRes*imu->magCalibration[0] - imu->magbias[0];  // get actual magnetometer value, this depends on scale being set
+        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;
+    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;
+    }
+
+    return (double)yaw;
+}