ryan lin / chair_MPU9250

library for chair for mpu

Dependencies:   MPU9250

chair_MPU9250.cpp@0:fd1a49d15f7f, 2018-07-20 (annotated)

Committer:
ryanlin97
Date:
Fri Jul 20 17:54:29 2018 +0000
Revision:
0:fd1a49d15f7f
Child:
1:ff1d286b3cf4
chair library for mpu9250 imu

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ryanlin97 0:fd1a49d15f7f 1 #include "chair_MPU9250.h"
ryanlin97 0:fd1a49d15f7f 2
ryanlin97 0:fd1a49d15f7f 3 Timer t;
ryanlin97 0:fd1a49d15f7f 4
ryanlin97 0:fd1a49d15f7f 5 chair_MPU9250::chair_MPU9250(Serial* out)
ryanlin97 0:fd1a49d15f7f 6 {
ryanlin97 0:fd1a49d15f7f 7 imu = new MPU9250(SDA, SCL);
ryanlin97 0:fd1a49d15f7f 8 usb = out;
ryanlin97 0:fd1a49d15f7f 9 }
ryanlin97 0:fd1a49d15f7f 10
ryanlin97 0:fd1a49d15f7f 11 chair_MPU9250::chair_MPU9250(PinName sda_pin, PinName scl_pin, Serial* out)
ryanlin97 0:fd1a49d15f7f 12 {
ryanlin97 0:fd1a49d15f7f 13 imu = new MPU9250(sda_pin, scl_pin);
ryanlin97 0:fd1a49d15f7f 14 usb = out;
ryanlin97 0:fd1a49d15f7f 15 }
ryanlin97 0:fd1a49d15f7f 16
ryanlin97 0:fd1a49d15f7f 17 void chair_MPU9250::setup()
ryanlin97 0:fd1a49d15f7f 18 {
ryanlin97 0:fd1a49d15f7f 19 // Read the WHO_AM_I register, this is a good test of communication
ryanlin97 0:fd1a49d15f7f 20 uint8_t whoami = imu->readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250
ryanlin97 0:fd1a49d15f7f 21 usb->printf("I AM 0x%x\n\r", whoami);
ryanlin97 0:fd1a49d15f7f 22 usb->printf("I SHOULD BE 0x71\n\r");
ryanlin97 0:fd1a49d15f7f 23
ryanlin97 0:fd1a49d15f7f 24 if (whoami == 0x71) { // WHO_AM_I should always be 0x68
ryanlin97 0:fd1a49d15f7f 25 usb->printf("MPU9250 is online...\n\r");
ryanlin97 0:fd1a49d15f7f 26
ryanlin97 0:fd1a49d15f7f 27 wait(1);
ryanlin97 0:fd1a49d15f7f 28
ryanlin97 0:fd1a49d15f7f 29 imu->resetMPU9250(); // Reset registers to default in preparation for device calibration
ryanlin97 0:fd1a49d15f7f 30 imu->calibrateMPU9250(imu->gyroBias, imu->accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
ryanlin97 0:fd1a49d15f7f 31 imu->initMPU9250();
ryanlin97 0:fd1a49d15f7f 32 imu->initAK8963(imu->magCalibration);
ryanlin97 0:fd1a49d15f7f 33 wait(2);
ryanlin97 0:fd1a49d15f7f 34 } else {
ryanlin97 0:fd1a49d15f7f 35 usb->printf("Could not connect to MPU9250: \n\r");
ryanlin97 0:fd1a49d15f7f 36 usb->printf("%#x \n", whoami);
ryanlin97 0:fd1a49d15f7f 37
ryanlin97 0:fd1a49d15f7f 38 while(1) ; // Loop forever if communication doesn't happen
ryanlin97 0:fd1a49d15f7f 39 }
ryanlin97 0:fd1a49d15f7f 40
ryanlin97 0:fd1a49d15f7f 41
ryanlin97 0:fd1a49d15f7f 42 imu->getAres(); // Get accelerometer sensitivity
ryanlin97 0:fd1a49d15f7f 43 imu->getGres(); // Get gyro sensitivity
ryanlin97 0:fd1a49d15f7f 44 imu->getMres(); // Get magnetometer sensitivity
ryanlin97 0:fd1a49d15f7f 45 usb->printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/imu->aRes);
ryanlin97 0:fd1a49d15f7f 46 usb->printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/imu->gRes);
ryanlin97 0:fd1a49d15f7f 47 usb->printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/imu->mRes);
ryanlin97 0:fd1a49d15f7f 48 imu->magbias[0] = +470.; // User environmental x-axis correction in milliGauss, should be automatically calculated
ryanlin97 0:fd1a49d15f7f 49 imu->magbias[1] = +120.; // User environmental x-axis correction in milliGauss
ryanlin97 0:fd1a49d15f7f 50 imu->magbias[2] = +125.; // User environmental x-axis correction in milliGauss
ryanlin97 0:fd1a49d15f7f 51 t.start();
ryanlin97 0:fd1a49d15f7f 52
ryanlin97 0:fd1a49d15f7f 53 }
ryanlin97 0:fd1a49d15f7f 54
ryanlin97 0:fd1a49d15f7f 55 double chair_MPU9250::yaw()
ryanlin97 0:fd1a49d15f7f 56 {
ryanlin97 0:fd1a49d15f7f 57
ryanlin97 0:fd1a49d15f7f 58 // If intPin goes high, all data registers have new data
ryanlin97 0:fd1a49d15f7f 59 if(imu->readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt
ryanlin97 0:fd1a49d15f7f 60
ryanlin97 0:fd1a49d15f7f 61 imu->readAccelData(imu->accelCount); // Read the x/y/z adc values
ryanlin97 0:fd1a49d15f7f 62 // Now we'll calculate the accleration value into actual g's
ryanlin97 0:fd1a49d15f7f 63 imu->ax = (float)imu->accelCount[0]*imu->aRes - imu->accelBias[0]; // get actual g value, this depends on scale being set
ryanlin97 0:fd1a49d15f7f 64 imu->ay = (float)imu->accelCount[1]*imu->aRes - imu->accelBias[1];
ryanlin97 0:fd1a49d15f7f 65 imu->az = (float)imu->accelCount[2]*imu->aRes - imu->accelBias[2];
ryanlin97 0:fd1a49d15f7f 66
ryanlin97 0:fd1a49d15f7f 67 imu->readGyroData(imu->gyroCount); // Read the x/y/z adc values
ryanlin97 0:fd1a49d15f7f 68 // Calculate the gyro value into actual degrees per second
ryanlin97 0:fd1a49d15f7f 69 imu->gx = (float)imu->gyroCount[0]*imu->gRes - imu->gyroBias[0]; // get actual gyro value, this depends on scale being set
ryanlin97 0:fd1a49d15f7f 70 imu->gy = (float)imu->gyroCount[1]*imu->gRes - imu->gyroBias[1];
ryanlin97 0:fd1a49d15f7f 71 imu->gz = (float)imu->gyroCount[2]*imu->gRes - imu->gyroBias[2];
ryanlin97 0:fd1a49d15f7f 72
ryanlin97 0:fd1a49d15f7f 73 imu->readMagData(imu->magCount); // Read the x/y/z adc values
ryanlin97 0:fd1a49d15f7f 74 // Calculate the magnetometer values in milliGauss
ryanlin97 0:fd1a49d15f7f 75 // Include factory calibration per data sheet and user environmental corrections
ryanlin97 0:fd1a49d15f7f 76 imu->mx = (float)imu->magCount[0]*imu->mRes*imu->magCalibration[0] - imu->magbias[0]; // get actual magnetometer value, this depends on scale being set
ryanlin97 0:fd1a49d15f7f 77 imu->my = (float)imu->magCount[1]*imu->mRes*imu->magCalibration[1] - imu->magbias[1];
ryanlin97 0:fd1a49d15f7f 78 imu->mz = (float)imu->magCount[2]*imu->mRes*imu->magCalibration[2] - imu->magbias[2];
ryanlin97 0:fd1a49d15f7f 79 }
ryanlin97 0:fd1a49d15f7f 80
ryanlin97 0:fd1a49d15f7f 81 float yaw = 0;
ryanlin97 0:fd1a49d15f7f 82 if(imu->gz>.3 || imu->gz < -.3) {
ryanlin97 0:fd1a49d15f7f 83 yaw = (yaw - t.read()*imu->gz);
ryanlin97 0:fd1a49d15f7f 84 t.reset();
ryanlin97 0:fd1a49d15f7f 85 if(yaw > 360)
ryanlin97 0:fd1a49d15f7f 86 yaw -= 360;
ryanlin97 0:fd1a49d15f7f 87 if(yaw < 0)
ryanlin97 0:fd1a49d15f7f 88 yaw += 360;
ryanlin97 0:fd1a49d15f7f 89 }
ryanlin97 0:fd1a49d15f7f 90
ryanlin97 0:fd1a49d15f7f 91 return (double)yaw;
ryanlin97 0:fd1a49d15f7f 92 }