ryan lin / chair_MPU9250

library for chair for mpu

Dependencies:   MPU9250

chair_MPU9250.cpp@2:66660dcf55fb, 2018-07-22 (annotated)

Committer:
ryanlin97
Date:
Sun Jul 22 18:46:12 2018 +0000
Revision:
2:66660dcf55fb
Parent:
1:ff1d286b3cf4 
updated for mpu9250

Who changed what in which revision?

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